%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%《基于無(wú)源時(shí)差定位系統(tǒng)的機(jī)動(dòng)目標(biāo)跟蹤算法》 陳　玲, 李少洪  系統(tǒng)工程與電子技術(shù)
%%% TDoA在二唯平面上需要4個(gè)觀測(cè)站(一個(gè)主站和3個(gè)輔站) ,令狀態(tài)矢量為：[x Vx y Vy]
%%% CT模型參考《A comparative study of multiple-model algorithms for maneuvering target》
%%% IMM算法流程可以參考《Survey of Maneuvering Target Tracking - Part V Multiple-Model Methods》TABLE II
%%% 用了3個(gè)模型，其他和CT_CV_IMM文件一樣 
%%%   劉兆霆  2007，11，14
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear
clf
posit_radar0=[0 5];
posit_radar1=[-30 0];
posit_radar2=[30 0];
posit_radar3=[0 -5]; % position of 4 radars

%%%%%%%%%%%% 計(jì)算目標(biāo)真實(shí)運(yùn)動(dòng)軌跡 %%%%%%%%%%%%  
%%% Step1 %%%
All_time=400;
Step1_Velocity_x=0;
Step1_Velocity_y=-0.2;
Begin_posit_x=10;
Begin_posit_y=100;
True_x(1)=Begin_posit_x;
True_y(1)=Begin_posit_y;

for k=2:200
    True_x(k)=True_x(k-1)+Step1_Velocity_x;
    True_y(k)=True_y(k-1)+Step1_Velocity_y;
    
end
%%% Step2 %%%
Velocity=0.094;
Velocity_angle=0.157;
angle(200)=-pi/2;
for k=201:220
    angle(k)=angle(k-1)+Velocity_angle;
    Step2_Velocity_x(k)=Velocity*cos(angle(k));
    Step2_Velocity_y(k)=Velocity*sin(angle(k));
        
    True_x(k)=True_x(k-1)+Step2_Velocity_x(k);
    True_y(k)=True_y(k-1)+Step2_Velocity_y(k);
   
end
%%% Step3 %%%
Step3_Velocity_x=0;
Step3_Velocity_y=0.2;
for k=221:300
    True_x(k)=True_x(k-1)+Step3_Velocity_x;
    True_y(k)=True_y(k-1)+Step3_Velocity_y;
end
%%% Step4 %%%
Velocity=0.054;
Velocity_angle=-pi/30;
angle(300)=pi/2;
for k=301:330
    angle(k)=angle(k-1)+Velocity_angle;
    Step2_Velocity_x(k)=Velocity*cos(angle(k));
    Step2_Velocity_y(k)=Velocity*sin(angle(k));
        
    True_x(k)=True_x(k-1)+Step2_Velocity_x(k);
    True_y(k)=True_y(k-1)+Step2_Velocity_y(k);
   
end
%%% Step5 %%%
Step3_Velocity_x=0;
Step3_Velocity_y=-0.2;
for k=331:400
    True_x(k)=True_x(k-1)+Step3_Velocity_x;
    True_y(k)=True_y(k-1)+Step3_Velocity_y;
end
figure(1)
plot(True_x,True_y)
grid
xlabel('x')
ylabel('y')
title('目標(biāo)真實(shí)運(yùn)動(dòng)軌跡')
axis([8 14 40 120])%axis([8 14 58 6
%%%%%%%%%%%% IMM_CV_CT filter %%%%%%%%%%%% 
Model_number=3;%模型的總數(shù)
T=1;
angle=pi/20;%CT運(yùn)動(dòng)角速度,就是上面的Velocity_angle
F(:,:,1)=[1 T 0 0
          0 1 0 0
          0 0 1 T
          0 0 0 1];%CV模型x(k)=F*X(k-1)+G*u(k-1)+G*w(k-1)
G(:,:,1)=[T^2/2  0
          T      0
          0      T^2/2
          0      T];%CV模型
F(:,:,2)=[1         sin(angle*T)/angle      0       -(1-cos(angle*T))/angle
          0         cos(angle*T)            0       -sin(angle*T)
          0         (1-cos(angle*T))/angle  1       sin(angle*T)/angle
          0         sin(angle*T)            0       cos(angle*T)];%CT模型  
angle2=-pi/30;
F(:,:,3)=[1         sin(angle2*T)/angle2      0       -(1-cos(angle2*T))/angle2
          0         cos(angle2*T)            0       -sin(angle2*T)
          0         (1-cos(angle2*T))/angle2  1       sin(angle2*T)/angle2
          0         sin(angle2*T)            0       cos(angle2*T)];%CT模型 
      
G(:,:,2)=G(:,:,1);%CT模型 
G(:,:,3)=G(:,:,1);

syms x y  Vx Vy   %開(kāi)始求非線性狀態(tài)方程線性化的Jacobian矩陣
R0=((x-posit_radar0(1))^2+(y-posit_radar0(2))^2)^(1/2);
R1=((x-posit_radar1(1))^2+(y-posit_radar1(2))^2)^(1/2);
R2=((x-posit_radar2(1))^2+(y-posit_radar2(2))^2)^(1/2);
R3=((x-posit_radar3(1))^2+(y-posit_radar3(2))^2)^(1/2);
R10=R1-R0;
R20=R2-R0;
R30=R3-R0;
Jacobian_H=zeros(3,4);
Jacobian_H=jacobian([R10;R20;R30],[x Vx y Vy]);


c=3*10^5;%光速度（公里）
deta_T=3*10^(-8);%觀測(cè)時(shí)間的標(biāo)準(zhǔn)方差
deta_r=deta_T*c;%因?yàn)?r=c*gap_time 所以時(shí)間的方差可以轉(zhuǎn)化成為距離差的方差
R=eye(3)*deta_r^2;%   R：觀測(cè)噪聲V的協(xié)方差矩陣
PI=[0.9 0.05 0.05
    0.05 0.9  0.05
    0.05 0.05  0.9];%Transfer_probability is given

Mode_probability_u=zeros(Model_number,All_time);
Mode_probability_u(:,1)=1/3; % Mode probability to be initialized %%%%%for any model?這可能有問(wèn)題：初始模型概率和應(yīng)該等于1
Updated_state_X=zeros(4,Model_number,All_time);%[x Vx y Vy]
Overall_estimate_X(:,1)=[10 0 100 -0.2]';
Updated_state_X(:,1,1)=[10 0 100 -0.2]';
Updated_state_X(:,2,1)=[10 0 100 -0.2]';
Updated_state_X(:,3,1)=[10 0 100 -0.2]';% Updated state to be initialized for any model
Updated_covariance_P(:,:,1,1)=eye(4);
Updated_covariance_P(:,:,2,1)=eye(4);
Updated_covariance_P(:,:,3,1)=eye(4);% Updated covariance to be initialized for any model


for n=1:10;%為求RMS，經(jīng)過(guò)100 次蒙特卡洛實(shí)驗(yàn)
    
    r0=((True_x-posit_radar0(1)).^2+(True_y-posit_radar0(2)).^2).^(1/2);
    r1=((True_x-posit_radar1(1)).^2+(True_y-posit_radar1(2)).^2).^(1/2);
    r2=((True_x-posit_radar2(1)).^2+(True_y-posit_radar2(2)).^2).^(1/2);
    r3=((True_x-posit_radar3(1)).^2+(True_y-posit_radar3(2)).^2).^(1/2);    
    r10=r1-r0;
    r20=r2-r0;
    r30=r3-r0;
    Disturbed_r10=r10+deta_r*randn(1,All_time);
    Disturbed_r20=r20+deta_r*randn(1,All_time);
    Disturbed_r30=r30+deta_r*randn(1,All_time);   
    
    for k=2:All_time  %時(shí)間周期循環(huán)開(kāi)始
       
       % for i=1:Model_number     %模式周期循環(huán)開(kāi)始  
            
            
%%%%%%%% 1. Model-conditioned reinitialization (for i = 1,2,: : : ,Model_number): %%%%%%%        
          for i=1:Model_number
              Predicted_mode_probability_u(i,k)=                                  PI(1,i)*Mode_probability_u(1,k-1);
              Predicted_mode_probability_u(i,k)=Predicted_mode_probability_u(i,k)+PI(2,i)*Mode_probability_u(2,k-1);
              Predicted_mode_probability_u(i,k)=Predicted_mode_probability_u(i,k)+PI(3,i)*Mode_probability_u(3,k-1);
          end


          for i=1:Model_number
              for j=1:Model_number
           Mixing_weight_u(j,i,k-1)=PI(j,i)*Mode_probability_u(j,k-1)/Predicted_mode_probability_u(i,k);
              end
          end
           
           for i=1:Model_number 
               Mixing_estimate_X(:,i,k-1)=                           Updated_state_X(:,1,k-1).*Mixing_weight_u(1,i,k-1);
               Mixing_estimate_X(:,i,k-1)=Mixing_estimate_X(:,i,k-1)+Updated_state_X(:,2,k-1).*Mixing_weight_u(2,i,k-1);
               Mixing_estimate_X(:,i,k-1)=Mixing_estimate_X(:,i,k-1)+Updated_state_X(:,3,k-1).*Mixing_weight_u(3,i,k-1);
           
           
           Mixing_covariance_P(:,:,i,k-1)=                               (Updated_covariance_P(:,:,1,k-1)+(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,1,k-1))*(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,1,k-1))')*Mixing_weight_u(1,i,k-1);
           Mixing_covariance_P(:,:,i,k-1)=Mixing_covariance_P(:,:,i,k-1)+(Updated_covariance_P(:,:,2,k-1)+(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,2,k-1))*(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,2,k-1))')*Mixing_weight_u(2,i,k-1);
           Mixing_covariance_P(:,:,i,k-1)=Mixing_covariance_P(:,:,i,k-1)+(Updated_covariance_P(:,:,3,k-1)+(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,3,k-1))*(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,3,k-1))')*Mixing_weight_u(3,i,k-1);
          
%%%%%%%% 2. Model-conditioned filtering (for i = 1,2,: : : ,Model_number): %%%%%%%%%%%
           Predicted_state_X(:,i,k)=F(:,:,i)*Mixing_estimate_X(:,i,k-1)+G(:,:,i)*[1 ; 1]*0.00001;
            
           Predicted_covariance_P(:,:,i,k)=F(:,:,i)*Mixing_covariance_P(:,:,i,k-1)*F(:,:,i)'+G(:,:,i)*G(:,:,i)'*0.00001;
          
           x=Predicted_state_X(1,i,k);
           y=Predicted_state_X(3,i,k);
           Vx=Predicted_state_X(2,i,k);
           Vy=Predicted_state_X(4,i,k);
           H(:,:,i,k)=eval(Jacobian_H);      %計(jì)算k時(shí)刻的Jacobian矩陣   

            
            Measure_z(:,k)=[Disturbed_r10(k) Disturbed_r20(k) Disturbed_r30(k)]';
          rr0(i,k)=((Predicted_state_X(1,i,k)-posit_radar0(1))^2+(Predicted_state_X(3,i,k)-posit_radar0(2))^2)^(1/2);
           
           rr1(i,k)=((Predicted_state_X(1,i,k)-posit_radar1(1))^2+(Predicted_state_X(3,i,k)-posit_radar1(2))^2)^(1/2);