一种快速Kalman滤波算法实现,。对于某些不能够采取离线计算的滤波过程来说，它可以在保证一定精度的同时极大地提高计算速度和减少计算占用资源-　EKF　 Extended Kalman Filter for nonlinear dynamic systems

　[x, P] = ekf(f,x,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

　　　　　　z_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)

　　　　　　x:　a priori　state estimate

　　　　　　P:　a priori　estimated state covariance

　　　　　　h: fanction handle for h(x)

　　　　　　z: current measurement

　　　　　　Q: process noise covariance

　　　　　　R: measurement noise covariance

　Output:　 x:　a posteriori　state estimate

　　　　　　P:　a posteriori　state covariance



　Example: