基于方向和距离的扩展卡尔曼状态估计maltba仿真

基于距离和方向的扩展卡尔曼状态估计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