parameters: (f, mx_k, P_k, h, y_kp1, Q, R, args)
return_statement: return (mx_kp1, P_kp1)
docstring: 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"
docstring_summary: 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"
docstring_tokens: 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
function: 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)
function_tokens: 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 )