• 大小: 157KB
    文件类型: .rar
    金币: 1
    下载: 0 次
    发布日期: 2021-06-03
  • 语言: 其他
  • 标签: EKF  

资源简介

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


评论

共有 条评论