快速强跟踪UKF算法及其在机动目标跟踪中的应用

鲍水达, 张 安, 毕文豪

(1. 西北工业大学电子信息学院, 陕西 西安 710129;2. 西北工业大学航空学院, 陕西 西安 710072)

0 引 言

过去几十年间,卡尔曼滤波(Kalman filter, KF)作为一种经典高效的滤波方法,广泛应用于信号处理、目标跟踪、故障检测、捷联导航等领域。传统的KF只能处理线性时不变系统,无法处理非线性系统。为此,学者们基于KF提出扩展卡尔曼滤波(extended Kalman filter, EKF)、无迹卡尔曼滤波(unscented Kalman filter, UKF)等新的非线性滤波算法。EKF通过Taylor展开,将非线性问题局部线性化,从而解决非线性滤波问题。但EKF对非线性函数近似精度为一阶,滤波精度方面并不如人意,且要计算雅可比矩阵,一定程度上限制它的应用。UKF基于无迹变换(unscented transform, UT),采用确定性采样方法近似非线性系统的后验概率密度函数[1]。UKF不用求解雅可比矩阵,对非线性系统状态转移函数和量测函数没有可导性要求,无需知道函数的具体表达形式,也可对黑盒系统进行处理;UKF至少能够达到三阶精度,明显优于EKF,而计算量同阶,获得更多的关注和更广泛的应用[2-5]。

无论是KF、EKF或UKF,它们都有一个共同的基本假设:系统模型是准确的。当系统模型不能正确描述真实系统时,将导致滤波精度下降,甚至滤波发散。为此,学者们提出了各式各样的自适应算法,用于提高KF及相关算法的鲁棒性,保证滤波精度,降低对系统模型准确性的要求[6-13]。自适应滤波算法可分为贝叶斯方法、最大似然函数、相关性和协方差匹配四大类[9]。前两种方法多应用于多模型系统,但计算量很大的不足阻碍了它们的实际应用[11-12]。相关性方法,是将一组方程将函数与未知参数关联起来,主要用于时变系统[13]。协方差匹配方法通过比较理论协方差与实际协方差之间的差异来判断滤波优劣,进而对滤波进行在线自适应调整,是一种广泛应用的自适应方法[6-7]。文献[6]基于状态向量估计残差协方差匹配,提出一种自适应渐消UKF(adaptive fading UKF, AFUKF),来克服模型不确定性,然而,与UKF相比,计算量增加较多,且要额外计算雅各比矩阵。文献[7]提出的强跟踪算法(strong tracking filter, STF)是一种基于新息协方差匹配的自适应滤波方法。STF通过引入次优渐消因子,调整预测状态协方差,进而在线调整滤波增益K,达到自适应的目的。将STF引入UKF,建立强跟踪UKF,能极大提高UKF鲁棒性,降低对系统模型依赖[14-18]。

文献[14]推导了渐消因子引入UKF的过程,并给出了在UKF形式下,次优渐消因子的等价计算方法。强跟踪UKF以其良好的滤波性能及鲁棒性,已经广泛应用在诸多领域中。文献[15]采用一种改良的平方根分解方法,提出改进的强跟踪平方根UKF,克服卫星自主导航中不确定性干扰和噪声的影响。文献[16]应用假设检验理论提出改进的强跟踪UKF,并实际应用于无人机动态导航。文献[17]提出强跟踪平方根无迹 Kalman 神经网络来提高能耗模型的精度和对电解槽突变状态的实时跟踪能力。文献[18]提出改进的强跟踪UKF算法,用于捷联惯性导航系统(strap-down inertial navigation system, SINS)大方位失准角初始对准中。然而,强跟踪UKF的UT变换变为3次,极大地增加算法计算量,限制它在一些工程中的应用,如板载小型卫星[6]。基于以上分析,本文采用Taylor展开分析渐消因子机理,建立渐消因子近似引入方法,减少UT变换次数,提出快速强跟踪UKF,并定性分析快速强跟踪UKF计算量增长。

1 强跟踪UKF算法

1.1 强跟踪UKF

考虑非线性离散系统

xk=f(xk-1)+wk-1

(1)

zk=h(xk)+vk

(2)

式中,xk∈Rn为系统状态向量;zk∈Rm为量测向量;n和m为对应维数;f(·)和h(·)分别为非线性系统的状态转移函数和量测函数;wk-1∈Rn为系统噪声,vk∈Rm为量测噪声,二者均为高斯白噪声,且互不相关,协方差矩阵为Qk和Rk。

强跟踪UKF将渐消因子引入UKF,兼具有STF鲁棒性强和UKF变换精度高,有效克服了UKF在系统模型不准确时鲁棒性差的问题。针对式(1)和式(2)描述的非线性系统,强跟踪UKF由4个步骤组成[12]。

步骤1选择采样策略

本文采用对称采样策略

(3)

式中,κ为可设参数;i=1,2,…,n。对应权值为

(4)

步骤2时间更新

χj=f(Xj)

(5)

(6)

(7)

式中,上标s表示λk引入前的变量。

步骤3计算渐消因子λk

(8)

(9)

(10)

(11)

λk计算公式为

(12)

(13)

新的预测状态协方差Pk/k-1为

(14)

步骤4量测更新

(15)

(16)

(17)

强跟踪UKF在时间更新、计算渐消因子λk和量测更新3个步骤共进行3次UT变换,比传统UKF多使用一次UT,额外使用UT变换会极大地增加算法计算量。可见,现有的引入渐消因子方法是以增加算法计算量为代价,来克服UKF在系统模型不准确时鲁棒性差的不足,仍需进一步完善改进。

1.2 渐消因子引入

STF通过引入次优渐消因子,调整状态协方差,进而在线调整增益K,迫使残差正交,即

(18)

式中,Pk-1是k-1时刻的状态协方差;λk是k时刻的渐消因子;上标s表示渐消因子引入前的变量。此时,预测状态协方差Pk/k-1为

(19)

式中,Fk为雅可比矩阵。式(19)为强跟踪UKF采用的渐消因子引入方法。

另一种渐消因子引入方法[18]为

(20)

对于EKF而言,采用式(19)或者式(20)不会产生显著的差异,因为EKF本身要使用雅可比矩阵F和H。但对于UKF等不用计算雅可比矩阵的滤波方法,基于式(20)的渐消因子引入方法,不用额外计算雅可比矩阵,故式(20)比式(19)更加合适。本文的分析和推导将基于式(20)。

根据式(20),渐消因子引入UKF后为

(21)

对应次优渐消因子计算方法[17]为

(22)

式中,Vk的估算公式为式(13);β为可调参数,β≥1。

2 快速强跟踪UKF算法

2.1 渐消因子近似引入方法

根据式(21),本文提出一种适用于UKF的渐消因子近似引入方法,即

(23)

(24)

(25)

(26)

在近似方法中,渐消因子直接作用于相关变量上,不再间接地通过UT变换引入UKF, UT变换使用次数将降低为2次。

渐消因子近似引入方法推导如下。

(27)

(28)

(29)

(30)

更新后的状态协方差Pk为

(31)

(32)

将式(20)和式(32)代入式(28),有

(33)

(34)

(35)

(36)

将式(36)代入式(10),得

(37)

(38)

将式(21)代入式(31)得

(39)

推导完毕。

2.2 算法流程

根据传统UKF算法流程以及式(1)和式(2)表示的非线性系统,基于近似引入方法的快速强跟踪UKF算法流程如下:

步骤1选择采样策略

本文采用对称采样策略,如式(3)和式(4)。

步骤2时间更新

步骤3计算渐消因子λk

根据式(22)计算得k时刻λk,并代入式(21)求得更新后的Pk/k-1。

步骤4量测更新

与强跟踪UKF相比,快速强跟踪UKF仅需要两次UT变换,在步骤3中直接套用求解公式即可求解得λk,过程简洁,基本与传统UKF一致。

3 计算量分析和收敛性分析

3.1 计算量分析

本节将利用统计浮点运算次数(flops)的方法,分析传统UKF、强跟踪UKF与快速强跟踪UKF的计算量,说明两强跟踪UKF相对于传统UKF在不同情况下的计算量增长情况,假设采样策略为对称采样,矩阵平方根算法为Cholesky分解。

强跟踪UKF和传统UKF均为O(n3)的算法,以最高幂次的系数(即n3的系数)为对比标准,粗略分析得计算量变化。一次flops定义为两个浮点数进行一次加、减、乘、除运算。UKF中计算量为n3的矩阵操作如表1所示[19]。对于矩阵乘法为A·B,A∈RN×M,B∈RM×L。

表1 各矩阵操作的计算量

UKF的计算量主要由n、m、f(·)和h(·)决定。先忽略f(·)和h(·)计算量的影响,讨论仅考虑n、m的影响。经统计,3种算法的计算量如表2所示。快速强跟踪UKF并没额外增加计算量为n3的操作,故不考虑f(·)和h(·)计算量时,其计算量与传统UKF相当,且不受采样策略与平方根求解算法影响。强跟踪UKF由于要多使用一次UT变换,且在渐消因子计算中有矩阵乘法,求逆等操作,计算量明显高于传统UKF。

表2 各算法的计算量

在快速强跟踪UKF中,仅在式(23)中多调用了1次h(·)。而强跟踪UKF中,额外使用UT变换导致h(·)被多调用2n次。对于f(·)的调用,3种算法并没有区别。

综上,快速强跟踪UKF的计算量增长明显低于强跟踪UKF,与传统UKF相近,且随着状态向量维数和h(·)计算量的增加,快速强跟踪UKF的计算量会越来越接近传统UKF。相反,如果f(·)的计算量变大,将会削弱快速强跟踪UKF在计算量增长方面的优势。

3.2 收敛性分析

本节采用新息序列的特性[20],分析快速强跟踪UKF的收敛性。采用的滤波收敛性判据为

(40)

式中,r为储备系数,r≥1。r=1为最严格的收敛性判据,当r足够大时,即可判定滤波发散。式(40)右侧是理论新息协方差,左侧是k时刻的实际新息协方差,在快速强跟踪UKF中即为Vk,对式(22)进行变换,有

β·tr(Rk)

(41)

(1) 渐消因子λk=1

当渐消因子λk=1,快速强跟踪UKF退化为标准UKF,保留UKF的优良性能。根据式(22),Ck≤1,代入式(41),有

β·tr(Rk)

(42)

快速强跟踪UFK中理论新息协方差为

(43)

对式(43)进行矩阵求迹,有

r·tr(Pzz,k)=

(44)

β·tr(Rk)≤r0·tr(Pzz,k)

(45)

(2) 渐消因子λk>1

λk>1表示快速强跟踪UKF检测到滤波错误,需要进行修正。实际新息协方差为

β·tr(Rk)

(46)

修正后,快速强跟踪UFK中新的理论新息协方差为

(47)

与式(1)中分析类似,必然存在一个略大于1的r0,使得以下不等式成立,满足收敛性判据:

β·tr(Rk)≤r0·tr(Pzz,k)

(48)

综上,无论λk取何值,快速强跟踪UKF均满足收敛性判据,保持滤波稳定性。

4 仿真实例

4.1 一维仿真实例

仿真实例采用强非线性模型[21](univariate nonstationary growth model,UNGM)

(49)

式中,α、β、μ是可设参数;cos(1.2(n-1))可视为时变噪声;c1和c2为调整参数,通过调整c1和c2来模拟系统模型相对于真实系统的失调程度。初始条件为x0=10,P0=10,Q=0.01,R=1,α=0.5,β=2.5,ν=8。

选择传统UKF,强跟踪UKF、文献[6]中的AFUKF和快速强跟踪UKF来对比验证本文算法鲁棒性、滤波性能、计算量。

首先,取c1=c2=1,即系统模型与真实模型一致,4种算法的滤波结果如图1所示。在模型准确时,新息理论协方差与新息实际协方差基本一致,渐消因子始终为1,强跟踪UKF理论上会退化为传统UKF算法。从图1可得,4种算法滤波结果基本一致,3种自适应UKF均正确地退化为传统UKF,与传统UKF滤波结果一致。

图1 模型准确时4种算法滤波结果Fig.1 Results of four algorithms in accurate model

算法运行时间可作为判断算法计算量的指标。在CPU为Intel i3 550,内存为2 GB的笔记本上,仿真时间N=60,蒙特卡罗仿真M=100,4种算法在Matlab 2014a上的运行时间和相对传统UKF增长的如表3所示。快速强跟踪UKF避免额外使用UT变换,运行时间增长明显小于强跟踪UKF,降低了约64.26%,计算量增长远小于强跟踪UKF,但需额外计算λk,计算量变大,故运行时间大于传统UKF,在本例中增长了约20.46%。AFUKF采用假设检验来判断是否进行修正,本例中不存在模型不确定性,故运行时间仅增长21.53%。

表3 一维模型时各算法运行时间

然后,对比4种算法在模型不准确时的性能差异。在真实模型中,取c1=0.6,c2=0.75;在系统模型中,仍然取c1=c2=1,即相比真实模型,系统模型在建模过程中α和γ取值偏大。图2为4种算法的滤波结果,图3为4种算法滤波误差,表4为均方根误差(root mean square error, RMSE),表5为对应运行时间。

表4 一维模型不准确时各算法额运行时间

图2 模型不准确时4种算法滤波结果Fig.2 Result of four algorithms in inaccurate model

对比4种算法的滤波结果,3种自适应UKF的性能明显优于传统UKF。AFUKF的性能介于传统UKF和强跟踪UKF之间。渐消因子提高UKF的鲁棒性,增强UKF适应模型失调的能力,使得2种强跟踪UKF在模型失调时仍能给出较好的滤波结果。由图3可知,传统UKF中大部分误差大的点,均被2种强跟踪UKF修正。快速强跟踪UKF作为强跟踪UKF的近似方法,性能差异极小,RMSE仅相差0.060 3。模型不确定性使得AFUKF进行修正,导致运行时间大大增加,与强跟踪UKF相近,比快速强跟踪UKF多增长了约51.28%。

图3 模型不准确时4种算法滤波误差Fig.3 Filtering error of four algorithms in inaccurate model

算法时间/s增长传统UKF1.4511强跟踪UKF2.4971.720 9AFUKF2.4341.677 5快速强跟踪UKF1.7191.184 7

4.2 机动目标跟踪

以一个典型的机动目标跟踪问题来验证算法的有效性。一个地面雷达连续瞄准和跟踪一个飞行器,飞行器以未知的转速下在水平面中执行机动转弯。飞行器在水平面中机动转弯的运动学模型[22]为

xk-1+wk-1

(50)

(51)

式中,T是雷达的采样间隔,T=1;φ1和φ2为调整参数,且φ1=1 m2/s3,φ2=1.75×10-3rad2/s3。

雷达安置在原点(0,0)处,获取与飞行器的距离σ和方位角θ

(52)

式中,arctan是求反正切函数;vk是相互独立的高斯白噪声,且均值为0,协方差为

R=diag[1 000 m2100 m·rad2]

(53)

初始条件为

x0=[1 000 m 300 m/s 1 000 m 0 m/s -3°/s]T

P0=

diag[100 m210 m2/s2100 m210 m2/s2100 mrad2/s2]T

(54)

k=15时,模拟飞行器发生位置突变，突变量Δxk,Δyk为

(55)

图4 状态突变时和Ωk的RMSEFig.4 RMSE of and Ωk in mutation of state

由图4可知,突变发生前,4种算法滤波结果基本一致,说明当模型准确时,2种强跟踪UKF算法均能正确地退化为UKF算法,保持了UKF算法的优良性能。突变发生时,4种算法在该时刻均无法处理该情况,滤波结果不理想。突变发生后,3种自适应UKF在几个时刻内迅速收敛,滤波误差下降到正常范围,不再受突变点影响。传统UKF则不然,突变点的影响持续了将近20个时刻,滤波误差较大。图5与以上分析相符,突变后,传统UKF的轨迹开始在参考值附近来回波动一段时间才慢慢稳定到参考值附近。渐消因子的引入增强了UKF应对状态突变的能力。AFUKF也具备一定应对状态突变能力,但稳定性明显不如两种强跟踪UKF。

图5 状态突变时4种算法的滤波结果Fig.5 Results of four algorithms in mutation of state

4种算法的运行时间如表6所示,运行计算机配置与仿真实例1相同。在该实例中,快速强跟踪UKF的运行时间已经非常接近传统UKF。这也验证了第3.1节计算量分析的结果,随着状态向量维数的增加,快速强跟踪UKF和传统UKF的计算量会越来越接近。

表6 4种算法的运行时间

5 结 论

渐消因子引入UKF,能有效提高UKF鲁棒性,增强适应模型失调、状态突变的能力,但要求额外使用UT变换,导致计算量急剧提高。本文利用Taylor展开,分析渐消因子在UKF中的作用机理。在UKF中渐消因子不仅在线调整滤波增益,还调整量测预测值,通过同时调整两者来达到充分提取新息序列中信息的目的。然后,本文建立一种适用于UKF的渐消因子近似引入方法,将渐消因子直接作用于相关的协方差和量测预测值上,提出快速强跟踪UKF。将快速强跟踪UKF用于机动目标跟踪问题进行仿真,表明快速强跟踪UKF能有效克服状态突变,保持滤波的稳定性,增强了UKF的鲁棒性,性能与强跟踪UKF相差无几。同时,快速强跟踪UKF计算量与UKF相近,远低于强跟踪UKF。

参考文献：

[1] JULIER S J, UHLMANN J K, DURRANT-WHYTE H F. A new approach for filtering nonlinear systems[C]∥Proc.of the American Control Conference, 1995: 1628-1632.

[2] JUNG K, KIM J, KIM J, et al. Positioning accuracy improvement of laser navigation using UKF and FIS[J]. Robotics and Autonomous Systems, 2014, 62(9): 1241-1247.

[3] SHAN B N, WANG J, DENG B, et al. UKF-based closed loop iterative learning control of epileptiform wave in a neural mass model[J]. Cognitive Neurodynamics, 2015, 9(1): 31-40.

[4] 许杰,戚大伟.基于改进加权质心和UKF的移动目标定位算法[J].吉林大学学报(工学版),2016,46(4):1354-1359.

XU J, QI D W. Moving target localization based on improved weighted centroid and UKF algorithm[J]. Journal of Jilin University (Engineering and Technology Edition), 2016, 46(4):1354-1359.

[5] HU G G, GAO S S, ZHONG Y M. A derivative UKF for tightly coupled INS/GPS integrated navigation[J]. ISA Transactions, 2015, 56: 135-144.

[6] SOKEN H E, HAJIYEV C. Adaptive fading UKF with Q-adaptation: application to picosatellite attitude estimation[J]. Journal of Aerospace Engineering, 2011, 26(3): 628-636.

[7] 周东华,席裕康.非线性系统带次优渐消因子的扩展卡尔曼滤波[J].控制与决策,1990,5(5): 1-6.

ZHOU D H, XI Y K. Suboptimal fading extended Kalman filtering for nonlinear systems[J].Control and Decision,1990,5(5): 1-6.

[8] ISHIHARA S. Robust Kalman filtering for nonlinear systems with parameter uncertainties[C]∥Proc.of the SICE Annual Conference, 2014: 1986-1991.

[9] HAJIYEV C, SOKEN H E. Robust adaptive unscented Kalman filter for attitude estimation of pico satellites[J]. International Journal of Adaptive Control and Signal Processing,2014,28(2): 107-120.

[10] LI W L, SUN S H, JIA Y M, et al. Robust unscented Kalman filter with adaptation of process and measurement noise covariances[J]. Digital Signal Processing, 2016, 48(C): 93-103.

[11] GAO B B, GAO S S, GAO L, et al. An adaptive UKF for nonlinear state estimation via maximum likelihood principle[C]∥Proc.of the 6th International Conference on Electronics Information and Emergency Communication, 2016: 117-120.

[12] MOCHNAC J, MARCHEVSKY S, KOCAN P. Bayesian filtering techniques: Kalman and extended Kalman filter basics[C]∥Proc.of the 19th International Conference Radioelektronika, 2009: 119-122.

[13] GROTHE O. A higher order correlation unscented Kalman filter[J].Applied Mathematics and Computation,2013,219(17):9033-9042.

[14] 王小旭,赵琳,夏全喜.基于Unscented变换的强跟踪滤波器[J].控制与决策,2010,25(7):1063-1068.

WANG X X, ZHAO L, XIA Q X. Strong tracking filter based on unscented transformation[J]. Control and Decision, 2010, 25(7): 1063-1068.

[15] 李敏,王松艳,张迎春,等.改进的强跟踪平方根UKF在卫星导航中应用[J].系统工程与电子技术,2015,37(8):1858-1865.

LI M, WANG S Y, ZHANG Y C, et al. Satellite autonomous navigation filtering algorithm based on improved strong tracking square-root UKF[J]. Systems Engineering and Electronics, 2015, 37(8): 1858-1865.

[16] HU G G, GAO S S, ZHONG Y M, et al. Modified strong tracking unscented Kalman filter for nonlinear state estimation with process model uncertainty[J]. International Journal of Adaptive Control and Signal Processing, 2015, 29(12): 1561-1577.

[17] 李太福,姚立忠,易军,等.强跟踪平方根UKFNN的铝电解槽工耗动态演化模型[J].自动化学报,2014,40(3): 522-530.

LI T F, YAO L Z, YI J, et al. An improved UKFNN based on square root filter and strong tracking filter for dynamic evolutionary modeling of aluminum reduction cell[J]. Acta Automatica Sinica, 2014, 40(3): 522-530.

[18] 郭泽,缪玲娟,赵洪松.一种改进的强跟踪UKF算法及其在SINS大方位失准角初始对准中的应用[J].航空学报,2014,35(1): 203-214.

GUO Z, MIAO L J, ZHAO H S. An improved strong tracking UKF algorithm and its application in sins initial alignment under large azimuth misalignment angles[J]. Acta Aeronautica et Astronautica Sinica, 2014, 35(1): 203-214.

[19] GREWAL M S, ANDREWS A P. Kalman filtering: theory and practice using matlab[M]. 3rd ed. New Jersey: Wiley, 2008.

[20] 徐树生,林孝工,赵大威,等.强跟踪SRCKF及其在船舶动力定位中的应用[J].仪器仪表学报,2013,34(6): 1266-1272.

XU S S, LIN X G, ZHAO D W, et al. Strong tracking SRCKF and its application in vessel dynamic positioning[J]. Chinese Journal of Scientific Instrument, 2013, 34(6): 1266-1272.

[21] KOTECHA J H, DJURIC P M. Gaussian particle filtering[J]. IEEE Trans.on Signal Processing,2003,51(10):2592-2601.

[22] ZHAO L Q, WANG J L, YU T, et al. Design of adaptive robust square-root cubature Kalman filter with noise statistic estimator[J].Applied Mathematics and Computation,2015, 256(C): 352-367.