function?[X_hat_new?P_new]?=?EKF（mears?F?X_hat_old?P_old?Q?R）
%
x_hat_old?=?X_hat_old（1）;
y_hat_old?=?X_hat_old（3）;
z_hat_old?=?X_hat_old（5）;

r_hat?=?sqrt（x_hat_old^2+y_hat_old^2+z_hat_old^2）;
theta_b_hat?=?atan2（y_hat_oldx_hat_old）;
theta_z_hat?=?atan2（sqrt（x_hat_old^2+y_hat_old^2）z_hat_old）;

mears_hat?=?[r_hat;theta_b_hat;theta_z_hat];

H?=?[sin（theta_z_hat）*cos（theta_b_hat）0sin（theta_z_hat）*sin（theta_b_hat）0cos（theta_z_hat）0;?
????-sin（theta_b_hat）/r_hat/sin（theta_z_hat）0cos（theta_b_hat）/r_hat/sin（theta_z_hat）000;
????cos（theta_b_hat）*cos（theta_z_hat）/r_hat0sin（theta_b_hat）*cos（theta_z_hat）/r_hat0-sin（theta_z_hat）/r_hat0];

%?predictor?step
X_hat_mid?=?F?*?X_hat_old;
P_mid?=?F*P_old*F‘?+?Q;

%?corrector?step
K?=?P_mid*H‘/（H*P_mid*H‘?+?R）;
X_hat_new?=?X_hat_mid?+?K*（mears?-?mears_hat）;
P_new?=?（eye（size（K1））-K*H）*P_mid;

end

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件????????893??2019-11-05?15:13??3D-ekf-v\EKF.m

?????文件???????1725??2019-11-05?16:06??3D-ekf-v\main.m

?????文件??????92933??2019-11-05?16:08??3D-ekf-v\matlab-11.jpg

?????文件?????110937??2019-11-05?16:04??3D-ekf-v\matlab-12.jpg

?????文件??????90671??2019-11-05?16:07??3D-ekf-v\matlab-13.jpg

?????文件????????370??2019-10-24?17:33??3D-ekf-v\measure.m

?????文件????????830??2019-11-05?16:07??3D-ekf-v\vehicle_dynamic.m

?????目錄??????????0??2019-10-30?10:09??3D-ekf-v

-----------?---------??----------?-----??----

???????????????298359????????????????????8