agimm.m

变结构的IMM算法---自适应模型集转换的IMM算法(AGIMM)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%  利用 AGIMM algorithm对目标进行了跟踪，并与不同模型数量（这里用了3或7）的固定结构的IMM（FSIMM）进行了比较，见FSIMM3.m和FSIMM7.m
%%%%%%%%%  参考文献：《design and comparison of model-set adaptive imm algorithm for maneuvering target tracking》
%%%%%%%%%          《Engineer's guide to variable-structure multiple-model estimation for tracking》P.556
%%%%%%%%%  Zhaoting Liu  2007,12,13   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%clear
True_trajectory
T=1;
max_degree=5.6*pi/180;

Model1_number=3;%模型的总数 
G(:,:,1)=[T^2/2  0
          T      0
          0      T^2/2
          0      T     ];
G(:,:,2)=G(:,:,1);
G(:,:,3)=G(:,:,1);
distance_L=1.87*pi/180;
distance_R=1.87*pi/180;
t1=0.05;
t2=0.92;

w=[2.50 2.50]';
v=[120 1 0.2*pi/180]';
Q=[2.5^2    0
   0    2.5^2];
R=[120^2   0    0
    0      1^2  0
    0      0   (0.2*pi/180)^2]; 
            
Model_number =3;
syms X VX Y  VY   %开始求非线性观测方程线性化的Jacobian矩阵
RR=(X^2+Y^2)^0.5 ;
RV=(X*VX+Y*VY)/RR ;
BETA=atan(X/Y) ;
Jacobian_H=zeros(3,4);
Jacobian_H=jacobian([RR;RV;BETA],[X VX Y VY]);

PI=[0.8     0.1   0   
    0.1     0.8   0.1  
    0       0.1   0.8    ];

Mode_probability_u(:,1)=[0.01   0.98  0.01 ]';%初始概率

Overall_estimate_X=zeros(4,All_time);
Updated_state_X=zeros(4,Model_number,All_time);
Updated_covariance_P(:,:,1,1)=eye(4);
Updated_covariance_P(:,:,2,1)=eye(4);
Updated_covariance_P(:,:,3,1)=eye(4);

for n=1:20;%为求RMS，经过100 次蒙特卡洛实验
    Overall_estimate_X(:,1)=[60*10^3+50*rand(1,1)  -300*cos(pi/4)+10*rand(1,1)  40*10^3+50*rand(1,1)  300*sin(pi/4)+10*rand(1,1)]';
    Updated_state_X(:,1,1)=[60*10^3+50*rand(1,1)  -300*cos(pi/4)+10*rand(1,1)  40*10^3+50*rand(1,1)  300*sin(pi/4)+10*rand(1,1)]';
    Updated_state_X(:,2,1)=[60*10^3+50*rand(1,1)  -300*cos(pi/4)+10*rand(1,1)  40*10^3+50*rand(1,1)  300*sin(pi/4)+10*rand(1,1)]';
    Updated_state_X(:,3,1)=[60*10^3+50*rand(1,1)  -300*cos(pi/4)+10*rand(1,1)  40*10^3+50*rand(1,1)  300*sin(pi/4)+10*rand(1,1)]';
    Measurement_r=(True_x.^2+True_y.^2).^0.5+120*randn(1,All_time);
    Measurement_rv=(True_x.*Velocity_x+True_y.*Velocity_y)./(True_x.^2+True_y.^2).^0.5+1*randn(1,All_time);
    Measurement_beta=atan(True_x./True_y)+0.2*pi/180*randn(1,All_time); 
    %============================================================================================================================================
    %模型集初始化
    angle_left=-max_degree;
    angle_center=0;
    angle_right=max_degree;
    angle=angle_left;
    if angle~=0
       F(:,:,1)=[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)          ];
    else
        F(:,:,1)=[1         T                       0       0
                  0         1                       0       0
                  0         0                       1       T
                  0         0                       0       1   ];
    end
    angle=angle_center;
    if angle~=0
       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)          ];
    else
        F(:,:,2)=[1         T                       0       0
                  0         1                       0       0
                  0         0                       1       T
                  0         0                       0       1   ];
    end
    angle=angle_right;
    if angle~=0
       F(:,:,3)=[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)          ];
    else
        F(:,:,3)=[1         T                       0       0
                  0         1                       0       0
                  0         0                       1       T
                  0         0                       0       1   ];
    end
    %============================================================================================================================================
    for k=2:All_time  %时间周期循环开始
    
%%%%%%%% 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)*w;
  
              Predicted_covariance_P(:,:,i,k)=F(:,:,i)*Mixing_covariance_P(:,:,i,k-1)*F(:,:,i)'+G(:,:,i)*Q*G(:,:,i)';