EKF_matlab

具体说明请见 http://blog.csdn.net/apsvvfb/article/details/12651999

function [x_kP_k]=ekf_rn（fxPhzQR）

% EKF   Extended Kalman Filter for nonlinear dynamic systems
% [x P] = ekf（fxPhzQR） 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（0Q） meaning w is gaussian noise with covariance Q
%       v ~ N（0R） 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
n = size（x1）;
%Jacobians
f_derivative=@（x）[10-x（4）*sin（x（3））-x（5）*x（4）*cos（x（3））/2cos（x（3））-x（5）*sin（x（3））/2x（4）*sin（x（3））/2;
    01x（4）*cos（x（3））-x（5）*x（4）*sin（x（3））/2sin（x（3））+x（5）*cos（x（3））/2x（4）*cos（x（3））/2;
    00101;
    00010;
    00001;];
F = f_derivative（x）;
H = [1 0 0 0 0;0 1 0 0 0];
% predict
x1 = f（x）;
P = F*P*F‘+Q; 
%update
Y = z - h（x1）;
S = H*P*H‘ + R;
K = P*H‘/S;
x_k = x1 + K*Y;
P_k = （eye（n） - K*H）*P;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1275  2013-10-12 14:36  ekf_rn.m

     文件       5741  2013-10-15 13:22  test2_20101011.m

     文件      15601  2013-10-14 09:16  曲线.png

     文件      12707  2013-10-14 09:16  曲线2.png

     文件      12194  2013-10-14 09:17  直线.png

----------- ---------  ---------- -----  ----

                47518                    5