cv_ct_imm3.m

基于无源时差定位系统的机动目标跟踪IMM算法

          
          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  %模式周期循环结束
    end      %时间周期循环结束
    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文件复制过来的，部分有删减(因为上面重复)，
%%%%%%   状态矢量[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滤波（状态方程是线性的，观测方程是非线性的） %%%%%%%%%%%%     
X(:,1)=[10 0 100 -0.2]';%状态初始值[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时刻状态一步预测
    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('两中算法效果比较')