双层无迹卡尔曼滤波

杨峰 郑丽涛 王家琦 潘泉

状态估计在信号处理、计算机视觉、自动控制、目标跟踪、导航、金融、通信等领域[1−6]有着广泛应用.在高斯噪声环境下,卡尔曼滤波(Kalman fllter,KF)[7]及其次优滤波算法可以很好解决该问题.在非高斯噪声环境下,KF算法及其次优滤波算法不再适用,因此存在着粒子滤波(Particlefilter,PF)[8]及其衍生滤波算法来解决状态估计问题.

基于无迹变换(Unscented transform,UT)的无迹卡尔曼滤波(Unscented Kalman fllter,UKF)[9−11]是一种计算非线性变换均值和协方差的次优卡尔曼滤波算法.相比于扩展卡尔曼滤波(Extended Kalman fllter,EKF),UKF不需要计算雅可比矩阵,且其可以达到非线性函数二阶泰勒展开式的精度[9].因此其在导航制导、目标跟踪、信号处理和图像跟踪等方面有着很广泛应用.但UKF算法也存在着在某些情况下估计效果差等问题.

目前,针对UKF算法估计值不准确的问题,有众多改进方法.为了解决UKF在工程应用中因舍入误差导致数值不稳定的问题,提出了求根UKF(Square-root unscented Kalman fllter,SRUKF)[12]算法.在加性噪声条件下,为了降低UKF算法的计算复杂度,提出了简化UKF(Simplified unscented Kalman fllter,SUKF)[13]算法.在先验信息不确定性大而量测精度高的情况下,只用一次量测值的UKF算法的估计效果较差.因此,提出了多次利用量测值的迭代UKF(Iterated unscented Kalman fllter,IUKF)[14],递归更新滤波器(递归更新扩展卡尔曼滤波(Recursive update extended Kalman fllter,RUEKF)[15]、递归更新容积卡尔曼滤波(Recursive update cubature Kalmanfllter,RUCKF)[16])等算法.基于二阶UT变换的UKF算法滤波估计精度只能达到二阶,为了提高滤波精度,提出了基于高阶UT变换和高阶容积变换(Cubature transform,CT)的高阶UKF[17−18]和高阶容积卡尔曼滤波(Cubature Kalman fllter,CKF)[19−21]等算法.

UKF及其改进算法虽然可以较好处理UKF算法的估计不准确的问题,但其仍然存在在非线性程度高的环境下估计效果差等问题,文献[22−23]中提出将UKF算法作为PF算法建议分布,将UKF算法估计值作为重要性密度函数,这就是无迹粒子滤波(Unscented particle fllter,UPF)[22−23]算法.从理论上讲,随着随机采样粒子数量提高,UPF算法的精度可以逐渐提高.但UPF算法也存在一些问题,如其运算时间很长,时效性较差.且UPF算法效果不总是好于UKF算法,在量测噪声较大时,UPF算法估计精度会不如UKF算法.

为了在低计算负载的情况下获得高的滤波估计精度,本文提出了双层无迹卡尔曼滤波器(Double layer unscented Kalman filter,DLUKF)算法.其核心思想是用带有权值的采样点表示前一个时刻的后验密度函数;而后用内层的UKF算法对每个带权值的采样点进行更新,并用最新的量测值对采样点的权值进行更新;然后将各个采样点进行加权融合,得到了初始的估计值;最后用外层UKF算法的更新机制对初始估计值进行更新得到最终的估计值.

1 无迹卡尔曼滤波(UKF)算法

1.1 UT变换

假设非线性函数为y=f(x),UT变换是通过近似非线性函数的概率密度分布来近似非线性函数.其在得到先验均值和协方差Pxx的基础上,用采样策略选取一组确定性采样点集.而后得到这些采样点集经非线性变换后的采样点集,进而求得经非线性变换后的均值和协方差Pyy.

UT变换算法可以归纳为以下三步:

1.2 UKF算法

考虑典型的非线性系统,其状态方程和量测方程分别为:

xk为k时刻n维的状态向量,zk+1为k+1时刻的量测向量.wk为m维的过程噪声,其服从均值为0方差为Q的高斯分布.vk+1为q维的量测噪声,其服从均值为0方差为R的高斯分布.滤波算法的目的就是从带有噪声的量测值zk+1中估计出真实值xk+1.

在实际应用中,受初始误差的影响,UKF算法存在着收敛速度慢,精度不高等问题.基于此,文献[14]提出了IUKF算法,文献[15]提出了RUEKF算法,文献[16]出了RUCKF算法.这三种算法的核心思想都是多次利用量测值对估计值进行更形,以获得更好的滤波估计效果.

2 无迹粒子滤波(UPF)算法

UPF[22−23]算法是在PF算法的基础上,用UKF算法的滤波估计值作为PF算法的建议密度函数.这虽然可以解决UKF算法不适用于非高斯环境等问题,但其由于要选取大量的随机性采样点来逼近密度函数,所以UPF算法会临着计算量大的问题.UPF算法具体步骤如下:

4)当粒子退化严重时,对粒子进行重采样.

5)计算每个粒子点x(i)对应的协方差.

6)重复步骤2)∼5).

3 双层无迹卡尔曼滤波(DLUKF)算法

UPF需要用大量的粒子点去逼近状态的后验密度函数,因此其有着运算量大的问题.本文所提的DLUKF算法用带权值的采样点去表征状态的后验密度函数,其核心思想为用内层的UKF对每个带权值的采样点进行更新,而后用最新的量测值对每个采样点的权值进行更新,并对更新后的采样点进行加权求和得到下一时刻初始估计值,然后将该初始估计值作为预测值运行外层UKF算法,从而得到最终估计值.

3.1 DLUKF算法框架

DLUKF算法由外层UKF算法和内层UKF算法组成,其算法流程如下:

内层UKF算法:

量测更新:

在采样点用内层UKF算法更新后,类似于UPF算法,表示一阶矩的权值和表示二阶矩的权值的更新可以表示为:

在得到权值更新的基础上,对权值进行归一化处理,有

k+1时刻的初始估计值及其协方差可以表示为

外层UKF算法:

不断重复方程(5)∼(25),即可求得DLUKF算法在每个时刻的估计值.

DLUKF算法的流程图如图1所示.

3.2 基于对称采样策略的DLUKF算法

根据选取粒子点的采样策略不同,又可以得到多种DLUKF算法.在UT变换中,目前的采样策略方法包括对称采样、单形采样、3阶矩偏度采样和高斯分布4阶矩对称采样[8]等.还有为了保证经过非线性变换后协方差Pyy的正定性而提出的对基本采样策略进行比例修正的算法框架.

下面主要详细介绍对称采样策略.

图1 DLUKF算法流程图Fig.1 The flow-chart of DLUKF

4 仿真分析

将本文所提的基于对称采样策略DLUKF算法与UKF算法、IUKF算法、RUEKF算法、RUCKF算法、高阶UKF算法、高阶UKF算法和UPF算法分别在一维和二维仿真场景下进行仿真对比分析,用滤波算法估计值与真实值间的均方根误差(Root mean square error,RMSE)来表示滤波算法估计效果.

4.1 单维仿真

假设有下述状态空间模型,其状态方程和量测方程分别可以表示为:

式(28)中wk表示过程噪声,其服从Ga(3,2)的伽马分布.式(29)中的vk+1表示量测噪声,其服从均值为0,方差为R=10−5的高斯分布.初始位置为x0=3,IUKF算法,RUEKF算法和RUCKF算法的迭代次数都为10次.UPF算法粒子数量为100个,DLUKF算法产生粒子的方法是对称采样策略.仿真时间为30s,蒙特卡洛仿真次数为100次.其仿真结果如图2所示.

通过图2可以看出,IUKF算法、RUEKF算法、RUCKF算法、高阶UKF算法、高阶UKF算法和UPF算法滤波估计效果都略好于UKF算法.这是因为IUKF算法、RUEKF算法、RUCKF算法、高阶UKF算法、高阶UKF算法和UPF算法都对UKF算法进行了改进,所以其效果是好于UKF算法的.本文所提的DLUKF算法在每个时刻的估计效果都好于其他的滤波算法.这说明,DLUKF算法对于UKF算法的改进效果比其他经典算法更加显著.且因为DLUKF算法用两层UKF算法对状态进行估计,所以可以有着很好的滤波估计效果.

图2 300次蒙特卡洛仿真的RMSEFig.2 The calculation time and RMSE of each algorithm

将UPF算法的粒子数由100逐渐增加到500,其与UKF算法、IUKF算法、RUEKF算法、RUCKF算法、高阶UKF算法、高阶UKF算法和DLUKF算法的单次运行时间以及平均RMSE如表1所示.

表1 各算法计算时间及RMSE对比分析表Table 1 The calculation time and RMSE of each algorithm

通过表1可以看出,UKF算法、RUEKF算法、高阶UKF算法和高阶CKF算法的用时都很少.IUKF算法、RUCKF和DLUKF算法的用时略长,这是由于这三种算法都进行了多次滤波计算.UPF算法用时最长.在UPF算法中,随着粒子数目的增多,用时也是逐渐增加.在RMSE方面,DLUKF算法比另外7种方法小很多.在UPF算法中,随着粒子数增多,RMSE也是逐渐变小的.但当500个粒子点时,UPF算法RMSE依然是DLUKF算法的两倍.这说明了基于带权值的采样点表征后验分布的方法是优于随机点表征后验分布的.

4.2 多维仿真

考虑一个二维匀速直线运动的例子,其状态方程和量测方程分别为:

式(31)中,Zk+1=[rk+1,θk+1]T为观测变量,分别表示对目标的径向距和方位角.vk+1为量测噪声,其为闪烁噪声,可以表示为:

量测方程h(·)可以表示为:

式(34)中,R1和R2分别可以表示为

仿真中,仿真时间为100s,蒙特卡洛仿真次数为300次.目标初始位置为(20000m,40000m),初始速度为(−160m/s,−150m/s).IUKF 算法、RUEKF 算法和RUCKF算法的迭代次数都为10次.UPF算法粒子数量为300个,DLUKF算法产生粒子的方法是对称采样策略.

其他的参数设置为:

表2 仿真参数设置Table 2 The Simulation parameters

图3是各个算法在位置方面的RMSE,可以看出,RUEKF算法、RUCKF算法、高阶UKF算法、高阶CKF算法和UKF算法的估计效果基本相同,而IUKF算法和UPF算法的估计效果优于UKF算法.而本文算法性能是最好的,这是因为本文算法用带权值的采样点表征后验分布,这比随机的粒子点表征后验分布更有优势,故DLUKF算法的RMSE是好于其他滤波算法的.这也说明所提的DLIKF算法对匀速直线运动可以有着很好的滤波估计效果.

图3 位置的RMSEFig.3 The RMSE of position

在匀速直线运动中,将UPF算法的粒子数由300逐渐增加到1000,其与其他算法的单次运行时间以及位置和速度的平均RMSE如表3所示.

表3 各个算法的性能Table 3 The performance of each algorithm

从表3可以看出,本文算法的运算时间虽然略长于UKF算法、IUKF算法、REUKF算法、RUCKF算法,高阶UKF算法和高阶CKF算法,却远远小于UPF算法.且由于DLUKF算法的外层UKF算法选取了9个确定性采样点,所以其运算时间是大约是UKF算法的9倍.在UPF算法中,随着粒子数目的增多,运算时间也是逐渐增加的.在各个方面的RMSE、DLUKF算法都是最好的.在UPF算法中,随着粒子数目的增多,RMSE也是逐渐减小的,但比起DLUKF算法、UPF算法的RMSE依然是很大的.这说明基于双层采样的的DLUKF算法在多维目标跟踪中有着很好的滤波估计效果.

5 结论

本文所提的DLUKF算法是在双层UKF算法的基础上,用采样策略选取带权值的采样点,而后用内层UKF算法对每个采样点进行更新,同时用最新的量测对采样点的权值进行更新,最后通过外层UKF算法的更新机制得到每个时刻的滤波估计值.仿真结果表明,在一维和二维的仿真场景中,相比于存在的经典算法,本文所提的DLUKF算法可以在较短的时间内获得很好的滤波估计效果.