(f, mx_k, P_k, h, y_kp1, Q, R, args)
return (mx_kp1, P_kp1)
EKF   Extended Kalman Filter for nonlinear dynamic systems
     ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P 
     for nonlinear dynamic system:
               x_k+1 = f(x_k) + w_k
               y_k   = h(x_k) + v_k
     where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
           v ~ N(0,R) meaning v is gaussian noise with covariance R
    Inputs:    f: function handle for f(x)
               mx_k: "a priori" state estimate
               P_k: "a priori" estimated state covariance
               h: fanction handle for h(x)
               y_kp1: current measurement
               Q: process noise covariance 
               R: measurement noise covariance
               args: additional arguments to f(x, *args)
    Output:    mx_kp1: "a posteriori" state estimate
               P_kp1: "a posteriori" state covariance
               
    Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of"
EKF   Extended Kalman Filter for nonlinear dynamic systems
     ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P 
     for nonlinear dynamic system:
               x_k+1 = f(x_k) + w_k
               y_k   = h(x_k) + v_k
     where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
           v ~ N(0,R) meaning v is gaussian noise with covariance R
    Inputs:    f: function handle for f(x)
               mx_k: "a priori" state estimate
               P_k: "a priori" estimated state covariance
               h: fanction handle for h(x)
               y_kp1: current measurement
               Q: process noise covariance 
               R: measurement noise covariance
               args: additional arguments to f(x, *args)
    Output:    mx_kp1: "a posteriori" state estimate
               P_kp1: "a posteriori" state covariance
               
    Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of"
EKF Extended Kalman Filter for nonlinear dynamic systems ekf ( f mx P h z Q R ) returns state estimate x and state covariance P for nonlinear dynamic system : x_k + 1 = f ( x_k ) + w_k y_k = h ( x_k ) + v_k where w ~ N ( 0 Q ) meaning w is gaussian noise with covariance Q v ~ N ( 0 R ) meaning v is gaussian noise with covariance R Inputs : f : function handle for f ( x ) mx_k : a priori state estimate P_k : a priori estimated state covariance h : fanction handle for h ( x ) y_kp1 : current measurement Q : process noise covariance R : measurement noise covariance args : additional arguments to f ( x * args ) Output : mx_kp1 : a posteriori state estimate P_kp1 : a posteriori state covariance Notation : mx_k = E [ x_k ] and my_k = E [ y_k ] where m stands for mean of
def ekf(f, mx_k, P_k, h, y_kp1, Q, R, args):
    """
     EKF   Extended Kalman Filter for nonlinear dynamic systems
     ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P 
     for nonlinear dynamic system:
               x_k+1 = f(x_k) + w_k
               y_k   = h(x_k) + v_k
     where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
           v ~ N(0,R) meaning v is gaussian noise with covariance R
    Inputs:    f: function handle for f(x)
               mx_k: "a priori" state estimate
               P_k: "a priori" estimated state covariance
               h: fanction handle for h(x)
               y_kp1: current measurement
               Q: process noise covariance 
               R: measurement noise covariance
               args: additional arguments to f(x, *args)
    Output:    mx_kp1: "a posteriori" state estimate
               P_kp1: "a posteriori" state covariance
               
    Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of"
    """


    xDim    = mx_k.size                         # dimension of the state
    mx_kp1  = f(mx_k, *args)                    # predict next state
    A       = numerical_jac(f, mx_k, *args)     # linearize process model about current state
    P_kp1   = dot(dot(A,P_k),A.T) + Q           # proprogate variance
    my_kp1  = h(mx_kp1)                         # predict future output
    H       = numerical_jac(h, mx_kp1)          # linearize measurement model about predicted next state
    P12     = dot(P_kp1, H.T)                   # cross covariance
    K       = dot(P12, inv( dot(H,P12) + R))    # Kalman filter gain
    mx_kp1  = mx_kp1 + dot(K,(y_kp1 - my_kp1))  # state estimate
    P_kp1   = dot(dot(K,R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1)  ,  (eye(xDim) - dot(K,H)).T ) 

    return (mx_kp1, P_kp1)
def ekf ( f , mx_k , P_k , h , y_kp1 , Q , R , args ) : xDim = mx_k . size # dimension of the state mx_kp1 = f ( mx_k , * args ) # predict next state A = numerical_jac ( f , mx_k , * args ) # linearize process model about current state P_kp1 = dot ( dot ( A , P_k ) , A . T ) + Q # proprogate variance my_kp1 = h ( mx_kp1 ) # predict future output H = numerical_jac ( h , mx_kp1 ) # linearize measurement model about predicted next state P12 = dot ( P_kp1 , H . T ) # cross covariance K = dot ( P12 , inv ( dot ( H , P12 ) + R ) ) # Kalman filter gain mx_kp1 = mx_kp1 + dot ( K , ( y_kp1 - my_kp1 ) ) # state estimate P_kp1 = dot ( dot ( K , R ) , K . T ) + dot ( dot ( ( eye ( xDim ) - dot ( K , H ) ) , P_kp1 ) , ( eye ( xDim ) - dot ( K , H ) ) . T ) return ( mx_kp1 , P_kp1 )