资源简介
基于距离和方向的扩展卡尔曼状态估计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
评论
共有 条评论