基于平方根CKF的多传感器序贯式融合跟踪算法

刘　华, 吴　文, 王世元

(1. 南京理工大学近程高速目标探测技术国防重点学科实验室, 江苏 南京 210094;

2. 西南大学电子信息工程学院, 重庆 400715)



基于平方根CKF的多传感器序贯式融合跟踪算法

刘华1, 吴文1, 王世元2

(1. 南京理工大学近程高速目标探测技术国防重点学科实验室, 江苏 南京 210094;

2. 西南大学电子信息工程学院, 重庆 400715)

摘要：为了提高非线性序贯式融合跟踪算法的精度,提出了基于平方根容积卡尔曼滤波器的多传感器序贯式融合跟踪算法。该算法使用三阶容积数值积分原则计算非线性过程的均值和方差,克服了扩展卡尔曼滤波器存在的滤波精度低及平方根无迹卡尔曼滤波器存在的参数复杂的缺点。同时，在滤波递归过程中以协方差平方根矩阵代替协方差矩阵,这样确保了滤波算法的数值稳定性,提高了跟踪的精度。最后用已知弹道系数的再入段弹道跟踪模型对所提算法的性能进行仿真测试。仿真结果表明,所提算法具有很好的跟踪性能,是一种有效的非线性序贯式融合跟踪算法。

关键词：多传感器; 序贯式融合; 平方根容积卡尔曼滤波器; 再入段

0引言

多传感器非线性融合跟踪算法是一种将多个相同或者不同类型传感器所提供的观测信息加以融合形成统一的状态估计的算法,它能提高单传感器目标跟踪算法的准确性,因此得到了学者的广泛关注。目前已有许多非线性融合算法,主要有集中式融合算法[1-2]和分布式融合算法[3-4]。但这两种算法都是在获取所有传感器的测量值后才能对系统状态进行融合估计,因此对于实时性要求很高的系统而言,难以满足其应用要求。

针对上述两种融合算法存在的实时性问题,文献[5]提出了多传感器非线性序贯式融合算法。该算法先用第一个传感器的测量值更新系统预测状态及对应的预测协方差,接着依次用下一个传感器的测量值更新前一个传感器更新所得的状态估计及对应的协方差,最后得到该时刻目标状态的全局融合估计值及其相应的协方差。文献[5]所提出的算法不仅在融合精度上与集中式融合算法等价,而且能做到测量值即到达即滤波,具有很好的实时性。但由于文献[5]的融合算法是基于扩展卡尔曼滤波(extended Kalman filter, EKF)算法的,因此存在EKF算法的一些固有缺点:①需要计算非线性系统的雅克比矩阵,这将导致算法的稳定性降低,甚至会出现发散;②处理强非线性系统时精度会降低。

针对文献[5]存在的不足,文献[6-7]分别提出了基于平方根无迹卡尔曼滤波器(square-root unscented Kalman filter, SRUKF)的序贯式融合算法和基于SRUKF的序贯融合粒子滤波算法。由于SRUKF算法能精确地捕获任何非线性高斯系态的二阶矩,因此,与文献[5]的算法比,基于SRUKF的序贯融合算法有更高的融合精度。但SRUKF算法存在3个可调参数,需选择适当才能达到良好的滤波效果,而且需要2n+1(n是系统状态维数)个采样点,这给工程实践带来了一定限制。

文献[8-9]提出一种基于3自由度球面-径向容积准则的非线性滤波器,即容积卡尔曼滤波器 (cubature Kalman filter, CKF)。该算法使用一组等权值的容积点集来计算非线性变换后的随机变量的均值和协方差。与EKF算法相比,该算法具有更优的非线性逼近性能(三阶矩)、数值精度以及滤波稳定性。与UKF算法相比,该算法具有实现简单、运算时间短的特点。该算法一经提出就在导航[10]、目标跟踪[11-13]、数据融合[14-15]等领域得到广泛应用。

由于CKF算法在数值计算中存在着噪声信号和计算误差等因素,有时会得到负的协方差阵,从而导致CKF出现发散问题。平方根CKF (square-root cubature Kalman filter, SRCKF)[9]算法采用了平方根滤波的思想,确保了协方差阵的非负定性,因此可有效提高CKF滤波的精度和稳定性。本文在结合SRCKF和多传感器序贯式融合算法优点的基础上,提出了基于SRCKF的多传感器非线性序贯式融合跟踪算法。

本文首先给出了多传感器非线性系统的系统描述,然后详细阐述了基于SRCKF算法的多传感器序贯式融合算法,最后用再入段弹道目标运动模型仿真对比分析了基于EKF、SRUKF、CKF与SRCKF的序贯式融合跟踪算法的性能。

1系统描述

考虑一个具有加性高斯白噪声并带有个传感器的非线性动态系统

(1)

(2)

式中,E代表均值符号;δtk=1 (t=k;t,k>0); δtk=0(t≠k);Qw与Ri为正定矩阵。

2基于SRCKF的多传感器序贯式融合算系统描述

2.1SRCKF算法介绍

CKF算法在迭代过程中,由于计算机字长限制而产生的舍入误差可能导致误差协方差阵失去对称性和正定性,致使CKF算法中求平方根操作无法进行,进而导致滤波中断,影响滤波器的稳定性,而且在每次迭代时都要计算协方差阵的平方根,这需要很大的计算量。为了增加计算过程中的数值稳定性并提高计算效率,Arasaratnam和Haykin在CKF算法的基础上进一步提出了SRCKF算法。

SRCKF算法的具体流程参见文献[8]。与CKF算法相比,SRCKF算法具有以下两个优点:

(1) 在滤波过程中直接以协方差阵的三角分解因子进行递推更新,而不是协方差阵本身,这样可降低计算负担,提高了算法的计算效率;

(2) 保证了协方差阵的非负性,避免了滤波器的发散问题,提高了滤波器的数值稳定性。

2.2基于SRCKF的多传感器序贯融合算法流程

步骤 1初始化,使用三阶容积准则产生2n个基本容积点及对应的权值

(3)

式中,n为系统状态变量的维数,记n维单位向量为e=[1,0,…,0]T,符号[l]表示对e中元素进行全排列和取反所生成的完全对称点集,[l]j表示[l]中第j列向量。当n=3时,[l]可表示为

(4)

(5)

(6)

(7)

(8)

(9)

式中,qr{·}表示矩阵的QR分解。

(10)

(11)

(12)

(13)

(14)

(15)

(16)

(17)

(18)

(19)

(20)

(21)

(22)

(23)

(24)

(25)

(26)

(27)

(28)

(29)

(30)

(31)

综上所述,可以得到基于SRCKF的多传感器序贯式融合滤波算法如图1所示。

图1　基于SRCKF的序贯滤波框图

3再入段弹道目标运动模型和目标测量模型

3.1目标运动模型

再入段导弹距离地面较近,飞行时间较短,可假定地球为平面,并可忽略地球公转与自转的影响,且假定目标在再入段时只受到空气阻力和地球引力的作用,因此可建立再入段弹道的目标运动方程为

(32)

式中,v为运动速度;ρ(z)为目标周围的大气密度;β为弹道系数;g是地球引力加速度。大气密度可建模为与高度z成指数关系的函数,ρ(z)=c1e-c2z,c1与c2均为常数。

对于目标运动方程(32),其相应的离散时间目标运动状态模型通过欧拉近似得到

(33)

非线性函数D(Sk,β)表达式为

(34)

式中,g=9.81m/s2。矩阵Φ与G分别表示为

fd(Sk-1,β)为空气阻力对再入目标产生的影响,表达式为

(35)

假设过程噪声wk是零均值的高斯白噪声,其协方差矩阵为

(36)

式中,q是过程噪声强度参数。

3.2目标测量模型

雷达观测量有两个,弹道的距离r与俯仰角φ,根据几何关系,可建立测量方程为

(37)

式中,Mk=[rk,φk]T,rk是目标距离,φk是目标俯仰角,计算公式为

(38)

φk=arctan(zk/xk)+vφ

(39)

雷达测量噪声vk=[vr,vφ]T为零均值的高斯白噪声,即vk~N(0,Rk),其协方差矩阵为

(40)

式中,diag{·}表示对角矩阵;σr为雷达的测距误差标准差;σφ为雷达的俯仰角测量误差标准差。

4仿真及结果

4.1仿真场景

图2　目标运动轨迹与测量轨迹

设滤波初始方差为P0=diag{[5002, 202, 5002, 202]},滤波初始位置为X0=[232 500,-2 255.2, 88 100,-397.6]T,采样周期为0.5s,仿真时间为100s,并假定各传感器均无虚警及漏检发生,进行200次蒙特卡罗仿真。仿真设备的处理器为AMDAthlon(tm) 64X2DualCoreProcessor4000+,内存为2G。

4.2实验结果

图3给出了基于EKF、SRUKF、CKF与SRCKF 4种算法的序贯式融合估计的位置均方根误差(root mean square error, RMSE)变化曲线,图4是这4种算法的速度均方根误差变化曲线。从图3和图4中可以看出,基于EKF的序贯式融合算法误差最大。这是由于EKF算法采用了一阶泰勒级数展开,丢弃了高阶项,引入了很大的高阶截断误差,降低了估计精度。从图中还可得到基于CKF与SRCKF算法的融合估计精度比SRUKF算法的融合估计精度要高,这是由于SRUKF算法仅捕获非线性高斯系统的二阶矩,而CKF与SRCKF算法使用容积数值积分原则对非线性系统方程的均值和方差进行计算,通过此方法能够达到泰勒展开式的三阶精度,因此具有更高的融合估计精度。而SRCKF算法采用了平方根滤波的思想,确保协方差矩阵的正定性,提高了CKF算法的融合估计精度。因此,采用基于SRCKF的序贯融算法能够提高目标跟踪精度。

图3　位置均方根误差对比

图4　速度均方根误差对比

比较算法性能的另一种指标是平均累加均方根误差(accumulated root mean square error,ARMSE)。下面分别定义位置平均累加均方根误差ARMSEp与速度平均累加均方根误差ARMSEv。

(41)

(42)

表1定量地给出了这4种算法的位置与速度的平均累加均方差误差。从表1可以看出,SRCKF估计精度最高,CKF算法与SRUKF算法次之,EKF算法精度最低。这与图3和图4的结果一致。

表1　位置与速度的ARMSE比较

表2给出了4种算法的200次蒙特卡罗仿真的平均运行时间。从表2可看出,SRUKF、CKF和SRCKF算法的计算复杂度均明显大于EKF算法,而在SRUKF、CKF与SRCKF这3种算法中,SRCKF算法的计算复杂度稍低于另外两种算法。

表2　算法的运行时间对比

5结论

本文将SRCKF算法与多传感器序贯式数据融合算法结合,提出了基于SRCKF算法的多传感器序贯式数据融合算法,并将该算法用于跟踪弹道系数已知的再入段弹道目标。仿真实验就估计精度、计算复杂度对本文算法与基于EKF、SRUKF及CKF的多传感器序贯式融合算法进行了比较,仿真结果证实本文算法的融合精度较高,计算复杂度适中,是一种有效的多传感器融合跟踪算法。

参考文献：

[1] Li X R. Comparison of two measurement fusion methods for Kalman-filter-based multisensor data fusion[J].IEEETrans.onAerospaceandElectronicSystems, 2001, 37(1): 273-280.

[2] Lang H. Centralized and distributed multisensor integration with uncertainties in communication networks[J].IEEETrans.onAerospaceandElectronicSystems, 1991, 27(2): 370-379.

[3] Rao B S, Durrant-whyte H F. Fully decentralized algorithm for multi-sensor Kalman filtering[J].IEEProceedings-DControlTheoryandApplications, 1991, 138(5): 413-420.

[4] Si X H, Li X B, Zhang Y, et al. Multi-sensor fusion target tracking of reentry phase based on square-root unscented Kalman filter[J].SystemsEngineeringandElectronics, 2012, 34(2): 303-306. (司学慧, 李小兵, 张彦, 等. 基于平方根UKF的多传感器融合再入段目标跟踪研究[J].系统工程与电子技术, 2012, 34(2): 303-306.)

[5] Wen C L, Lü B, Ge Q B. A data fusion algorithm based on filtering step by step[J].ACTAElectronicaSinica, 2004, 32(8): 1264-1267. (文成林, 吕冰, 葛泉波. 一种基于分步式滤波的数据融合算法[J].电子学报, 2004, 32(8): 1264-1267.)

[6] Guo W Y, Han C Z, Lian F. Multiple-sensor fusion tracking based on square-root unscented Kalman filter[J].JournalofSystemSimulation,2008,20(12):3237-3240.(郭文艳,韩崇昭,连峰.基于平方根UKF的多传感器融合跟踪[J].系统仿真学报,2008,20(12):3237-3240.)

[7] Pan B, Feng J F, Li Q, et al. Tracking algorithm for MMW/IR multi-sensors fusion[J].JournalofInfraredandMillimeterWaves,2010,29(3):231-235.(潘勃,冯金富,李骞,等.毫米波/红外多传感器融合跟踪算法研究[J].红外与毫米波学报,2010,29(3):231-235.)

[8] Arasaratnam I, Haykin S. Cubature Kalman filters[J].IEEETrans.onAutomaticControl, 2009, 54(6): 1254-1269.

[9] Arasaratnam I, Haykin S. Cubature Kalman smoothers[J].Automatica, 2010, 47(10): 2245-2250.

[10] Sun F, Tang L J. INS/GPS integrated navigation filter algorithm based on cubature Kalman filter[J].ControlandDecision, 2012, 27(7): 1032-1036. (孙枫, 唐李军. 基于cubature Kalman filter的INS/GPS组合导航滤波算法[J].控制与决策, 2012, 27(7): 1032-1036.)

[11] Mu J, Cai Y L. Iterated cubature Kalman filter and its application[J].SystemsEngineeringandElectronics, 2011, 33(7): 1454-1459,1509. (穆静, 蔡远利. 迭代容积卡尔曼滤波算法及其应用[J].系统工程与电子技术, 2011, 33(7): 1454-1459,1509.)

[12] Yuan Z J, Zheng N N, Jia X C. The Gauss-Hermit particle filter[J].ActaElectronicSinca,2003,31(7):970-973.(袁泽剑,郑南宁,贾新春.高斯-厄米特粒子滤波器[J].电子学报,2003,31(7):970-973.)

[13] Lu C G, Feng X X, Zhang D. Pure bearing tracking based on improved cubature Kalman filter[J].SystemsEngineeringandElectronics, 2012, 34(1): 28-33. (鹿传国, 冯新喜, 张迪. 基于改进容积卡尔曼滤波的纯方位目标跟踪[J].系统工程与电子技术, 2012, 34(1): 28-33.)

[14] Ao Z G, Tang C C, Fu C Q, et al. Multi-sensor adaptive cubature Kalman filter data fusion algorithm[J].ApplicationResearchofComputers,2014,31(5):1312-1315,1331.(敖志刚,唐长春, 付成群, 等. 多传感器自适应容积卡尔曼滤波融合算法[J].计算机应用研究, 2014, 31(5): 1312-1315,1331.)

[15] Hu Z T,Cao Z W,Li S,et al.Heterogeneous multi-sensor fusion algorithm based on cubature Kalman filter[J].JournalofOptoelectronics·Laser,2014,25(4):697-703.(胡振涛,曹志伟,李松,等.基于容积卡尔曼滤波的异质多传感器融合算法[J].光电子·激光,2014,25(4):697-703.)

刘华(1980-),男,博士研究生,主要研究方向为非线性滤波、目标跟踪、多源信息融合。

E-mail: peterliuh@126.com

吴文(1968-),男,研究员,博士,主要研究方向为毫米波制导技术和多模复合制导技术。

E-mail: wuwen@njust.edu.cn

王世元(1980-),男,副教授,博士,主要研究方向为非线性信号处理、自适应滤波器设计、数字通信系统及生物信息学。

E-mail: wsy@swu.edu.cn

网络优先出版地址：http://www.cnki.net/kcms/detail/11.2422.TN.20150109.0953.001.html

Multi-sensor sequential fusion tracking algorithm based on

square-root cubature Kalman filter

LIU Hua1, WU Wen1, WANG Shi-yuan2

(1.MinisterialKeyLaboratoryofJGMT,NanjingUniversityofScienceandTechnology,Nanjing210094,China;

2.SchoolofElectronicandInformationEngineering,SouthwestUniversity,Chongqing400715,China)

Abstract:In order to improve the accuracy of the nonlinear sequential fusion algorithm, a new multi-sensor sequential fusion algorithm based on square-root cubature Kalman filter (SRCKF) is proposed. The proposed algorithm uses the third degree spherical-radial cubature rule to calculate the mean and covariance of the nonlinear process, and hence, overcomes the shortcomings of low performance in extended Kalman filter and complex parameters in square-root unscented Kalman filter. Meanwhile, the square-root covariance matrix replaces the covariance matrix in filtering recursion. In this way, the numerical stability of the algorithm is guaranteed and the tracking accuracy is improved. The performance of the proposed algorithm is tested by the reentry trajectory tracking model with known ballistic coefficients. Simulation results show that the proposed algorithm has good tracking performance, and is therefore an effective nonlinear sequential fusion tracking algorithm.

Keywords:multiple sensors; sequential fusion; square-root cubature Kalman filter (SRCKF); reentry phase

作者简介：

中图分类号：TP 911

文献标志码：A

DOI:10.3969/j.issn.1001-506X.2015.07.05

收稿日期：2014-07-25；修回日期：2014-12-03；网络优先出版日期：2015-01-09。