%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方向的濾波誤差')