基于迭代积分粒子滤波的目标跟踪算法*

毛少锋,冯新喜,鹿传国,危 璋

(1 空军工程大学信息与导航学院,西安 710077;2 95806部队,北京 100000)

基于迭代积分粒子滤波的目标跟踪算法*

毛少锋1,冯新喜1,鹿传国2,危 璋1

(1 空军工程大学信息与导航学院,西安 710077;2 95806部队,北京 100000)

针对粒子滤波在非线性目标跟踪中存在粒子退化的问题,提出一种迭代积分粒子滤波的目标跟踪算法。该算法从改进重要性函数的角度入手,在积分卡尔曼滤波的基础上,通过高斯牛顿迭代的方法进行量测更新,并对粒子集合中的粒子进行迭代积分卡尔曼滤波,使得构造的重要性函数更加贴近真实后验分布。仿真结果表明,与粒子滤波算法、积分粒子滤波算法相比,该算法在有效改善非线性目标跟踪中粒子退化的同时,提高了跟踪精度。

高斯牛顿迭代;积分卡尔曼滤波;重要性函数;非线性目标跟踪

0 引言

现代战场环境中,通过角度获得的目标数据信息大都是非线性量测,其跟踪实质就是利用角度信息来估计目标的运动状态[1]。而粒子滤波(particle filter,PF)[2]可以较好的解决这一非线性滤波估计问题。

然而粒子退化问题一直是影响粒子滤波精度的一个重要方面。粒子滤波使用先验概率密度作为重要性函数,没有考虑最新的量测信息,使得状态估计存在较大误差[2]。因此,文献[3]采用扩展卡尔曼滤波(extended kalman filter,EKF)产生重要性函数,但是EKF需要进行求导运算,容易引入截断误差;文献[4]采用的是无迹卡尔曼滤波(unscented kalman filter,UKF),UKF需要设计合理的Sigma点,否则可能导致滤波发散;文献[5]利用迭代扩展卡尔曼滤波(iterated extended kalman filter,IEKF),运用高斯-牛顿迭代方法进行线性化时误差较大,精度要求较高时也难以满足需求;文献[6]采用容积卡尔曼滤波(cubature kalman filter,CKF),CKF一般在高维情况下有较好的实时性,维数较低时效果不是很好。

针对以上问题,文中利用高斯牛顿迭代方法[7]对积分卡尔曼滤波(quadrature kalman filter,QKF)[8]的量测进行迭代更新,构造重要性函数,提出一种迭代积分粒子滤波(iterated quadrature particle filter,IQPF)算法。该算法通过高斯牛顿迭代对QKF算法进行迭代处理,使得迭代QKF产生的重要性密度函数更加接近真实后验概率分布。仿真结果表明,该算法有效的提高了目标的跟踪精度。

1 非线性目标跟踪模型

文中以二维无源传感器目标跟踪系统为例。此时,无源传感器只提供角度信息,具体的几何关系如图1所示。

图1 目标与传感器之间的几何关系

对于匀速运动目标,运动模型可以写为:

X(k)=FX(k-1)+W(k-1)

(1)

式中:F为状态转移矩阵;W(k)为过程高斯白噪声,服从N(0,Q),Q为协方差矩阵。

目标跟踪模型的量测方程为:

Z(k)=h(X(k))+V(k)= [α(k)β(k)]T+V(k)

(2)

(3)

(4)

其中：α(k)、β(k)分别为传感器1和2的方位量测值。X=(x(k),y(k))为目标在k时刻的位置坐标,S1=(x1(k),y1(k))为传感器1在k时刻的位置坐标,S2=(x2(k),y2(k))为传感器2在k时刻的位置坐标。V(k)为观测噪声,服从N(0,R),而R为量测噪声协方差矩阵。

从量测方程中可以看出,这是一个典型的非线性问题,需要通过非线性滤波技术来获得优估计,下面给出迭代积分粒子滤波跟踪算法。

2 基于迭代的积分粒子滤波算法

2.1 QKF算法

2007年A.Ienkaran[8]等提出的QKF滤波算法通过使用求积分的原则计算非线性随机变量的均值和协方差,避免了求导运算,并且在低维情况下有较好的滤波效果。同时它除了具有UKF的优点以外,采样点数还可以根据不同的系统要求而改变,具体的算法流程见文献[9]。

2.2 迭代QKF的量测更新

然而QKF算法对模型具有较强的依赖性,且对滤波的初始条件比较敏感,因此每次迭代相比上一步迭代能否更逼近理想值是不确定的。而迭代QKF利用观测更新得到的状态估计,对系统的非线性量测方程进行线性化,这样每次迭代都采用最新的迭代值,可以有效的减少QKF算法线性化过程中带来的误差。

高斯牛顿迭代主要用来求解非线性最小二乘问题。由于QKF属于一种高斯次优滤波算法,其中所包含的随机量符合高斯分布,因此,可使用最大后验估计来计算更新状态。

利用最小均方误差估计准则,可得目标状态的估计值为:

(5)

根据贝叶斯理论,后验概率为:

预测概率为:

(7)

其中,p(Zk|Xk)为似然概率,由量测方程和噪声协方差矩阵确定。Bell指出卡尔曼滤波的量测更新可以表示为一个求极大后验估计的问题。经过上面的分析,也就是求极大似然估计的问题,即极小化的“费用函数”[10],这样就可以将问题转化为求下列函数的极小值:

(8)

用观测量和状态预测这两个相互独立的随机量构造了一个伪观测量:

(9)

则此观测量同样为高斯随机变量,均值和方差分别表示为:

E(Xk)=[h(Xk)Xk]T

(10)

V=diag([RP])

(11)

则式(8)可表示为:

(12)

令STS=V-1,L(Xk)=S[Z-E(Xk)],则有:

(13)

根据上式可以看出,QKF的量测更新则转化为最小二乘问题,按照高斯牛顿方法,对于目标函数JQ(Xk),高斯牛顿迭代可表示为:

(14)

其中i为迭代次数,函数L(Xk)的一阶导数为:

(15)

因此,将式(15)代入式(14),整理可得到迭代QKF的量测更新方程为:

同时可得到迭代QKF的协方差矩阵方程为:

(17)

2.3 IQPF算法

通过对粒子集合中的每个粒子进行迭代QKF滤波,来产生粒子滤波的重要性函数,使得粒子的分布更接近状态的后验概率分布。

IQPF中粒子采样的具体步骤如下:

(18)

(19)

则归一化权值为:

(20)

5)重采样,根据有效粒子数和预设门限来判断是否需要重采样,得到等权值粒子集。

(21)

(22)

3 仿真分析

为了验证算法的有效性,通过两组实验,对IQPF算法和PF算法、QPF算法的性能进行对比分析。

实验1:通过一组典型的一维非线性模型估计问题验证IQPF算法的滤波性能,设系统模型的状态方程和观测方程为:

Xk=1+sin (0.4πk)+0.5Xk-1+wk-1

式中：过程噪声服从Gamma分布,wk～Gamma(3,2),观测噪声服从正态分布,Vk～N(0,0.000 1),初始状态X0=1,粒子数为N=200,蒙特卡罗实验仿真次数为50次,重采样部分采用残差采样法,仿真时间为T=60 s,迭代QKF采用的积分点数为m=3。

图2 系统状态估计

图3 位置均方根误差

表1 各算法均方根误差的均值和方差

图2为各种算法的状态估计曲线,图3为它们的均方根误差曲线图,表1为各算法均方根误差的均值和方差以及平均运行时间。从仿真结果可以看出,PF算法在进行系统状态估计过程中,有时会出现较大的偏离,而IQPF相比PF、QPF能够较好的估计系统状态。此外,从表1可以看出,IQPF的均方根误差较小,有效改善了滤波估计的精度,这主要在于IQPF迭代时选取的是上一次迭代的最新值。

图4 位置均方根误差

图5 速度均方根误差

表2 各算法均方根误差的均值

图4为位置均方根误差曲线,图5它们的速度均方根误差曲线图,表2为各算法均方根误差的均值和方差以及平均运行时间。从仿真曲线以及表2可以看出,PF跟踪目标的误差较大,难以实现目标的准确跟踪,主要在于目标滤波的初始状态与目标真实状态偏离较大,粒子退化现象严重;QPF能够实现目标的稳定跟踪,这主要取决于QKF较优的非线性逼近性能、数值精确度以及滤波稳定性,使得利用QKF重要性函数更加贴近于真实后验;IQPF在QKF的基础上,通过对量测方程进行迭代处理,每次迭代都采用上一步更新得到的最新的迭代值,使得跟踪精度相比QPF有了较好的改善,但运行时间也有了些许增加。

4 结论

文中针对在非线性目标跟踪中的粒子退化问题,提出了一种迭代的积分粒子滤波目标跟踪算法。利用高斯牛顿迭代对QKF量测进行迭代更新,使得改进后的重要性函数更加贴近真实后验分布。仿真实验表明,文中算法有效地解决了粒子退化以及非线性目标跟踪中的非线性估计问题,提高了目标跟踪精度。然而如何将所提算法更好的运用到多目标跟踪中,将是下一步研究的重要方向。

[1] 张洋, 芮国胜, 苗俊, 等. 扩展容积卡尔曼滤波定位技术研究 [J]. 光电工程, 2012, 39(4): 38-43.

[2] 胡士强, 敬中良. 粒子滤波算法综述 [J]. 控制与决策, 2005, 20(4): 361-365.

[3] De Freitas N, Niranjan M, Gee A H, et al. Sequential Monte Carlo methods to train neural network models [J]. Neural Computa-tion, 2000, 12(4): 955-993.

[4] Van der Merwe R, Douce A, de Freitas N, et al. The unscented particle filter [R]. Cambridge, UK: Engineering Department, Cambridge University, 2000: 1-45.

[5] 李良群, 姬红兵, 罗军辉. 迭代扩展卡尔曼粒子滤波器 [J]. 西安电子科技大学学报, 2007, 34(2): 233-238.

[6] 孙枫, 唐李军. Cubature粒子滤波 [J]. 系统工程与电子技术, 2011, 31(11): 2554-2557.

[7] BELL B, CATHEY F. The iterated Kalman filter update as a Gauss-Newton method [J]. IEEE Transactions on Automatic Control, 1993, 38(2): 294-297.

[8] Ienkaran A, Simon H, Robert J E. Discr-etetime nonlinear filtering algorithms using Gauss-Hermite quadrature [J]. Proceedings of the IEEE. 2007, 95(5): 953-977.

[9] 巫春玲, 韩崇昭. 求积分卡尔曼粒子滤波算法 [J]. 西安交通大学学报, 2009, 43(2): 25-28.

[10] Freitas J F G, Niranjan M, Gee A H, et al. Sequential Monte Carlo Methods to train neural network models [J]. Neural Computation, 2000, 12(4): 993-995.

[11] 孔云波, 冯新喜, 鹿传国. 基于改进高斯混合粒子滤波的纯方位目标跟踪算法 [J]. 宇航学报, 2012, 33(7): 971-876.

[12] 郝燕玲, 杨峻巍, 陈亮, 等. 平方根容积卡尔曼滤波器 [J]. 弹箭与制导学报, 2012, 32(2): 169-172.

[13] 鹿传国, 冯新喜, 张迪, 等. 蒙特卡罗马尔可夫链容积粒子滤波器 [J]. 电子科技大学学报, 2012, 41(6): 859-864.

Target Tracking Algorithm Based on Iterative Quadrature Particle Filter

MAO Shaofeng1, FENG Xinxi1, LU Chuanguo2, WEI Zhang1

(1 Information and Navigation College, Air Force Engineering University, Xi’an 710077, China; 2 No.95806 Unit, Beijing 100000, China)

In view of particle degradation problem of nonlinear target tracking, an iterative Quadrature particle filter (IQPF) target tracking algorithm was proposed based on the improved importance function. The algorithm is the basis of the Quadrature Kalman filter (QKF), the Gauss-Newton iteration method is used to make iterative processing, which can take the measurement into account by means of the iterative Quadrature Kalman filter, to make the importance function be closer to the true posterior. The computer simulation results indicate that compared with the particle filter (PF) algorithm and the Quadrature particle filter (QPF) algorithm, this algorithm has better performance in accuracy and effectively solves the particle degradation problem in nonlinear tracking.

Gauss-Newton iteration; Quadrature Kalman filter; importance function; nonlinear tracking problem

2014-03-25

陕西省自然科学基金(2011JM8023)资助

毛少锋(1989-),男,陕西澄城人,硕士研究生,研究方向:目标跟踪。

TN953

A