基于IMM和UKF的机动目标跟踪matlab仿真

1.算法描述

    交互式多模型(Interacting Multiple Model，简称IMM)是一种算法，具有自适应的特点，能够有效地对各个模型的概率进行调整，尤其适用于对机动目标的定位跟踪。交互式多模型算法包含了多个滤波器（各自对应着相应的模计器，一个交互式作用器和一个估计混合器），多模型通过交互作用跟踪一个目标的机动运动，各模型之间的转移由马尔可夫概率转移矩阵确定，其中的元素Pij表示目标由第i个运动模型转移到第j个运动模型的概率。

　在Kalman滤波算法中用到了状态转移方程和观测方程，被估计量随时间变化，他是一种动态估计。在目标跟踪中，不必知道目标的运动模型就能够实时地修正状态参量（位置、速度等信息），有良好的适应性。但是当目标运动运动变得复杂时（比如加速、减速等），仅仅用kalman滤波得不到理想的效果。这时就需要用自适应算法。交互多模型（IMM）是一种软切换算法，现在在机动目标领域得到广泛应用。该算法主要通过两个或更多的模型来描述工作过程中可能的状态，最后通过有效的加权融合进行系统状态估计，能够很好的克服单模型估计误差大的问题。

　　IMM算法采用多个Kalman滤波器进行并行处理。每一个滤波器对应着不同的状态空间模型，不同的状态空间模型描述不同的目标运动模式，因此每一个滤波器对目标的状态估计是不同的。

　　IMM算法的基本思想：

　　1、在每个时刻，假设某个模型在现在时刻有效的前提下，通过混合前一时刻所有滤波器的状态估计值来获得与这个特定模型匹配的滤波器的初始条件；

　　2、然后对每一个并行实现正规滤波（预测和修正）步骤；

　　3、最后，以模型匹配似然函数为基础更新模型概率，并组合所有滤波器修正后的状态估计值（加权和）来得到状态估计。

　　因此IMM算法的估计结果是对不同模型所得估计的混合，而不是仅仅在每个时刻选择完全正确的模型来估计。下面我门通过一些公式来具体学习一下。

　　IMM算法步骤：

　　假定运动目标有r种运动状态，对应有r个运动模型（相当于有r个状态转移方程），假设第j个模型的目标状态方程为：

　　公式中，Wj(k)是均值为0，Gj(k)是噪声驱动矩阵,协方差矩阵为Qj的白噪声序列。各个模型之间的转移是通过马尔科夫概率转移矩阵确定的，其中元素pij表示目标由第i个运动模型转移到第j个运动模型的概率。

2.仿真效果预览
matlab2022a仿真结果如下：

3.MATLAB核心程序

    %第一步 模型条件重初始化
    %1。首先计算混合概率
    %计算c
    c_1=pai(1,1)*miu_CV+pai(2,1)*miu_CA;
    c_2=pai(1,2)*miu_CV+pai(2,2)*miu_CA;
    %计算miu_temp
    %2。进行混合估计
    %匀速运动模型
    X1=X11*miu11+X22*miu21;%
    P1=(P11+(X1-X1)*(X11-X1)')*miu11+(P22+(X22-X1)*(X22-X1)')*miu21;
    PP(:,:,k)=P1;
............................................................................
    si1(:,1)=X1;
    for ii=2:7
        si1(:,ii)=X1+((i+a)^(0.5))*A1(:,ii-1);
    end
    for ii=8:13
        si1(:,ii)=X1-((i+a)^(0.5))*A1(:,ii-7);
    end
    X1=Wm0*SI1(:,1);
    for ii=2:13
        X1=X1+Wm*SI1(:,ii);
    end
    
    Xk1=X1;
    
    Pk1=Wc0*(SI1(:,1)-X1)*((SI1(:,1)-X1)');
    for ii=2:13
        Pk1=Pk1+Wc*(SI1(:,ii)-X1)*((SI1(:,ii)-X1)');
    end
    Pk1=Pk1+Qk1; 
    
    %利用预测取样点预测测量取样点
     for ii=1:13
      zk1(:,ii)=FZ(SI1(:,ii));
     end
     
    %预测测量值
    Zk1=Wm0*zk1(:,1);
    for ii=2:13
        Zk1=Zk1+Wm*zk1(:,ii);
    end
    %%%%%%量测更新%%%%%%%
..............................................................................
    si2(:,1)=X2;
    for ii=2:7
        si2(:,ii)=X2+((i+a)^(0.5))*A2(:,ii-1);
    end
    for ii=8:13
        si2(:,ii)=X2-((i+a)^(0.5))*A2(:,ii-7);
    end
    %%%%%%时间更新%%%%%%%
    %利用状态方程传递取样点
  for ii=1:13
      SI2(:,ii)=FX2(si2(:,ii));
  end
%     %利用预测取样点，权值计算预测均值和协方差
    X2=Wm0*SI2(:,1);
    for ii=2:13
        X2=X2+Wm*SI2(:,ii);
    end
    
    Xk2=X2;
    
    Pk2=Wc0*(SI2(:,1)-X2)*((SI2(:,1)-X2)');
    for ii=2:13
        Pk2=Pk2+Wc*(SI2(:,ii)-X2)*((SI2(:,ii)-X2)');
    end
    Pk2=Pk2+Qk2; 
    
    %利用预测取样点预测测量取样点
     for ii=1:13
      zk2(:,ii)=FZ(SI2(:,ii));
     end
     
    %预测测量值
    Zk2=Wm0*zk2(:,1);
    for ii=2:13
        Zk2=Zk2+Wm*zk2(:,ii);
    end
   %计算UKF增益，更新状态向量和方差
    zk2=Z(:,k)-Zk2;
    K2=Pxz2*(inv(Pzz2));
    X2=X2+K2*(Z(:,k)-Zk2);
    P2=Pk2-K2*Pzz2*(K2)';
    Xe2(:,k)=X2;                                  
    %...........................................................................
    %第四步 估计融合  
    X=X1*miu_CV+X2*miu_CA;
end