时变转移概率IMM-SRCKF机动目标跟踪算法

2015-06-19董春云蔡远利于振华

系统工程与电子技术 2015年1期

关键词：协方差机动滤波器

郭志，董春云，蔡远利，于振华

（1．西安交通大学电子与信息工程学院，陕西西安710049；2．空军工程大学信息与导航学院，陕西西安710077）

时变转移概率IMM-SRCKF机动目标跟踪算法

郭志1，董春云1，蔡远利1，于振华2

（1．西安交通大学电子与信息工程学院，陕西西安710049；2．空军工程大学信息与导航学院，陕西西安710077）

给出了一种交互多模型（interacting multiple model，IMM）算法中Markov转移概率矩阵在线修正的方法，并将平方根容积卡尔曼滤波器（square-root cubature Kalman filter，SRCKF）引入到IMM算法中，提出一种时变转移概率的机动目标跟踪IMM-SRCKF算法。该算法利用当前量测中包含的模式信息，对IMM算法中的转移概率矩阵进行实时递推估计，避免了常规IMM算法中转移概率先验确定的困难，提高了模型切换速度和跟踪精度；同时，SRCKF以目标状态协方差的平方根进行迭代更新，确保了滤波过程中协方差矩阵的对称性和半正定性，改善了数值精度和稳定性。仿真实验结果表明，该算法对机动目标的跟踪性能优于常规的IMM及IMM-CKF算法。

机动目标跟踪；交互多模型；平方根容积卡尔曼滤波；Markov转移概率

0 引言

机动目标跟踪是状态估计与信息融合领域的研究热点之一，一直被研究者所关注，其难点在于机动模型的设计和滤波器的选择［1］。由于目标机动的不可预测性，采用单一模型的滤波器常因模型选择与目标实际运动不匹配而导致滤波发散，交互多模型（interacting multiple model，IM M）算法较好地解决了这一问题。它以一组不同的子模型描述目标的运动过程，各子模型并行滤波运算，模型之间的切换服从Markov过程，最终将各模型滤波器输出的加权和作为目标运动状态的最终估计［2］。IM M算法已广泛应用于机动目标跟踪领域［310］。

Markov转移概率矩阵（以下简称Markov矩阵）是IMM算法的重要参数之一，影响模型之间的交互与切换，一般根据先验信息人为选取为固定的主对角占优矩阵［11］。研究表明，由于目标运动的不确定性，该固定先验假设的模型概率转换方式是模式切换与未切换情况下的折衷［12］。由于Markov矩阵先验信息的不确定会使滤波跟踪精度下降［9］，因此，转移概率的在线自适应估计一直是国内外研究的热点问题，文献［8，11，13- 14］分别给出了几种不同的设计方法。考虑到系统当前的模式信息隐含在当前量测中，为了提高模型的切换速度和滤波精度，应充分利用当前的量测信息，在线更新多模型滤波器的转移概率参数。本文讨论了一种简单有效的利用量测数据在线估计模型转移概率的方法，并给出了递推公式。

滤波器的选择是IMM算法中另一个重要问题，滤波器的性能直接影响着滤波与跟踪的精度。实际目标跟踪过程中，量测值多在极坐标系中得到，而目标的运动状态信息常常用直角坐标描述，二者之间的关系是非线性的，需要选择合适的非线性滤波器。最经典的非线性滤波方法是扩展卡尔曼滤波（extended Kalman filter，EKF），思路和实现都比较简单，但由于一阶线性化过程中会引入近似误差，而且需要计算Jacobian矩阵，影响了滤波的精度和实时性，对于强非线性问题滤波容易发散。粒子滤波算法（particle filter，PF）计算量过大，难以满足目标跟踪的实时性要求。文献［15］提出的无迹卡尔曼滤波（unscented Kalman filter，UKF）利用UT变换对非线性概率密度进行近似，对任何非线性系统的近似精度都能达到二阶以上，精度高于EKF，计算量远小于PF。将UKF作为IMM算法中滤波器的IMM-UKF算法已经得到了广泛的应用［1，3］。但运用UKF滤波器处理高维系统（n≥4）时必须合理地选择参数才能达到较高的精度［15］。

容积卡尔曼滤波器（cubature Kalman filter，CKF）是文献［16］提出的一种新的非线性滤波器，它采用一组等权值的Cubature点，基于球面径向准则求解Bayesian滤波的积分，与EKF、UKF等相比具有更好的非线性逼近能力，数值精度更高，稳定性更好。文献［5］首次将CKF滤波器应用到IMM算法中，设计了一种用于机动目标跟踪的IMMCKF算法，取得了比IMM-UKF更好的跟踪效果。此后，IMM-CKF算法已经越来越受到国内外学者的关注［4，6，10，17］。但由于IMM-CKF采用的是常规的CKF滤波器，而常规CKF在计算中，每步迭代都需要进行矩阵开方运算，计算机舍入误差的积累容易引起误差协方差矩阵失去非负定性甚至对称性，甚至导致滤波结果发散，严重影响目标的跟踪性能。平方根CKF算法［18］（square-root cubature Kalman filter，SRCKF）通过引入正交三角分解，避免了每步迭代的矩阵开方运算，而是直接对协方差矩阵的平方根进行迭代计算，解决了常规CKF算法中滤波结果易发散的问题，提高了滤波的精度和稳定性。

本文在IMM-CKF算法的基础上，借鉴平方根UKF改善常规UKF滤波计算数值精度和稳定性的思想［3，19］，将一种新的更具数值稳定性的SRCKF滤波器与IMM算法相结合，并引入一种时变转移概率的技巧，提出了带有转移概率在线修正的IMM-SRCKF机动目标跟踪算法，为解决目标跟踪中非线性系统的动态滤波问题提供了一种新的思路与方法。计算机仿真结果表明，与IMM-CKF相比，本文算法能有效提高滤波精度和稳定性，实时性更好，具有更好的目标机动跟踪能力。

1 SRCKF算法

对于离散时间动态系统

式中，xk∈Rnx为目标状态向量；zk为传感器量测向量。假设过程噪声wk－1～N（0，Qk－1），量测噪声vk～N（0，Rk），且wk－1和vk相互独立。

SRCKF算法首先要计算Cubature点和对应的权值。3阶容积准则下的基本Cubature点和对应权重为

式中，m表示Cubature点总数，且m＝2nx，记nx维单位向量为e＝［1，0，…，0］T；符号［1］表示对e中元素进行全排列和取反所生成的点集，称为完整全对称点集，［1］j表示点集［1］中的第j个点。以nx＝2为例，［1］表示为

如果k－1时刻状态估计的后验概率分布为p（xk－1| z1∶k－1）～N（xk－1；^xk－1，Pk－1），方差Pk－1的Cholesky分解因子为Sk－1，即Sk－1＝Chol｛Pk－1｝。SRCKF算法步骤如下：

步骤1 时间更新

步骤1.1 计算状态Cubature点

步骤1.2 计算状态Cubature点的传播

步骤1.3 计算状态一步预测和预测方差的平方根

式中，Tria（·）为三角分解；SQ，k－1＝Chol（Qk－1）；¯Sk为下三角阵；矩阵χ*k定义为

步骤2 量测更新

步骤2.1 计算量测Cubature点

步骤2.2 计算量测Cubature点的传播

步骤2.3 计算量测一步预测、预测误差协方差的平方根和协方差

式中，矩阵γk、χk定义为

步骤2.4 计算滤波增益矩阵

式中，“／”表示矩阵的右除。

步骤2.5 计算k时刻的最优状态估计和协方差的平方根

2 IMM-SRCKF算法

IMM算法包含交互作用器、卡尔曼滤波器、模型概率估计器和估计混合器4个部分。采用第1节给出的SRCKF算法作为IMM算法中的卡尔曼滤波器，便可得到IMM-SRCKF算法。现给出IM M-SRCKF算法从k－1到k时刻的递推步骤。

步骤1 输入初值交互

令^xj（k－1|k－1）为k－1时刻滤波器j的状态估计，Pj（k－1|k－1）为相应的状态协方差阵，且假设系统r个模型间的转移服从Markov链，其转移概率为则交互计算后滤波器j

在k时刻的输入初值为

步骤2 卡尔曼滤波

步骤3 模型概率更新

假定模型j的滤波残差服从高斯分布，则其似然函数和模型概率更新分别为

式中，；滤波残差vj（k）及其协方差Sj（k）分别表示为

步骤4 状态估计与协方差组合

设^x（k|k）、P（k|k）分别为k时刻的状态估计及其协方差，则有

3 模型转移概率实时修正算法

上述IMM算法假定输入交互过程为一Markov过程，模型之间的跳变规律服从Markov链［2021］，即

式中，M（k）＝Mj表示j时刻系统模式为Mj；pij是系统由模型i转移到模型j的转移概率。

转移概率决定着输入交互的作用程度，而标准IMM中Markov矩阵Pt是根据历史先验信息或主观决策在滤波开始前确定并保持不变的，由于目标的机动性和先验信息的失真，这种先验假设的Markov矩阵并不能反映实际目标运动模式的转换，给跟踪滤波器带来了误差［9］。为此，我们考虑用后验信息对其进行实时修正。由于目标的实时运动模型影响着当前的量测信息，所以可以充分利用当前的量测，在线更新IMM算法中模型转移概率，使之符合实际情况。

对于IMM算法的r个子模型，k时刻子模型j的概率uj（k）越大，说明此子模型与真实的目标运动模式越匹配，那么其他子模型向这一匹配模型转移的概率应越大。基于这种思想对Markov矩阵Pt进行修正。

假设k－1时刻模型j概率为uj（k－1），k时刻的模型概率为uj（k），那么相邻时刻的模型概率之差反映了某一模型与实际运动模式的匹配程度的变化，这种后验信息可以用来修正模型之间的转移概率。考虑到转移概率值的非负性，取对数形式的模型概率变化率为

从式（21）可以看出，当模型j的概率增大时，κj（k）＞1；当模型j的概率减小时，κj（k）＜1。基于此，可以利用κj（k）与Markov矩阵Pt中的元素相乘对其进行修正。假设k－1时刻Markov矩阵第i行第j列的元素为pij（k－1），用κj（k）对其进行修正，可得

考虑到k时刻某一模型向所有其他模型（包括自身）的转移概率之和应该为1，为满足这一性质需对式（22）进行归一化。对于i，j＝1，2，…，r，最终得到k时刻修正后的转移概率计算公式为

从式（23）可知，当模型j的概率随时间增大时，修正后的Markov矩阵Pt的第j列元素随之增大，那么在下一时刻滤波进行前的模型相互交互中，模型概率大的子模型（匹配模型）的估计输出在交互过程中所占的比重更大；相反，模型概率较小（非匹配模型）的子模型滤波器的估计输出在交互过程中所占的比重减小。通过这种利用量测数据自适应地修正模型转移概率的方法，放大了匹配模型的作用，抑制了非匹配模型的作用。模型转换过程中更多地利用匹配模型的信息，而减小非匹配模型的影响，使收敛速度得到提高［14］。不难看出，修正之后的转移概率仍然满足转移概率的两条基本性质［8］，即

带转移概率修正的IMM-SRCKF算法流程如图1所示。

图1 转移概率修正IMM-SRCKF算法流程图

4 仿真与结果分析

以二维平面坐标系内的单雷达单机动目标跟踪为例，目标x和y方向初始位置和速度分别为（10 km，35 km）和（－270 m／s，290 m／s），在平面内做直线-转弯机动，仿真周期T＝5 s，仿真时长100T。目标实际运动过程如下：

（1）t＝0～26T，直线运动；

（2）t＝27T～43T，以角速度ω1＝1°／s向左做慢速转弯机动；

（3）t＝44T～68T，直线运动；

（4）t＝69T～73T，以角速度ω2＝3°／s向右做快速转弯机动；

（5）t＝74T～100T，直线运动。

实际跟踪过程中，目标的机动形式和发生的时刻等信息对于跟踪雷达来说是未知的，因此机动目标模型集的建立是IMM算法研究的热门，本文不做详细论述，这里假设所建立的模型集可以覆盖目标可能的运动模式，但目标机动时刻和机动大小对于雷达仍具有很强的不确定性。文中采用CV模型和CT模型对目标进行建模［5，22］，即模型个数r＝2。取状态向量x＝［x，x·，y，˙y，ω］，其中x和y分别为目标在x轴和y轴的位置分量，x·和˙y分别为目标在x轴和y轴的速度分量，ω为转弯角速度。对于匀速直线运动，其状态方程为

其中

系统过程噪声为

式中，q1和q2分别为x和y方向的过程噪声系数，这里取q1＝q2＝0.01。

对于圆周转弯运动，其状态方程为

其中

相应地可计算其过程噪声

式中，q1，q2同上，q3为转弯速度噪声系数，且q3＝10－6。

假设量测雷达位于二维平面（x0，y0）坐标处，这里x0＝y0＝20 km，并可以对目标的距离和方位角进行实时量测，量测方程［23］为

假设量测误差服从正态分布并相互独立，即vk～N（0，R），且R＝diag（σr2，σθ2），这里σr＝10 m，σθ＝0．1°。

滤波器的初值计算方法为：由1时刻的量测解算目标位置坐标，由0时刻和1时刻的量测数据利用差分法求速度，并令初始转弯速率ω＝0，可得1时刻目标的状态初值为

状态初值估计协方差［24］可由下式确定：

式中

a为转弯速率初值估计方差，这里取a＝1。

文献［5］和文献［10］已经证实IMM-CKF比IMMUKF和IMM-EKF等具有更强的跟踪性能，因此，这里仅将本文算法与IMM-CKF进行比较。进行N＝100次蒙特卡罗仿真，设定IMM算法中两个子模型的初始概率均为0．5，且初始Markov矩阵取为

算法的跟踪性能可以通过位置分量和速度分量的均方根误差（root mean square error，RMSE）［6］和累计均方根误差（accumulative root mean square error，ARMSE）［25］曲线做比较进行评价，位置RMSE和ARMSE的表达式为

式中，k＝1，2，…，K为仿真时间序列；n＝1，2，…，N为蒙特卡罗仿真次数。同理可得速度相应公式。

图2给出了本文算法的目标跟踪轨迹与真实运动轨迹，为了更清楚地展示算法跟踪效果，图3和图4对图2中两个转弯机动时刻附近的雷达跟踪效果进行了局部放大。图5～图8为本文算法与IMM-CKF的对比，其中位置RMSE和速度RMSE分别如图5和图6所示，模型概率变化曲线如图7和图8所示。表1给出了两种算法跟踪性能指标的统计对比。仿真平台是2．33G主频CPU、2G内存的PC机，软件环境是Matlab 2009。

图2 目标真实运动轨迹与雷达跟踪轨迹

图3 目标第1次转弯机动时刻附近跟踪轨迹

图4 目标第2次转弯机动时刻附近跟踪轨迹

图5 位置均方根误差变化曲线

仿真结果表明，本文算法和IMM-CKF算法均得到了较好的目标跟踪效果，没有出现滤波发散现象。但本文算法优于常规IMM-CKF，具体表现在：

（1）跟踪精度更高。从表1的统计数据可以发现，对于上述机动目标的跟踪，无论是位置还是速度，本文算法的跟踪精度都高于IMM-CKF，位置跟踪误差减小了9．71%，速度跟踪误差减小了32．41%。在轨迹跟踪中，当目标不发生机动或机动较小（27T）时，两种算法跟踪精度都较好，且跟踪精度差别不大；但是在目标发生较大机动时（69 T），虽然两种算法的位置和速度跟踪误差均较大，但本文算法精度明显优于IMM-CKF，表现在误差超调更小，收敛更快。

（2）模型切换速度更快更合理。从图7和图8可以看出，在t＝27T，44T，69T，74T目标机动时刻，本文算法模型概率能很快地过渡切换到真实的模型，滞后比IMM-CKF算法要小得多，且两个模型概率曲线与真实模型概率曲线十分接近。因此本文算法的模型切换速度更快，且模型转换更合理。对实际目标运动模型跟踪性能良好也是导致跟踪精度高于IM M-CKF算法的主要原因。

（3）鲁棒性、实时性更好。从表1的算法运行时间一栏可以看出，本文算法耗时0．167 s，低于IMM-CKF算法16．92%。因为本文SRCKF算法直接用协方差矩阵的平方根进行迭代计算，因此不仅避免了累计舍入误差导致的协方差矩阵负定，提高了数值鲁棒性［26］，而且不需要常规CKF算法中每次迭代需要的矩阵分解，所以实时性更好。进一步对仿真结果数据进行分析可知，本文算法跟踪误差方差更小，稳定性更好。

图7 CV模型概率变化曲线

图8 CT模型概率变化曲线

表1 本文算法与IMM-CKF算法跟踪性能对比

5 结论

本文提出的时变转移概率IM M-SRCKF算法融合了IMM算法对目标不同机动模式的自适应能力和SRCKF滤波精度高的优点，有效克服了常规IMM-CKF算法模型转移概率不准确、数值稳定性差的缺点，跟踪精度、模型切换速度和计算量都优于常规IMM-CKF算法，是一种非常有效的机动目标跟踪算法。如何进一步提高算法在目标模型切换时的估计精度，是我们下一步打算开展的工作。

［1］Jiang B A，Wan Q．Maneuvering target passive tracking with dual infrared observers using IMM algorithm based on UKF［J］．Systems Engineering and Electronics，2008，30（8）：1454- 1459．（江宝安，万群．基于UKF-IMM的双红外机动目标跟踪算法［J］．系统工程与电子技术，2008，30（8）：1454- 1459．）

［2］Bar-Shalom Y，Li X R，Kirubarajan T．Estimation with applications to tracking and navigation：theory algorithms and software［M］．New York：John Wiley＆Sons，2004．

［3］Zhang SC，Hu G D．Target tracking for maneuvering reentry vehicles with interactive multiple model unscented Kalman filter［J］．Acta Automatica Sinica，2007，33（11）：1220- 1225．（张树春，胡广大．跟踪机动再入飞行器的交互多模型Unscented卡尔曼滤波方法［J］．自动化学报，2007，33（11）：1220- 1225．）

［4］Tian S，Chen L，Yang J，et al．A novel algorithm of maneuvering target tracking based on AIMM-CKF［C］∥Proc.of the Electronic and Mechanical Engineering and Information Technology，2011，4：2135- 2138．

［5］Wan M，Li P，Li T．Tracking maneuvering target with angleonly measurements using IMM algorithm based on CKF［C］∥Proc.of the Communications and Mobile Computing，2010，3：92- 96．

［6］Li W，Jia Y．Location of mobile station with maneuvers using an IMM-based cubature Kalman filter［J］．IEEE Trans.on Industrial Electronics，2012，59（11）：4338- 4348．

［7］Du J S，Bi X．An adaptive interacting multiple model for vehicle target tracking method［C］∥Proc.of the 3rd International Conference on Advanced Measurement and Test，2013：1286- 1289．

［8］Zuo D G，Han C Z，Zheng L，et al．Maneuvering target tracking based on time-varying Markov transition probabilities［J］．Journal of Xi’an Jiaotong University，2003，37（8）：824- 828．（左东广，韩崇昭，郑林，等．基于时变马尔科夫转移概率的机动目标多模型跟踪［J］．西安交通大学学报，2003，37（8）：824- 828．）

［9］Yang C Y，Chen B S，Liao F K．Mobile location estimation using fuzzy-based IMM and data fusion［J］．IEEE Trans.onMobile Computing，2010，9（10）：1424- 1436．

［10］Chen H，Shan G L．Maneuvering target tracking based on IMMCKF algorithm［J］．Electronics Optics&Control，2011，18（10）：1- 5．（陈海，单甘霖．基于IMMCKF的机动目标跟踪算法［J］．电光与控制，2011，18（10）：1- 5．）

［11］Doucet A，Ristic B．Recursive state estimation for multiple switching models with unknown transition probabilities［J］．IEEE Trans.on Aerospace and Electronic Systems，2002，38（3）：1098- 1104．

［12］Liang Y，Cheng Y M，Jia Y G，et al．Analysis on the performance and properties of interacting multiple models algorithm［J］．Control Theory and Applications，2001，18（4）：487- 492．

［13］Wang G．ML estimation of transition probabilities in jump Markov systems via convex optimization［J］．IEEE Trans.on Aerospace and Electronic Systems，2010，46（3）：1492- 1502．

［14］Feng P W，Huang C Q，Cao L P，et al．Research on adaptive Markov matrix IMM tracking algorithm［J］．Systems Engineering and Electronics，2013，35（11）：2269- 2274．（封普文，黄长强，曹林平，等．马尔可夫矩阵修正IMM跟踪算法［J］．系统工程与电子技术，2013，35（11）：2269- 2274．）

［15］Julier S J，Uhlmann J K．Unscented filtering and nonlinear estimation［J］．Proceedings of the IEEE，2004，92（3）：401- 422．

［16］Arasaratnam I，Haykin S．Cubature Kalman filters［J］．IEEE Trans.on Automatic Control，2009，54（6）：1254- 1269．

［17］Chen H，Shan G L．Attitude angle aided IMMCKF algorithm［C］∥Proc.of the 10th IEEE International Conference on Electronic Measurement&Instruments，2011：197- 200．

［18］Arasaratnam I，Haykin S，Hurd T R．Cubature Kalman filtering for continuous-discrete systems：theory and simulations［J］．IEEE Trans.on Signal Processing，2010，58（10）：4977- 4993．

［19］Jafarzadeh S，Lascu C，Fadali M S．Square root unscented Kalman filters for state estimation of induction motor drives［J］．IEEE Trans.on Industry Applications，2013，49（1）：92- 99．

［20］Blom H A P，Bar-Shalom Y．The interacting multiple model algorithm for systems with Markovian switching coefficients［J］．IEEE Trans.on Automatic Control，1988，33（8）：780- 783．

［21］Cao Y，Bao M．Improved IMMPF tracking methods for airborne laser communication［J］．Research Journal of Applied Sciences，Engineering and Technology，2014，7（3）：462- 465．

［22］Li X R，Jilkov V P．Survey of maneuvering target tracking．Part I．Dynamic models［J］．IEEE Trans.on Aerospace and Electronic Systems，2003，39（4）：1333- 1364．

［23］Li X R，Jilkov V P．Survey of maneuvering target tracking：III．measurement models［C］∥Proc.of the SPIE Signal and Data Processing of Small Targets，2001：423- 446．

［24］He Y，Xiu J J，Guan X．Radar data processing with applications［M］．3rd ed．Beijing：Publishing House of Electronics Industry，2013：44- 47．（何友，修建娟，关欣．雷达数据处理及应用［M］．3版．北京：电子工业出版社，2013：44- 47．）

［25］Arasaratnam I．Sensor fusion with square-root cubature information filtering［J］．Intelligent Control&Automation，2013，4（1）：11- 17．

［26］Arasaratnam I．Cubature Kalman filtering：theory＆applications［D］．Hamilton：Mc Master University，2009．

Time-varying transition probability based IMM-SRCKF algorithm for maneuvering target tracking

GUO Zhi1，DONG Chun-yun1，CAI Yuan-li1，YU Zhen-hua2
（1.School of Electronic and Information Engineering，Xi’an Jiaotong University，Xi’an 710049，China；2.School of Information and Navigation，Air Force Engineering University，Xi’an 710077，China）

An on-line updating method of Markov transition probability for the interacting multiple model（IMM）algorithm is proposed，and the square-root cubature Kalman filter（SRCKF）is introduced into IMM，so a novel time-varying Markov transition IMM-SRCKF algorithm is obtained．Using real-time recursive estimation method based on the system mode information implicit in the current measurements，the proposed algorithm effectively avoids the problem of prior determination of the Markov transition probability matrix in traditional IMM．Furthermore，SRCKF propagates the square root of the covariance in filter interaction so that it guarantees the symmetry and positive semi-definiteness of the covariance matrix and greatly improves the numerical stability and numerical accuracy．Simulation results show that the proposed algorithm has better tracking performance and higher efficiency compared with the conventional IMM and IMM-CKF．

maneuvering target tracking；interacting multiple model（IMM）；square-root cubature Kalman filter（SRCKF）；Markov transition probability

TN 953

10．3969／j．issn．1001-506X．2015．01．05

郭志（1986-），男，硕士研究生，主要研究方向为目标跟踪与非线性滤波。

E-mail：guo．zhi0621＠stu．xjtu．edu．cn

董春云（1989-），女，博士研究生，主要研究方向为飞行器轨迹优化与优化方法评估、飞行器跟踪。

E-mail：dongdong_2007y＠163．com

蔡远利（1963-），通讯作者，男，教授，博士，主要研究方向为飞行器制导控制与仿真、动态系统估计与滤波、复杂系统建模与仿真。

E-mail：ylicai＠mail．xjtu．edu．cn

于振华（1977-），男，副教授，博士，主要研究方向为信息物理融合系统形式化建模与分析、目标跟踪。

E-mail：zhenhua_yu＠163．com

1001-506X（2015）01-0024-07