rr2(i,k)=((Predicted_state_X(1,i,k)-posit_radar2(1))^2+(Predicted_state_X(3,i,k)-posit_radar2(2))^2)^(1/2);
           
           rr3(i,k)=((Predicted_state_X(1,i,k)-posit_radar3(1))^2+(Predicted_state_X(3,i,k)-posit_radar3(2))^2)^(1/2);            
            
           rr10(i,k)=rr1(i,k)-rr0(i,k);
           
         rr20(i,k)=rr2(i,k)-rr0(i,k);
           
           rr30(i,k)=rr3(i,k)-rr0(i,k);   
           
           zz(:,i,k)=[rr10(i,k) rr20(i,k) rr30(i,k)]';%測量預測            
           
           Measurement_residual_z(:,i,k)=Measure_z(:,k)-zz(:,i,k);%測量殘差
            
          Residual_covariance_S(:,:,i,k)=H(:,:,i,k)*Predicted_covariance_P(:,:,i,k)*H(:,:,i,k)'+R;
           
           Filter_gain_K(:,:,i,k)=Predicted_covariance_P(:,:,i,k)*H(:,:,i,k)'*Residual_covariance_S(:,:,i,k)^(-1);
           
          Updated_state_X(:,i,k)=Predicted_state_X(:,i,k)+Filter_gain_K(:,:,i,k)*Measurement_residual_z(:,i,k);
           
           Updated_covariance_P(:,:,i,k)=Predicted_covariance_P(:,:,i,k)-Filter_gain_K(:,:,i,k)*Residual_covariance_S(:,:,i,k)*Filter_gain_K(:,:,i,k)';
           
%%%%%%%% 3. Mode probability update (for i = 1,2,: : : ,Model_number): %%%%%%%%%%%    
           Model_likelihood_L(i,k)=exp(-Measurement_residual_z(:,i,k)'*Residual_covariance_S(:,:,i,k)^(-1)*Measurement_residual_z(:,i,k)/2)/((2*pi)^(3/2)*det(Residual_covariance_S(:,:,i,k)^(0.5)));
          end 
           for i=1:Model_number 
               mu=   Predicted_mode_probability_u(1,k)*Model_likelihood_L(1,k);
               mu=mu+Predicted_mode_probability_u(2,k)*Model_likelihood_L(2,k);
               mu=mu+Predicted_mode_probability_u(3,k)*Model_likelihood_L(3,k);
               Mode_probability_u(i,k)=Predicted_mode_probability_u(i,k)*Model_likelihood_L(i,k)/mu;
           end
         
%%%%%%%% 4. Estimate fusion: %%%%%%%%%%%         
            Overall_estimate_X(:,k)=                        Updated_state_X(:,1,k)*Mode_probability_u(1,k);
            Overall_estimate_X(:,k)=Overall_estimate_X(:,k)+Updated_state_X(:,2,k)*Mode_probability_u(2,k);
            Overall_estimate_X(:,k)=Overall_estimate_X(:,k)+Updated_state_X(:,3,k)*Mode_probability_u(3,k);
            Overall_covariance_P(:,:,k)=                            ( Updated_covariance_P(:,:,1,k)+(Overall_estimate_X(:,k)-Updated_state_X(:,1,k))*(Overall_estimate_X(:,k)-Updated_state_X(:,1,k))' )*Mode_probability_u(1,k);
            Overall_covariance_P(:,:,k)=Overall_covariance_P(:,:,k)+( Updated_covariance_P(:,:,2,k)+(Overall_estimate_X(:,k)-Updated_state_X(:,2,k))*(Overall_estimate_X(:,k)-Updated_state_X(:,2,k))' )*Mode_probability_u(2,k);
            Overall_covariance_P(:,:,k)=Overall_covariance_P(:,:,k)+( Updated_covariance_P(:,:,3,k)+(Overall_estimate_X(:,k)-Updated_state_X(:,3,k))*(Overall_estimate_X(:,k)-Updated_state_X(:,3,k))' )*Mode_probability_u(3,k);
                            
           
      %  end  %模式周期循環(huán)結束
    end      %時間周期循環(huán)結束
    RMS(n,:)=((True_x-Overall_estimate_X(1,:)).^2+(True_y-Overall_estimate_X(3,:)).^2).^(1/2);  
    CV_Mode_probability_u(n,:)=Mode_probability_u(1,:);
    CT_Mode_probability_u(n,:)=Mode_probability_u(2,:);
    CT3_Mode_probability_u(n,:)=Mode_probability_u(3,:);
    %為了計算兩種模型的使用概率
end          %100次蒙特卡洛實驗結束
figure(2)
plot(Overall_estimate_X(1,:),Overall_estimate_X(3,:))
grid
xlabel('x')
ylabel('y')
title('經過IMM融合算法目標的跟蹤軌跡')
%axis([5 15 55 65])
for k=1:All_time
    Mean_RMS_IMM(k)=mean(RMS(:,k));
    CV_Mode_probability(k)=mean(CV_Mode_probability_u(:,k));
    CT_Mode_probability(k)=mean(CT_Mode_probability_u(:,k));
    CT3_Mode_probability(k)=mean(CT3_Mode_probability_u(:,k));
end

figure(3)
plot(Mean_RMS_IMM)
grid   
xlabel('采樣時間 /s')
ylabel('RMS(均方根誤差)')
title('跟蹤誤差（IMM）')
k=1:All_time;
figure(4)
plot(k,CV_Mode_probability,'--*',k,CT_Mode_probability,'r',k,CT3_Mode_probability)
grid   
xlabel('采樣時間 /s')
ylabel('Mode probability(模型利用概率)')
title('模型利用概率比較')
axis([0 400 0 1])
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%   以下作圖比較IMM_CV_CT和CV的跟蹤效果，CV的跟蹤程序可以參考自己編寫的CV2.m文件 
%%%%%%   以下的CV的跟蹤程序也是從CV2.m文件復制過來的，部分有刪減(因為上面重復)，
%%%%%%   狀態(tài)矢量[x Vx y Vy]'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
T=1;
c=3*10^5;%光速度（公里）
All_time=400;
deta_T=3*10^(-9);%觀測時間的標準方差
deta_r=deta_T*c;%因為 r=c*gap_time 所以時間的方差可以轉化成為距離差的方差
posit_radar0=[0 5];
posit_radar1=[-30 0 ];
posit_radar2=[30 0];
posit_radar3=[0 -5];

X=zeros(4,All_time);
Begin_posit_x=10;
Begin_posit_y=100;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%% 計算真實目標軌跡（省略了） %%%%%%%%%%%%  
%%% 
%%%%%%%%%%%% 計算真實的距離差（省略了） %%%%%%%%%%%%  
%%%
%%%%%%%%%%%% 進行Kalman濾波（狀態(tài)方程是線性的，觀測方程是非線性的） %%%%%%%%%%%%     
X(:,1)=[10 0 100 -0.2]';%狀態(tài)初始值[x Vx y Vy]
F=[1 T 0 0
          0 1 0 0
          0 0 1 T
          0 0 0 1];%轉移矩陣
G=[T^2/2  0
          T      0
          0      T^2/2
          0      T];%CV模型
M(:,:,1)=eye(4);
Q=G*G'*0.000001;%即論文的q1=0.00001
R=eye(3)*deta_r^2;
syms x y Vx Vy 
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);
H=zeros(3,4,All_time);
Jacobian_H=jacobian([R10;R20;R30],[x Vx y Vy]);


for i=1:10;%為求RMS，經過100 次蒙特卡洛實驗,
    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 %一次Kalman濾波開始   
    XX(:,k)=F*X(:,k-1); %表示對k時刻狀態(tài)一步預測
    x=XX(1,k);
    y=XX(3,k);
    Vx=XX(2,k);
    Vy=XX(4,k);
    H(:,:,k)=eval(Jacobian_H);
    
    MM(:,:,k)=F*M(:,:,k-1)*F'+Q;
    K(:,:,k)=MM(:,:,k)*H(:,:,k)'*[H(:,:,k)*MM(:,:,k)*H(:,:,k)'+R]^(-1);
    M(:,:,k)=[eye(4)-K(:,:,k)*H(:,:,k)]*MM(:,:,k);
          
    Measure_z(:,k)=[Disturbed_r10(k) Disturbed_r20(k) Disturbed_r30(k)]';
    
    
    rr0(k)=((XX(1,k)-posit_radar0(1))^2+(XX(3,k)-posit_radar0(2))^2)^(1/2);
    rr1(k)=((XX(1,k)-posit_radar1(1))^2+(XX(3,k)-posit_radar1(2))^2)^(1/2);
    rr2(k)=((XX(1,k)-posit_radar2(1))^2+(XX(3,k)-posit_radar2(2))^2)^(1/2);
    rr3(k)=((XX(1,k)-posit_radar3(1))^2+(XX(3,k)-posit_radar3(2))^2)^(1/2);

    rr10(k)=rr1(k)-rr0(k);
    rr20(k)=rr2(k)-rr0(k);
    rr30(k)=rr3(k)-rr0(k);
    
    zz(:,k)=[rr10(k) rr20(k) rr30(k)]';%測量預測
    
    
    X(:,k)=XX(:,k)+K(:,:,k)*(Measure_z(:,k)-zz(:,k));
  
end %一次Kalman濾波結束
RMS(i,:)=((True_x-X(1,:)).^2+(True_y-X(3,:)).^2).^(1/2);
%plot(RMS(i,:))

end %100 次蒙特卡洛實驗結束
figure(5)
plot(X(1,:),X(3,:))
grid
axis([5 15 55 65])
for k=1:All_time
    Mean_RMS_CT(k)=mean(RMS(:,k));
end

figure(6)
plot(Mean_RMS_CT)
grid   
xlabel('采樣時間 /s')
ylabel('RMS(均方根誤差)')
title('跟蹤誤差（CV）')
k=1:400;
figure(7)
plot(k,Mean_RMS_CT,'--*',k,Mean_RMS_IMM,'r')
grid   
xlabel('采樣時間 /s')
ylabel('RMS(均方根誤差)')
title('兩中算法效果比較')