基于弦线法的去导迭代扩展卡尔曼滤波器

李 科，常国宾，李胜全，金际航

(海军海洋测绘研究所，天津 300061)

0 引言

在线性高斯滤波中，卡尔曼滤波(Kalman Filter，KF)可以得到状态量的极大验后估计(Maximum a Posteriori，MAP)。但对于非线性观测方程，需要对方程进行线性化近似才可以应用KF算法，这种近似包括扩展卡尔曼滤波［1］(Extended Kalman Filter，EKF)中的一阶Taylor级数近似和无敏卡尔曼滤波［2］(Unscented Kalman Filter，UKF)中的统计线性化近似［3］。近似方法一般得到的估计值不是MAP估计。之前的应用表明［1，4-9］，在观测信息值得信任时，迭代扩展卡尔曼滤波(Iterated Extended Kalman Filter，IEKF)可以使估计值逐渐向MAP估计收敛。Bell［10］通过构造伪观测量，把MAP估计问题转化为极大似然估计(ML)，指出IEKF中的观测迭代是一种高斯-牛顿迭代过程。然而高斯-牛顿算法是一种解析算法，当观测方程的表达式比较复杂，或者其函数不能用初等函数表达时，求解Jacobian矩阵比较困难［11］，一定程度上限制了IEKF的应用。

1 IEKF是高斯－牛顿迭代过程

高斯-牛顿迭代用于求解非线性最小二乘问题，是牛顿迭代算法的一种近似应用。Bell从高斯-牛顿迭代的角度推导了IEKF的观测迭代公式［10］。

假设各随机量都为高斯分布，KF的观测更新可以表示为一个求极大验后估计的问题。验后概率可表示为

即求状态量的极大验后估计可转化为所构造的伪观测量的极大似然估计。令STS=C-1，f(x)=S(Zg(x))，

由上式可知，卡尔曼滤波中的观测更新过程为一个典型的非线性最小二乘估计问题。应用高斯-牛顿迭代算法［12］可以得到

式中，H为观测方程的Jacobian矩阵。将式(6)代入式(5)并整理，得

至此可以发现，传统的迭代扩展卡尔曼滤波算法的观测迭代过程为一个高斯-牛顿迭代过程。

2 去导迭代扩展卡尔曼滤波器

牛顿迭代法需要求非线性函数的导数，这限制了IEKF的应用。用两点间的割线斜率矩阵取代牛顿迭代法中的Jacobian矩阵，可以得到弦线迭代法。设xn，xn-1是非线性函数h(x)=0的两个近似解，过点(xn，h(xn))和点(xn-1，h(xn-1))做一条直线，则弦线方程为

当x为一维时，H由式(8)唯一确定，即H=(h(xi)-h(xi-1))/(xi-xi-1);当x为多维时，仅由式(8)不能唯一确定H，此时用多维弦线法，即Broyden法［6］求H，得

令L(x)=0的解为xi+1，则

与牛顿法一样，当h(x)在根的某个邻域内有二阶的连续导数，且一阶导数不为0时，弦线法具有局部收敛性，可以证明弦线法具有超越性的收敛速度。

把弦线法的思想引入迭代扩展卡尔曼滤波，可以得到一种去导的迭代扩展卡尔曼滤波算法，其观测迭代公式为

由于每次迭代需要已知之前的两次迭代值作为迭代更新的近似解，而在初次迭代时只能得到预测值这一个近似解，为此，采用有限差分法通过状态估计预测值和方差矩阵预测值构造迭代初始值的方法，则

其中，α为衰减因子，此值根据观测方程的非线性程度、观测噪声和系统噪声的强度进行选择，取0＜α≤1。

3 UNGM实例仿真

通过一个具有代表性的例子来考察本文提出的DF-IEKF以及EKF、IEKF和UKF等滤波算法的滤波性能。单变量非平稳增长模型(Univariate Nonstationary Growth Model，UNGM)在经济领域应用很普遍，由于其高度的非线性以及状态量分布的双模态特性，使得其经常被作为验证非线性滤波算法的基准模型。其离散时间动态系统方程为

其中:系统噪声wk-1～N(0，1);观测噪声vk～N(0，1);仿真时刻K=500;仿真时用于产生仿真数据的初始真值x0=0.1;Monte Carlo仿真次数M取为50。

各种滤波算法的性能分别用两种指标进行表示。一种是每次Monte Carlo仿真的时间平均均方误差(T_MSE)，另一种指标是在每个时间点处的Monte Carlo集总平均均方误差(S_MSE)，其定义为

m和k分别为Monte Carlo仿真次数参数和时间参数。图1和图2分别给出了各种滤波算法得出的上述两种指标曲线图(为便于显示，图2只给出k为200～250之间的估计误差。

图1 各滤波算法时间平均均方误差曲线Fig.1 MSE of each filtering algorithm time

图2 各滤波算法集平均均方误差曲线图Fig.2 MSE of each filtering algorithm set

表1给出了各种滤波算法在时间域和集总域都求平均后的均方误差，其定义为

表1 各滤波算法的MSETable 1 MSE of each filtering algorithm

图1、图2和表1都说明，UKF、IEKF和DF-IEKF的性能都明显优于EKF，IEKF和DF-IEKF而略优于UKF，而IEKF和DF-IEKF的性能没有明显的区别，这也验证了本文的结论，即本文提出的新方法在不损失滤波精度的前提下，应用更方便。

4 结论

本文从IEKF的高斯-牛顿推导过程入手，揭示了其存在的缺陷和观测迭代中引入弦线法的可行性，得到了一种去导迭代扩展卡尔曼滤波算法(Derivative Free Iterated Extended Kalman Filter，DF-IEKF)，并论述了新算法的应用细节，最后把新算法应用于一个非线性实例。仿真实验的结果表明了新算法在应用方面的优势，并验证了相对于传统迭代扩展卡尔曼滤波，新算法并没有损失滤波精度。

［1］CANDY J V.Bayesian signal processing［M］.New Jersey:John Wiley＆Sons，2006.

［2］JULIER SJ，UHLMANN JK.A new extension of the Kalman filter to nonlinear systems［C］//Proc.SPIE-Int.Soc.Opt.Eng.Orlando.1997，3068:182-193.

［3］LEFEBVRE T，BRUYNINCKX，H，DE SCHUTTER J.Comment on“a new method for the nonlinear transformation of means and covariances in filters and estimators”［J］.IEEE Transactions on Automatic Control，2002，47(8):1406-1409.

［4］ZHAN R，WAN J.Iterated unscented Kalman filter for passive target tracking［J］.IEEE Transactions on Aerospace and Electronic Systems，2007，43(3):1155-1163.

［5］刘大鹏，马晓川，朱昀，等.基于混合迭代UKF的捷联惯导对准算法设计［J］.系统仿真学报，2010，22(10):2404-2406.

［6］张俊根，姬红兵.IMM迭代扩展卡尔曼粒子滤波跟踪算法［J］.电子与信息学报，2010，32(5):1116-1120.

［7］程水英，毛云祥.迭代无味卡尔曼滤波器［J］.数据采集与处理，2009，24(s):43-48.

［8］李茜，贾旭东，马寅峰，等.弹箭定姿系统的改进卡尔曼滤波算法研究［J］.电光与控制，2011，18(8):47-50.

［9］李明锁，井亮，邹杰，等.结合扩展卡尔曼滤波的Cam-Shift移动目标跟踪算法［J］.电光与控制，2011，18(4):1-5.

［10］BELL B M，CATHEY F W.The iterated Kalman filter update as a Gauss-Newton method［J］.IEEE Transactions on Automatic Control，1993，38(2):294-297.

［11］MERWE R.Sigma-point Kalman fiters for probabilistic inference in dynamic state-space models［D］.Portland，USA:Oregon Health＆Sci.Univ.，2004.

［12］DENNIS J E，SCHNABEL R B.Numerical methods for unconstrained optimization and nonlinear equations［M］.New Jersey:Prentice-Hall，Englewood Cliffs，1983.