Improved Adaptive IMM-UKF Algorithm Based on Monotonous Transformation of Model Probability

1.   引言

交互式多模型(Interacting Multiple Model, IMM)算法将目标状态定义为多个运动模型的融合来解决目标的复杂运动态势估计，目前已经有广泛应用^[1-3]，通过不同的滤波器进行目标状态估计实现目标跟踪。但由于滤波器需利用目标先验或后验信息，导致模型切换存在迟滞性和模型转移速率慢的缺点，从而影响目标跟踪精度。

IMM算法中的滤波器选择很大程度影响了目标跟踪精度，现有的主要滤波算法有扩展卡尔曼滤波(Extended Kalman Filter, EKF)、无迹卡尔曼滤波(Unscented Kalman Filter, UKF)、粒子滤波(Particle Filter, PF)等^[4,5]。EKF通过泰勒级数展开实现非线性系统线性化，近似算法导致误差增大，且计算过程涉及雅可比矩阵，计算量较大；UKF采用无迹变化(Unscented Transformation, UT)来处理均值和协方差的非线性系统^[6,7]，算法简单且精度较高；PF采用空间随机样本，独立于系统模型，精度很高，但计算量远超前两种算法。本文选用UKF作为IMM算法中的滤波器。

现有的IMM-UKF算法优化方向主要有模型选择^[8,9]、滤波算法优化^[10]及后验概率修正^[11-15]。文献[8]通过修正转弯模型对目标转弯角速度进行实时修正，减少算法中的模型数量，从而提高了运算速度。文献[9]建立Skew-T分布非对称重尾噪声来表示模型。文献[10]提出了一种改进的强跟踪自适应无迹卡尔曼滤波对目标进行跟踪，缩短误差收敛时间，有效提高了跟踪精度。文献[11]提出一种端到端学习的自适应IMM算法，提高了跟踪精度和鲁棒性。文献[12]通过对相邻时刻同一模型概率的变化修正转移概率，文献[13]定义了两模型间误差压缩率比值，对转移概率进行修正，文献[14]在文献[13]的基础上对修正因子进行改正，并设立转移概率的阈值，对大于阈值的转移模型概率进行二次修正，文献[15]在文献[14]的基础上，通过设立步长判定窗的方式，加快了模型转移后模型的稳定性。文献[11-15]均是利用后验信息修正马尔可夫转移矩阵提高算法精度，但后验信息修正需要基于新的信息，而水下目标在模型间切换初期，初速和角速度变化都较小，在运动轨迹上的体现不明显，单纯利用此信息无法使当前不匹配模型概率快速下降及匹配模型概率快速提升，因此仍存在模型之间切换的迟滞和模型转换慢的问题。

本文在现有的自适应IMM-UKF算法的基础上^[13-15]，利用后验信息中的各模型概率的单调性变化对马尔可夫转移概率矩阵及模型概率进行二次修正，实现了更快的模型转换，有效解决了模型的迟滞性，提高了目标跟踪精度。

2.   目标模型

2.1   运动模型

基于2维直角坐标系建立目标的运动模型和观测模型。本文采用匀速直线(Constant Velocity, CV)模型、匀加速直线(Constant Acceleration, CA)模型、协同转弯(Coordinate Turn, CT)模型对目标运动状态建模。目标运动的离散时间状态方程为

其中，目标状态向量${\boldsymbol{X}}\left( k \right) = {\left[ {{x_k},{y_k},{{\dot x}_k},{{\dot y}_k},{{\ddot x}_k},{{\ddot y}_k}} \right]^{\text{T}}}$分别表示目标在$x$和$y$方向的位置、速度和加速度信息。${\boldsymbol{A}}$为状态转移矩阵，${\boldsymbol{B}}$为噪声传输矩阵，${\boldsymbol{w}}\left( k \right)$为2维零均值的高斯白噪声向量。

2.2   观测模型

以声呐位于原点对同一目标进行观测，目标的观测模型为

其中，${\boldsymbol{z}}\left( k \right){\text{ = }}\left( {{r_k},{\theta _k}} \right)$为$k$时刻距离和方位的观测值；${\boldsymbol{X}}\left( k \right)$为$k$时刻目标状态信息；${{v}}\left( k \right)$为观测噪声，由传感器的量测误差决定；$h(\cdot)$为状态向量和观测值之间的转换方程，其转换关系为

声呐在实际量测过程中，量测误差会随着距离的增大而增大。为使模型更加贴近实际，可以将其考虑为随距离线性增大，假设声呐的距离误差取实际量测值的$\alpha$倍，方位误差取固定值$\varphi$ rad，因此噪声协方差矩阵${\boldsymbol{G}}\left( k \right)$为

其中，${r_k}$为$k$时刻目标距离，${\boldsymbol{v}}\left( k \right)$为噪声传输矩阵，${{\boldsymbol{\omega}} _k}$为2维零均值的高斯白噪声向量。

3.   自适应IMM-UKF算法

3.1   标准IMM-UKF算法

标准的IMM-UKF算法可分为4个模块，可根据上一时刻状态的输入交互、UKF滤波、模型概率更新及状态估计融合输出交互计算，算法原理如图1所示，初始化目标状态估计及模型概率和模型转移概率，交互后作为IMM模型的输入，通过UKF对模型概率进行更新，最后通过对不同的模型进行交互，得到各模型状态和对应的协方差的融合值，估计混合器将结果输出。

图  1  IMM-UKF算法流程图

下载: 全尺寸图片 幻灯片

标准的IMM-UKF算法的基本过程如下。

(1) 交互输入。由$k$时刻的目标状态估计和每个模型对应的概率，得到滤波器输入的状态估计和协方差。

其中，式(5)和式(6)分别为对模型概率和协方差矩阵概率的初始化，${{\boldsymbol{\hat X}}^{0j}}\left( {k\left| k \right.} \right)$为$k$时刻模型$j$的状态交互值，${{\boldsymbol{\hat X}}^i}\left( {k\left| k \right.} \right)$为$k$时刻滤波器$i$的状态输出值，${{\boldsymbol{\hat P}}^{0j}}\left( {k\left| k \right.} \right)$为$k$时刻$j$的协方差矩阵状态交互值，${{\boldsymbol{\hat P}}^i}\left( {k\left| k \right.} \right)$为$k$时刻滤波器$i$的协方差矩阵输出值。${\mu _{ij}}\left( {k + 1\left| k \right.} \right)$为转移概率矩阵从模型$i$切换至$j$的概率，式(7)中，${\pi _{ij}}$为转移概率矩阵的第$i$行第$j$列元素，${\mu _i}\left( k \right)$为$k$时刻$i$模型为匹配模型的概率预测值。

(2) UKF滤波。通过交互作用输出的混合状态估计和混合协方差，在不同运动模型下的UKF滤波器滤波，得到$k + 1$时刻状态估计${{\boldsymbol{\hat X}}_i}\left( {k\left| k \right.} \right)$和协方差估计${{\boldsymbol{\hat P}}_i}\left( {k\left| k \right.} \right)$。

(3) 模型概率更新。

其中，${\varLambda _j}\left( {k + 1} \right)$为模型的似然概率；${{\boldsymbol{z}}_j}\left( {k + 1} \right)$为量测残差；${{\boldsymbol{S}}_j}\left( {k + 1} \right)$为残差协方差，通过UKF滤波获得，${\mu _j}\left( {k + 1} \right)$为$k + 1$时刻模型$j$为匹配模型的概率，$\mu_i(k)$为$k$时刻模型$i$为匹配模型的概率。

(4) 估计融合。对当前时刻各个子滤波器输出的状态估计值和对应的协方差进行加权求和得到融合后的状态估计。

3.2   自适应IMM-UKF算法

标准的IMM-UKF算法根据先验信息确定转移概率矩阵，这种办法可以很好地解决多模型切换的问题，但是由于使用的是先验信息，必然会存在模型切换迟滞的问题，进而导致跟踪精度不高。基于此，文献[13]依据当前时刻自适应更新转移概率${\pi _{ij}}$，修正后的转移概率${\pi '_{ij}}$为

但实际应用中，转移概率矩阵易发散，文献[14]对概率转移矩阵进一步修正，重新定义修正因子为

其中，$l$为调整系数，该值越大调整速度越快，该种办法仍存在下一时刻模型转移错误的问题。文献[15]在文献[14]的基础上，提出用步长定义判定窗对转移概率进行2次修正，若模型概率最大的模型在3个步长内有2个步长为同一模型，且模型概率高于设定的阈值，则提高该模型的转移概率。该方法可使匹配模型概率维持在较高水平，从而提高模型匹配时的跟踪精度。若模型发生切换，该法转移概率会相应改变，但无法快速增加匹配模型概率和减少不匹配模型概率，使不匹配模型在较长时间内仍为概率最高的模型，模型间的切换速度会因此滞后。

4.   改进自适应IMM-UKF算法

模型切换速度是影响跟踪精度的重要因素。若模型发生切换，当前概率极大模型变为不匹配模型，其概率呈递减趋势，与目标运动状态匹配的模型概率呈递增趋势，但由于模型概率变化是一个渐变的过程，易出现切换滞后的问题。因此，为了加快模型切换速度，提出一种根据模型概率单调性变换对转移概率矩阵和模型概率进行2次修正的改进算法。该法将极大模型概率的单调性变化作为模型切换的判定依据，分两类判定修正其模型转移概率，从而加快模型概率变化过程。

根据文献[14,15]的仿真数据分析，在发生模型切换的情况下，以3个运动模型的交互式多模型算法为例，修正规则如下：

(1) 排序，根据时刻各个模型概率的预测值由大到小进行排序，定义标志矩阵$L$, $k$时刻模型概率极大值、次极大值和极小值分别赋值${l_{{\text{max}}}}\left( k \right) = 1$, ${l_{{\text{mid}}}}\left( k \right) = 0$, ${l_{{\text{min}}}}\left( k \right) = - 1$。则

其中，标志矩阵${\boldsymbol{L}}$的横排为各模型的标志值，纵列为各时刻模型的标志值。

(2) 判定，通过标志矩阵${\boldsymbol{L}}$连续3个步长的标志值进行两类判定

第1类判定：

第2类判定：

其中，$l\left( {n,i} \right)$为$i$时刻第$n$个矩阵的标志值，${\mu _n}\left( k \right)$为$k$时刻$n$模型的模型概率，${\lambda _1}$,${\lambda _2}$为本文设定的概率判定阈值。本文判定阈值${\lambda _1}$, ${\lambda _2}$取0.9与文献[14]相同。

第1类判定是连续3个步长，极大模型概率递增且大于判定阈值；第2类判定是连续3个步长，极大模型概率单调递减，次极大模型概率单调递增，且极大值大于判定阈值。关于判定阈值${\lambda _1}$,${\lambda _2}$的取值，通过仅改变判定阈值进行控制变量的仿真实验，结果表明${\lambda _1}$,${\lambda _2}$的取值均不宜过小，当取值低于0.9时，更易落入判定，使得模型切换变得不稳定，当取值高于0.9时，更难落入判定，使得算法无法实现。

(3) 调节参数

第1类判定：

其中，${\pi _{{\text{max}}}}$,${\pi _{{\text{mid}}}}$,${\pi _{{\text{min}}}}$为模型概率极大、次级大和极小的模型转移概率，${\pi '_{{\text{max}}}}$, ${\pi '_{{\text{mid}}}}$, ${\pi '_{{\text{min}}}}$为修正后模型概率极大、次级大和极小的模型转移概率。

当3个步长内的模型符合第1类判定，则判定目标状态在向极大值模型转变，将最大模型的转移概率提升至调节阈值$\lambda$，剩下的模型概率归一化。

第2类判定：

当3个步长内的模型符合第2类判定，则判定目标状态开始从极大值模型向次极大值转变，将次极大值模型的转移概率提升值调节阈值$\lambda$，剩下的模型概率归一化。调节阈值$\lambda$的概率应在[0.9,1]之间取值，以提升模型转换速度。

(4) 在数据长度小于3个步长和不满足两类判断的情况时，采用文献[14]的自适应算法进行修正。

5.   仿真分析

5.1   CA-CV-CT运动模型

为验证本文提出的自适应IMM-UKF算法的效果，进行200次蒙特卡罗仿真实验，并与文献[14,15]的算法效果进行比对。

假设目标在2维平面运动，以声呐平台位置为原点$\left( {0,0} \right)$，运动目标0～30 s做匀加速直线运动，30～150 s做匀速直线运动，150～210 s做角速度为0.02 rad/s的匀速右转弯运动，210～300 s做匀速直线运动，间隔5 s做一采样。目标初始状态向量$\left( {1\;000\;{\text{m}},2\;000\;{{{\rm{m}},0 \;{\rm{m}}/{\rm{s}},0\;{\rm{m}}/{\rm{s}},0}}{\text{.3}}\;{{\text{m}}^2}/{\rm{s}},{{0}}{\text{.3}}\;{{\text{m}}^2}/{\text{s}}} \right)$，声呐量测误差服从正态分布，协方差矩阵为${\boldsymbol{G}}\left( k \right)= {\text{diag}}\left\{ {{{\left( {\alpha {r_k}} \right)}^2},{\varphi ^2}} \right\}$，设$\varphi {\text{ = }}0.09\;{\text{rad}}$。IMM算法使用CA, CV, CT模型。3个模型在第1个目标点的概率均为${1 \mathord{\left/ {\vphantom {1 3}} \right. } 3}$，概率转移矩阵${{\boldsymbol{P}}_{ij}} = \left( 0.8,0.1,0.1;0.1,0.8, 0.1;0.1,0.1,0.8 \right)$，经过200次蒙特卡罗仿真，得到本文自适应IMM算法与文献[14,15]算法的对比如图2—图4所示。

图  2  CA-CV-CT轨迹3种算法跟踪轨迹对比

下载: 全尺寸图片 幻灯片

图  3  CA-CV-CT轨迹3种算法均方根误差对比

下载: 全尺寸图片 幻灯片

图  4  CA-CV-CT轨迹3种算法模型概率对比

下载: 全尺寸图片 幻灯片

图2是3种算法的跟踪轨迹对比，从图中可以看出，本文算法在目标匀速状态和转弯状态均更接近真实值，受声呐量测影响小，可以保持良好的模型状态匹配。图3是3种算法的位置均方根误差和速度均方根误差的均值对比，可以看出在模型切换的时刻，本文算法位置均方根误差突变明显更小。图4是3种算法模型概率对比，从图中可以看出，在运动模型切换至匀速转弯模型时，本文算法较文献[14]在保证切换时刻较快的同时，模型概率明显更高，较文献[15]能够提前35 s切换模型，且匹配模型的模型概率能够快速提升，匹配的模型概率基本在60%以上，不仅使得主次模型更加分明，而且算法的模型与实际情况更加匹配。表1是3种算法位置ARMSE和速度ARMSE的对比，可以看出本文算法的位置均方根误差均最小，相比文献[14]减小了6.0%，比文献[15]减小了3.8%，目标跟踪精度有所提升。

表  1  CA-CV-CT轨迹3种算法ARMSE对比

算法	位置ARMSE(m)	速度ARMSE(m/s)
本文算法	77.72	3.8
文献[14]算法	82.70	4.1
文献[15]算法	80.84	3.9

下载: 导出CSV 

| 显示表格

5.2   CV-CT(左)-CT(右)运动模型

为充分验证算法性能，参照文献[14,15]的目标轨迹及参数进行仿真对比，运动目标0～60 s做匀速直线运动，60～110 s做角速度为0.02 rad/s的匀速左转运动，110～180 s做匀速直线运动，180～240 s做角速度为0.02 rad/s的匀速右转弯运动，240～300 s做匀速直线运动。经过200次蒙特卡罗仿真，本文算法与文献[14]、文献[15]算法对比如图5—图7所示。

图  5  CV-CT(左)-CT(右)轨迹3种算法跟踪轨迹对比

下载: 全尺寸图片 幻灯片

图  6  CV-CT(左)-CT(右)轨迹3种算法均方根误差对比

下载: 全尺寸图片 幻灯片

图  7  CV-CT(左)-CT(右)3种算法模型概率对比

下载: 全尺寸图片 幻灯片

通过对比图5—图7及表2可以看出，总体上本文算法跟踪精度优于文献[14,15]算法。从图6可以看出，本文位置和速度ARMSE均优于文献[14,15]，从表2的结果可以看出，本文的目标跟踪精度较文献[14,15]均有所提升，其中位置ARMSE较文献[14]提高12.5%，较文献[15]略有提升，速度ARMSE较文献[14]提升11.1%，较文献[15]略有提升。从图7可以看出，在目标运动模型转入CT右模型时，本文算法较文献[14]提前了15 s，较文献[15]提前了35 s切换至匹配模型，改善了模型切换迟滞问题，同时模型的概率峰值高于文献[14,15]，提升了模型概率。本例中，由于目标状态改变较为频繁，因此文献[14,15]算法的模型概率较低，跟踪误差较大，本文算法通过减少迟滞和提高匹配模型概率的方式，能更好地适应模型状态快速变化，跟踪效果更优。

表  2  CV-CT(左)-CT(右)3种算法ARMSE对比

算法	位置ARMSE(m)	速度ARMSE(m/s)
本文算法	14.30	1.27
文献[14]算法	16.35	1.43
文献[15]算法	14.97	1.33

下载: 导出CSV 

| 显示表格

6.   结束语

本文针对已有自适应IMM-UKF算法中模型间切换的迟滞和切换后转换慢的不足，在文献[14,15]的基础上，提出一种改进的自适应IMM-UKF算法，利用模型概率单调性变化，对马尔可夫转移概率矩阵进行二次修正。仿真实验表明，较现有的自适应IMM-UKF算法，该算法能有效提升水下目标跟踪精度，具体而言，无论是目标运动状态平稳，还是目标运动状态频繁切换，本文算法精度均更佳，因此在实际水下目标跟踪中更具应用价值。