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