基于目标运动模型和观测模型的交互多模算法IMM卡尔曼滤波目标跟踪matlab仿真

1.算法描述 IMM算法的基本思想是用多个不同的运动模型匹配机动目标的不同运动模式，不同模型间的转移概率是–个马尔可夫矩阵，目标的状态估计和模型概率的更新使用卡尔曼滤波。

   交互式多模型 IMM（Interacting Multiple Model）控制算法的主体思想是基于贝叶斯理论而提出的模型间的自动识别与切换：在任意跟踪时刻，通过设置对应目标可能模型数量的模型滤波器来进行实时的机动模型检测，对每一个滤波器设置权重系数和模型更新的概率，最后加权计算得出当前最优估计状态，从而达到模型自适应跟踪的目的。

    机动目标模型描述了目标状态随着时间变化的过程。一个好的模型抵得上大量的数据。当前几乎所有的目标跟踪算法都是基于模型进行状态估计的。在卡尔曼滤波器被引入目标跟踪领域后，基于状态空间的机动目标建模成为主要研究对象之一。

  从算法层面，在机动目标跟踪系统中，常用的滤波算法是以卡尔曼滤波器为基本框架的估计算法。卡尔曼滤波器是一种线性、无偏、以误差均方差最小为准则的最优估计算法，它有精确的数学形式和优良的使用效能。卡尔曼滤波方法实质上是一种数据处理方法，它采用递推滤波方法，根据获取的量测数据由递推方程递推给出新的状态估计。由于计算量和存储量小，比较容易满足实时计算的要求，在工程实践中得到广泛应用。 交互多模型(Interacting Multiple Model，简称IMM)算法具有自适应的特点，能够有效地对各个模型的概率进行调整，尤其适用于对机动目标的定位跟踪。交互式多模型算法包含了多个滤波器（各自对应着相应的模计器，一个交互式作用器和一个估计混合器），多模型通过交互作用跟踪一个目标的机动运动，各模型之间的转移由马尔可夫概率转移矩阵确定，其中的元素 表示目标由第i个运动模型转移到第j个运动模型的概率。

   在并行计算后，得到多个模型的后验概率，再结合每个模型的滤波估计输出结果，就可以计算各模型交互融合后的联合状态估计结果。

计算目标状态估计：

计算目标状态估计协方差：

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

3.MATLAB核心程序

T=2;
I=diag([1,1,1,1,1,1]);
Phi=[1,T,0,0,(T^2)/2,0;0,1,0,0,T,0;0,0,1,T,0,(T^2)/2;0,0,0,1,0,T;0,0,0,0,1,0;0,0,0,0,0,1];
H=[1,0,0,0,0,0;0,0,1,0,0,0];
G=[(T^2)/2,0;T,0;0,(T^2)/2;0,T;1,0;0,1];
R=[10000,0;0,10000];     %  观测噪声方差阵
alpha=0.8;               %  加权衰减因子
window=1/(1-alpha);      %  检测机动的有效窗口长度
Xm_estimate(k-1,:)=Xm_est;
if qq==1   %由非机动进入机动模型，需进行修正， 初始化
    Xm_predict(k,:)=Xm_pre;
    Xm_estimate(k,5)=[z1(1)-Xm_predict(k,1)]*2/(T^2);
    Xm_estimate(k,6)=[z1(2)-Xm_predict(k,3)]*2/(T^2);
    Xm_estimate(k,1)=z1(1);
    Xm_estimate(k,3)=z1(2);
    Xm_estimate(k,2)=Xm_estimate(k-1,2)+Xm_estimate(k,5)*T;
    Xm_estimate(k,4)=Xm_estimate(k-1,4)+Xm_estimate(k,6)*T;
    % 修正协方差阵
    Pm_estimate(1,1)=R(1,1);
    Pm_estimate(3,3)=R(2,2);
 
    Pm_estimate(1,2)=R(1,1)*2/T;
    Pm_estimate(2,1)=Pm_estimate(1,2);
    Pm_estimate(3,4)=R(2,2)*2/T;
    Pm_estimate(4,3)=Pm_estimate(3,4);
 
    Pm_estimate(1,5)=R(1,1)*2/(T^2);
    Pm_estimate(5,1)=Pm_estimate(1,5);
    Pm_estimate(3,6)=R(2,2)*2/(T^2);
    Pm_estimate(6,3)=Pm_estimate(3,6);
 
    Pm_estimate(5,5)=[R(1,1)+P(1)+P(2)*2*T+P(3)*T*T]*4/(T^4);
    Pm_estimate(6,6)=[R(2,2)+P(4)+P(5)*2*T+P(6)*T*T]*4/(T^4);
 
    Pm_estimate(2,2)=R(1,1)*4/(T^2)+P(1)*4/(T^2)+P(3)+P(2)*4/T;
    Pm_estimate(4,4)=R(2,2)*4/(T^2)+P(4)*4/(T^2)+P(6)+P(5)*4/T;
 
    Pm_estimate(2,5)=R(1,1)*4/(T^3)+P(1)*4/(T^3)+P(3)*2/T+P(2)*6/(T^2);
    Pm_estimate(5,2)=Pm_estimate(2,5);
    Pm_estimate(4,6)=R(2,2)*4/(T^3)+P(4)*4/(T^3)+P(6)*2/T+P(5)*6/(T^2);
    Pm_estimate(6,4)=Pm_estimate(4,6);
    Xm_est=Xm_estimate(k,:);
 
    qq=0;%修正后，标志qq复位（不再初始化），ua1设为１０，使不进入非机动模型
    ua1=10;
    m=0;
else 
    %  滤波方程
    Xm_predict(k,:)=(Phi*Xm_estimate(k-1,:)')';
    Q=[(Xm_estimate(k-1,5)/20)^2,0;0,(Xm_estimate(k-1,6)/20)^2];
    Pm_predict=Phi*Pm_estimate*(Phi)'+G*Q*G';
    K=Pm_predict*(H)'*inv(H*Pm_predict*(H)'+R);
    Xm_estimate(k,:)=(Xm_predict(k,:)'+K*(z1-H*Xm_predict(k,:)'))';
    Pm_estimate=(I-K*H)*Pm_predict;
    Xm_est=Xm_estimate(k,:);
    m=m+1;
    delta(k)=[Xm_estimate(k,5),Xm_estimate(k,6)]*[Pm_estimate(5,5),0;0,Pm_estimate(6,6)]*[Xm_estimate(k,5);Xm_estimate(k,6)];
    if m>=window
        ua(k)=delta(k)+delta(k-1)+delta(k-2)+delta(k-3)+delta(k-4);
        ua1=ua(k);
    else
        ua1=10;
    end
end
A105