資源簡介
基于距離和方向的擴展卡爾曼狀態(tài)估計maltba仿真。3維場景下,測量得到目標的距離和方位,通過擴展卡爾曼估計目標的位置和速度信息。適用于目標勻速運動的情況。

代碼片段和文件信息
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
評論
共有 條評論