基于交互式多模型(IMM)和卡尔曼滤波器(UKF)的机动目标跟踪算法matlab仿真

目录

1.理论概述

1.1 交互式多模型(IMM)

1.2 卡尔曼滤波器(UKF)

2.MATLAB核心程序

3.仿真结论

---

1.理论概述

       基于交互式多模型（IMM）和卡尔曼滤波器（UKF）的机动目标跟踪算法，是一种有效的目标跟踪方法，其基本思想是利用多个不同的运动模型匹配机动目标的不同运动模式，并使用卡尔曼滤波器进行状态估计。在机动目标跟踪中，通常需要考虑目标的加速度、速度和位置等状态量。为了处理这些状态量之间的耦合关系，可以采用非线性滤波方法，例如扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）。

1.1 交互式多模型(IMM)

       交互式多模型算法（IMM）的核心思想是利用多个不同的运动模型来匹配目标的不同运动模式。这些模型可以通过马尔可夫转移概率进行转移，从而形成一个具有多种可能性的模型概率集合。然后，利用卡尔曼滤波器对目标的状态进行估计和更新。IMM算法的流程图如图所示。

具体而言，IMM算法的流程如下：

1. 初始化：确定初始状态量、初始模型概率和滤波器参数。
2. 模型选择：根据当前状态量和模型概率，选择一个最适合的模型进行状态估计。
3. 状态估计：利用选择的模型和卡尔曼滤波器对目标的状态进行估计。
4. 模型转移：根据已知的马尔可夫转移概率，对模型概率进行更新。
5. 判断终止条件：如果满足终止条件，则结束算法；否则，返回步骤2继续执行。

       在IMM算法中，可以采用多种运动模型，例如匀速模型、匀加速模型、转弯模型等。同时，为了处理非线性问题，可以采用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）。其中，UKF算法相对于EKF算法具有更高的精度和稳定性。

1.2 卡尔曼滤波器(UKF)

        无迹卡尔曼滤波（Unscented Kalman Filter，UKF）摒弃了对非线性函数进行线性化的传统做法，采用kalman线性滤波框架，对用于预测方程，使用无迹变换（Unscented Transform,UT）来处理均值和协方差的非线性传递问题。UKF算法是对非线性函数的概率密度分布进行了近似，用一系列确定样本来逼近状态的后验概率密度，而不是对非线性函数进行近似，不需要对雅可比矩阵进行求导。同时，UKF没有把高阶项忽略，因此对于非线性分布的统计量有较高的计算精度，有效地克服了EKF的估计精度低、稳定性差的问题。

基于UKF的目标跟踪算法流程如下：

1. 初始化：确定初始状态量、初始协方差和滤波器参数。
2. 状态预测：利用非线性方程在预测值附近进行泰勒展开，忽略高阶项得到线性化的方程，然后利用卡尔曼滤波器对目标状态进行预测。
3. 更新协方差：根据已知的测量值和预测值，对协方差进行更新。
4. 状态估计：利用更新后的协方差和测量值，对目标状态进行估计。
5. 判断终止条件：如果满足终止条件，则结束算法；否则，返回步骤2继续执行。

      在实际应用中，IMM算法可以与UKF算法结合使用，以实现更精确的目标跟踪。首先，根据目标的运动特征选择合适的运动模型；然后，利用IMM算法确定各模型的初始概率；最后，利用UKF算法对目标状态进行估计和更新。

      假设我们的目标是跟踪一个在二维空间中运动的机动目标，其运动模型可以表示为：

x(k+1) = f(x(k), u(k), w(k))
y(k) = h(x(k), v(k))

其中 x(k) 和 y(k) 分别表示在时刻 k 的状态向量和测量向量，f 和 h 是已知的非线性函数，u(k) 是控制输入，w(k) 和 v(k) 是过程噪声和测量噪声。

我们可以用一个包含两个模型的IMM来处理这个目标跟踪问题：一个模型是匀速模型（C），另一个模型是匀加速模型（A）。

C模型的转移方程为：
x_c(k+1) = [x_c(k) + v_c(k)Δt]
y_c(k) = [x_c(k)]
其中 v_c(k) 是恒定的速度。

A模型的转移方程为：
x_a(k+1) = [x_a(k) + u_a(kΔt) + w_a(kΔt^2/2)]
y_a(k) = [x_a(k)]
其中 u_a(k) 是恒定的加速度。

2.MATLAB核心程序

...................................................................
for k=1:501
    %第一步 模型条件重初始化
    %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
    miu11=pai(1,1)*miu_CV/c_1;   
    miu12=pai(1,2)*miu_CV/c_2;
    miu21=pai(2,1)*miu_CA/c_1;
    miu22=pai(2,2)*miu_CA/c_2;
    %2。进行混合估计
    %匀速运动模型
    X1=X11*miu11+X22*miu21;%
    P1=(P11+(X1-X1)*(X11-X1)')*miu11+(P22+(X22-X1)*(X22-X1)')*miu21;
    PP(:,:,k)=P1;
    A1=sqrtm(P1);
    A1=A1';
    %匀加速运动模型
    X2=X11*miu12+X22*miu22;%
    P2=(P11+(X11-X2)*(X11-X2)')*miu12+(P22+(X22-X2)*(X22-X2)')*miu22;
    A2=sqrtm(P2);
    A2=A2';
    %----------------------------------------------------------------------
    %第二步 模型条件滤波
    
    
                                                                  %%%%%UKF滤波器开始滤波%%%%%
    %%%%%%计算Sigma点%%%%%%%
    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
    %%%%%%时间更新%%%%%%%
    %利用状态方程传递取样点
  for ii=1:13
      SI1(:,ii)=FX1(si1(:,ii));
  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
    %%%%%%量测更新%%%%%%%
    %预测协方差
    Pzz1=Wc0*(zk1(:,1)-Zk1)*((zk1(:,1)-Zk1)');
    for ii=2:13
        Pzz1=Pzz1+Wc*(zk1(:,ii)-Zk1)*((zk1(:,ii)-Zk1)');
    end
    Pzz1=Pzz1+Rk1;
   
    Pxz1=Wc0*(SI1(:,1)-X1)*((zk1(:,1)-Zk1)');
    for ii=2:13
        Pxz1=Pxz1+Wc*(SI1(:,ii)-X1)*((zk1(:,ii)-Zk1)');
    end
   %计算UKF增益，更新状态向量和方差
    zk1=Z(:,k)-Zk1;
    K1=Pxz1*(inv(Pzz1));
    X1=X1+K1*(Z(:,k)-Zk1);
    P1=Pk1-K1*Pzz1*(K1)';
    Xe1(:,k)=X1;                                  
    
                                                                  %%%%%UKF滤波器开始滤波%%%%%
    %%%%%%计算Sigma点%%%%%%%
    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
    %%%%%%量测更新%%%%%%%
    %预测协方差
    Pzz2=Wc0*(zk2(:,1)-Zk2)*((zk2(:,1)-Zk2)');
    for ii=2:13
        Pzz2=Pzz2+Wc*(zk2(:,ii)-Zk2)*((zk2(:,ii)-Zk2)');
    end
    Pzz2=Pzz2+Rk2;
   
    Pxz2=Wc0*(SI2(:,1)-X2)*((zk2(:,1)-Zk2)');
    for ii=2:13
        Pxz2=Pxz2+Wc*(SI2(:,ii)-X2)*((zk2(:,ii)-Zk2)');
    end
   %计算UKF增益，更新状态向量和方差
     zk2=Z(:,k)-Zk2;
    K2=Pxz2*(inv(Pzz2));
    X2=X2+K2*(Z(:,k)-Zk2);
    P2=Pk2-K2*Pzz2*(K2)';
    Xe2(:,k)=X2;                                  
    %----------------------------------------------------------------------
    %第三步 模型概率更新
     %计算模型匹配函数
    like1=exp(-0.5*zk1'*inv(Pzz1)*zk1)/(sqrt(det(2*3.1415926*Pzz1)));
    like2=exp(-0.5*zk2'*inv(Pzz2)*zk2)/(sqrt(det(2*3.1415926*Pzz2)));   
    
    c_1=pai(1,1)*miu_CV+pai(2,1)*miu_CA;
    c_2=pai(1,2)*miu_CV+pai(2,2)*miu_CA;
    c=like1*c_1+like2*c_2;
    miu_CV=like1*c_1/c;
    miu_CA=like2*c_2/c;
    
    %----------------------------------------------------------------------
    %第四步 估计融合  
    X=X1*miu_CV+X2*miu_CA;
    
    Xe(:,k)=X;
    X11=X1;
    X22=X2;
    P11=P1;
    P22=P2;
    
end
up2242

3.仿真结论