交互式多模型-无迹卡尔曼滤波IMM-UKF仿真一——机动目标跟踪中的应用

原创不易，路过的各位大佬请点个赞

针对机动目标跟踪的探讨、技术支持欢迎联系，也可以站内私信
WX: ZB823618313

基于IMM机动目标跟踪算法设计最重要的核心部分主要包括：

1. IMM框架
2. 滤波器选择：(这里基于UKF)
3. 目标运动模型：（这里基于CV CT）

1. IMM算法介绍

核心思想： IMM算法的基本思想是用多个不同的运动模型匹配机动目标的不同运动模式，不同模型间的转移概率是–个马尔可夫矩阵，目标的状态估计和模型概率的更新使用卡尔曼滤波。其算法流程图如图5.3所示。

具体算法推导见另一个博客，这里不再赘述

2. UKF介绍

  从算法层面，在机动目标跟踪系统中，常用的滤波算法是以卡尔曼滤波器为基本框架的估计算法。卡尔曼滤波器是一种线性、无偏、以误差均方差最小为准则的最优估计算法，它有精确的数学形式和优良的使用效能。卡尔曼滤波方法实质上是一种数据处理方法，它采用递推滤波方法，根据获取的量测数据由递推方程递推给出新的状态估计。由于计算量和存储量小，比较容易满足实时计算的要求，在工程实践中得到广泛应用。
  除此之外，非线性滤波也广泛应用与机动目标跟踪，比如：

扩展卡尔曼滤波EKF
无迹卡尔曼滤波UKF
容积卡尔曼滤波CKF
求积卡尔曼滤波QKF
中心差分卡尔曼滤波CDKF
Divided difference filter DDF
高斯混合滤波GSF
强跟踪滤波STF
粒子滤波PF
… …

UKF算法的推导及介绍，见博主其他博客

3. 目标运动模型概述

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

匀速运动模型CV
匀速转弯运动CT
匀加速运动CA
Singer模型
Jerk模型
当前统计模型CS
… …

这对这些模型的介绍，见博主其他博客

4. IMM-UKF仿真实现：案列一

4.1. 仿真参数

一、目标模型：CV CT CV
第一阶段：1:39s，匀速运动CV
第二阶段：40:91s，匀速圆周运动CT，角速度：$2\ast \pi /180;$
第三阶段：92:99s，匀速运动CV

CV CT 模型的具体方程形式见另一个博客

二、测量模型：2D主动雷达
在二维情况下，雷达量测为距离和角度
$$\begin{gathered} r_k^m=r_k+{\tilde{r}}_k \\ {b}_k^m=b_k+{\tilde{b}}_k \end{gathered}$$
其中
$$\begin{gathered} r_k=\sqrt{(x_k-x_0{)}^{+}(y_k-y_0{)}^2)} \\ {b}_k={\tan }^{-1}\frac{y_k-y_0}{x_k-x_0} \end{gathered}$$
$[x_0,y_0]$为雷达坐标，一般情况为0。雷达量测为$z_k=[r_k,b_k{]}^{\prime }$。雷达量测方差为
R k = cov ( v k ) = [ σ r 2 0 0 σ b 2 ] R_k=\text{cov}(v_k)=

Rk​=cov(vk​)=[σr2​0​0σb2​​]且${\sigma }_r=70m$， ${\sigma }_b=0.3^o$。

三、性能评估
RMSE(Root mean-squared error):蒙塔卡罗次数$M=500$，${\hat{x}}_{k|k}^i$为第$i$次仿真得到的估计。
$$\text{RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum _{i=1}^M({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k}^i)({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k}^i{)}^{\prime }}$$
$$\text{Position RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum _{i=1}^M(x_k-{\hat{x}}_{k|k}^i{)}^2+(y_k-{\hat{y}}_{k|k}^i{)}^2}$$
$$\text{Velocity RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum _{i=1}^M({\dot{x}}_k-{\hat{\dot{x}}}_{k|k}^i{)}^2+({\dot{y}}_k-{\hat{\dot{y}}}_{k|k}^i{)}^2}$$
ANEES（average normalized estimation error square）,$n$ 为状态维数，${\mathbf{P}}_{k|k}^i$为第$i$次仿真滤波器输出的估计协方差
$$\text{ANEES}(\hat{x})=\frac{1}{Mn}\sum _{i=1}^M({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k}^i{)}^{\prime }({\mathbf{P}}_{k|k}^i{)}^{-1}({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k}^i)$$

4.2. 跟踪轨迹

4.3. 位置/速度RMSE

4.4. 模型概率

4.5. 部分代码

%模型概率初始化
m=[0.8;0.1;0.1];
%模型转移概率
Pa=[0.8 0.1 0.1;
    0.1 0.8 0.1;
    0.1 0.1 0.8;];
Pa=[0.9 0.05 0.05;
    0.05 0.9 0.05;
    0.05 0.05 0.9;];

%误差存储
X_true=zeros(4,steps,runs);
Z_true=zeros(2,steps,runs);
X_err_IMM=zeros(4,steps,runs);
X_IMM=zeros(4,steps,runs);
PI=zeros(3,steps,runs);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


randn('state',sum(100*clock));  %每次给不同的状态重置随机数产生器（因为clock每次都不同）

%%

for index=1:runs %蒙特卡洛次数
    index         %显示运行次数
    
    %滤波初始化
    X=X_aver_zero+sqrtm(P_zero)*randn(4,1);%产生真实X0
 %三个滤波器的初始化   
    xk_UKF1=X_aver_zero;             %X(0|0)= X_aver_zero
    Pk_UKF1=P_zero;                  %P(0|0)= P_zero
    xk_UKF2=X_aver_zero;
    Pk_UKF2=P_zero;
    xk_UKF3=X_aver_zero;
    Pk_UKF3=P_zero;
    
    t1=39; t2=91; t3=99;t4=131;t5=steps;
    %% 产生真实轨迹
    for k=1:t1
        X=Fk1*X+Gk*sqrtm(Qk1)*randn(4,1);     %产生真实轨迹
        X_true(:,k,index)=X;
    end
    for k=t1+1:t2
        X=Fk2*X+Gk*sqrtm(Qk2)*randn(4,1);
        X_true(:,k,index)=X;
    end
    for  k=t2+1:steps
        X=Fk1*X+Gk*sqrtm(Qk1)*randn(4,1);
        X_true(:,k,index)=X;
    end
% %     for  k=t3+1:t4
% %         X=Fk3*X+Gk*sqrtm(Qk3)*randn(4,1);
% %         X_true(:,k,index)=X;
% %     end
% %     for  k=t4+1:t5
% %         X=Fk1*X+Gk*sqrtm(Qk1)*randn(4,1);
% %         X_true(:,k,index)=X;
% %     end
    
    %% 状态递归实现
    for k=1:steps
        
        v=normrnd(v_mu,[sigma_r; sigma_b]);%雷达噪声
        % 雷达测量
        [r,b] = measurements2(X_true(:,k,index),xp(:,1)); %rm=距离，bm=角度 dm=多普勒速度
        rm=r+v(1);
        bm=b+v(2);
        Z_true(:,k,index)=[rm,bm]';
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68

原创不易，路过的各位大佬请点个赞