extendkalman.txt

卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。 对于解决很大部分的问题

%Decoupling joint probabilistic data association algorithm for multiple target tracking 

%杂波环境下多传感器的数据融合

%三维常速CV模型



clear all;

clc;



T=1;                                                     % 采样周期

hits=2000;                                               % 采样点数

MCNum=10;                                                % Monte Carlo仿真次数

Qn=50;                                                   % 观测误差标准差 

R_Q=50;                                                  % R方向观测误差标准差

THETA_Q=0.1;                                               % THETA方向观测误差标准差

PHI_Q=0.1;                                                 % PHI方向观测误差标准差

v_x=300; v_y=200; v_z=100;                               % X、Y和Z方向的速度

x0=1000; y0=5000;  z0=10000;                           % 初始位置



DX_Average=zeros(1,hits);                                % Monte Carlo仿真的需要

DY_Average=zeros(1,hits);

DZ_Average=zeros(1,hits);
_T(:,t)-Z_predict);                 %估计矩阵(最后的输出值)

    Pk=(eye(6)-K*H)*Pk_predict;                                %估计误差协方差阵

    P0=Pk;                                                     %估计误差协方差阵

    X0=Xk(:,t);                                                %估计矩阵更新

end



% [x,y,z] = sph2cart(THETA,PHI,R)



DX=abs(X-Xk(1,:));                                             %X方向的滤波误差

DY=abs(Y-Xk(3,:));                                             %Y方向的滤波误差

DZ=abs(Z-Xk(5,:));   



DX_Average=DX_Average+DX;

DY_Average=DY_Average+DY;

DZ_Average=DZ_Average+DZ;

end



DX_Average=DX_Average/MCNum;

DY_Average=DY_Average/MCNum;

DZ_Average=DZ_Average/MCNum;





t=1:hits;

subplot(2,1,1);

plot3(Xk(1,t),Xk(3,t),Xk(5,t), 'r',Z_X(t),Z_Y(t),Z_Z(t), 'g');

grid on

xlabel('x')

ylabel('y')

zlabel('z')

title('原始轨迹及其跟踪')



subplot(2,1,2);

plot(t,DX_Average(t),'r',t,DY_Average(t),'g',t,DZ_Average(t),'b');

title('X、Y和Z方向的滤波误差')