%參考文獻
%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);

O2=[0 0; 0 0]; % 2*2階的零矩陣，為了以后賦值的方便
O3=[0,0,0]'; 

f=[1 T;0 1];
F=[f O2 O2;O2 f O2;O2 O2 f]; % 狀態轉移矩陣

q=[T^4/4 T^3/2;T^3/2 T^2]; 
Q=[q O2 O2;O2 q O2;O2 O2 q]; % 模型噪聲協方差陣？gama
Qq=0.001; % 模擬加速度的過程噪聲方差？w
Q=Q*Qq;
R=[R_Q^2 0 0;0 THETA_Q^2 0;0 0 PHI_Q^2]; % VV=R觀測噪聲協方差陣

% H=[1 0 0 0 0 0 ;0 0 1 0 0 0;0 0 0 0 1 0 ]; % 觀測矩陣

p1=[R_Q^2 0;0 1];
% p2=[THETA_Q^2 0;0 1];
% p3=[PHI_Q^2 0;0 1];

P0=[p1 O2 O2;O2 p1 O2;O2 O2 p1]; % 估計誤差協方差陣初值

X0=[x0,v_x,y0,v_y,z0,v_z]'; % 狀態向量初值

%模擬軌跡
for t=1:T:hits
    if(t==1)
        X(t)=x0+v_x;
        Y(t)=y0+v_y;
        Z(t)=z0+v_z;
    else
        X(t)=X(t-1)+v_x;
        Y(t)=Y(t-1)+v_y;
        Z(t)=Z(t-1)+v_z;
    end
end

%極坐標下的分量（真值）
[THETA,PHI,Ro] = cart2sph(X,Y,Z);

% Monte Carlo 仿真的開始
for i=1:1:MCNum
    %噪聲產生于之后
    noise_r=R_Q*randn(1,hits);
    noise_theta=THETA_Q*randn(1,hits);
    noise_phi=PHI_Q*randn(1,hits);

    %模擬觀測值
    Z_R=Ro+noise_r; %X方向加噪聲
    Z_THETA=THETA+noise_theta; %Y方向加噪聲
    Z_PHI=PHI+noise_phi; %Z方向加噪聲

    %量測矢量 （3個極坐標分量）
    Z_T=[Z_R;Z_THETA;Z_PHI];

    %注意每次循環都要給X0和P0賦初值
    X0=[x0,v_x,y0,v_y,z0,v_z]'; % 狀態向量初值
    P0=[p1 O2 O2;O2 p1 O2;O2 O2 p1]; %估計誤差協方差陣初值

    %EKF濾波方程
    for t=1:T:hits % 這樣也可以
        Xk_predict=F*X0;

        xm=Xk_predict(1);
        ym=Xk_predict(3);
        zm=Xk_predict(5);

        r1=sqrt(xm^2+ym^2+zm^2);
        r2=sqrt(xm^2+ym^2);
        h1=[xm/r1, -ym/r2^2, -xm*zm/(r1^2*r2)]';
        h2=[ym/r1, xm/r2^2, -ym*zm/(r1^2*r2)]';
        h3=[zm/r1, 0, r2/r1^2 ]';
        H=[h1,O3,h2,O3,h3,O3]; %測量矩陣需要轉換為極坐標系中

        Pk_predict=F*P0*F'+Q;  %預測誤差協方差陣
        S=H*Pk_predict*H'+R;   %信息協方差陣
        K=Pk_predict*H'*inv(S);%增益矩陣

        Z_predict=[sqrt(xm^2+ym^2+zm^2),atan(ym/xm),atan(zm/sqrt(xm^2+ym^2))]';

        Xk(:,t)=Xk_predict+K*(Z_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;%10次MC的誤差平均值
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',X(t),Y(t),Z(t), 'g');%1  3  5 為距離
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方向的濾波誤差')