改进的迭代卡尔曼滤波及其在全球导航星系统/惯导紧组合中的应用

化雪荟,陈大力

(佛山职业技术学院,广东 佛山 528000)

改进的迭代卡尔曼滤波及其在全球导航星系统/惯导紧组合中的应用

化雪荟*,陈大力

(佛山职业技术学院,广东 佛山 528000)

为改善GNSS/INS(Global Navigation Satellite System/Inertial Navigation System)紧组合中非线性滤波器的稳定性和精度,基于Backtracking线搜索(BLS)方法提出一种新型的迭代容积卡尔曼滤波方法(BLS-ICKF)。首先建立Sigma点滤波方法的迭代框架,采用阻尼Newton迭代改善状态初始误差较大时的滤波更新精度,同时,为提高组合系统状态预测过程的鲁棒性定义了一种简化的迭代更新结构。实验和仿真结果表明,BLS-ICKF牺牲较小的时间复杂度较CKF的航向角误差减少约59%。

组合导航;迭代滤波框架;容积卡尔曼滤波;Backtracking线搜索

以GPS为代表的GNSS受短时信号衰减和干扰影响较大,而惯性器件的噪声和漂移导致惯性导航系统(INS)误差随运行时间积累,因此两者误差具有互补特性。近年来,融合两者信息的组合导航系统获得了广泛的应用,由于INS状态误差方程和GNSS伪距、伪距率观测方程具有非线性特征,两者的信息融合属于典型非线性滤波问题。扩展卡尔曼滤波器(EKF)是一种广泛采用的非线性滤波方法,其采用线性截断逼近滤波过程中的非线性,当系统中存在较强的非线性时易发散,且EKF实现过程需要计算雅克比矩阵。以无迹变换(UT)为代表的采样逼近方法获得了广泛的关注,Julier[1]将UT引入卡尔曼滤波(KF)过程提出了 UKF,分析结果表明其较EKF具有更高的非线性逼近精度,且无需计算雅克比矩阵。然而,在处理高阶系统问题时,UKF需要特殊的采样策略保证滤波稳定性,Arasaratnam基于三阶Spherical-Radial准则提出了具有较好稳定性的CKF[2],本文将采用确定性采样的滤波方法统称为 Sigma点滤波。

文献[3]对比分析了EKF和Sigma点滤波在组合导航中的性能,结果表明初始姿态误差较大时后者具有较快的收敛速度。Wendel等人[4-5]基于GNSS/INS紧组合模型分析了两者的性能,结果表明Sigma点滤波较EKF并无优势。然而,上述分析均没有考虑滤波过程中状态的可观测性问题,Zhao[6]基于车载导航系统的状态可观测分析对CKF和EKF的性能进行了验证,结果表明当系统非线性较强或状态可观测性较弱时,CKF优于EKF。鉴于Zhao和 Wendel的结论不一致,即Sigma点滤波方法是否能改进紧组合导航性能尚无一致结论,因此有必要对适用于GNSS/INS紧组合的非线性滤波方法进一步验证。无论是Sigma点滤波还是EKF,其滤波过程都包括状态先验和量测似然分布的逼近,且均基于KF框架完成状态估计。Morelande等人[7]的研究表明只有当观测量的信噪比较低时,Sigma点滤波逼近状态后验概率密度函数(PDF)才优于KF。目前大多数非线性滤波的研究集中在新型的数值逼近策略上,然而在滤波过程中除非线性截断误差外身的缺陷导致。

在非线性系统的状态估计过程中,Gustafsson建议单独研究状态先验PDF的预测和滤波更新过程[8-9]。KF滤波的发散问题多出现在量测更新阶段,GNSS/INS滤波过程中非线性量测方程导致的稳定性问题更加突出。量测方程的非线性强度和状态预测误差直接影响KF更新过程中量测似然函数的逼近精度,而Newton迭代法是解决非线性回归问题的常用方法[10]。文献[11]从最优化思想的角度对几种不同的迭代EKF算法进行了对比分析,结果表明迭代算法较常规EKF滤波结果有更好的精度和一致性。韩萍等人[12]采用迭代的中心差分卡尔曼滤波器估计飞机姿态取得较好效果,但是算法收敛速度受迭代步长影响较大。宋宇等人[13]采用迭代Sigma点逼近策略解决粒子滤波中的粒子退化问题,并将其应用于移动机器人的定位中,结果表明其在系统噪声较大量测质量较好的情况下较非迭代算法有较大的优势。上述方法均在迭代量测更新中采用Newton迭代的方法,其在初始误差较大时的收敛速度有待改善。

本文提出一种基于阻尼Newton法改进的迭代CKF,不同于文献[14]中的迭代UKF(IUKF),其在状态先验分布的预测过程中不重新产生Sigma点,以避免预测误差协方差阵不正定导致的滤波发散并减少运算量。为改善状态初始误差较大时非线性量测更新的稳定性,基于Backtracking线搜索(BLS)优化迭代步长控制系数,加快迭代过程的收敛速度。将上述基于BLS的迭代CKF应用于GNSS/INS紧组合中,数值仿真和跑车实验验证了算法的有效性。

1 非线性量测更新

考虑非线性离散系统

(1)

(2)

(3)

(4)

无论是EKF还是Sigma滤波其均做关于p(xk|z1:k-1)的统计线性回归(SLR)逼近量测似然分布。当p(xk|z1:k-1)误差较大时,或者量测值较精确时(即后验PDF较先验PDF窄很多时)上述SLR方法可能失败。文献[11]和文献[16]采用迭代方法逼近状态后验PDF,具有较好的滤波稳定性和精度,然而前者基于线性化逼近p(zk|xk)的框架,非线性较强时容易导致滤波发散,后者的计算复杂度较大且当p(xk-1|z1:k-1)误差较大时可能会得到不精确的后验PDF。

2 Sigma点迭代滤波框架

2.1 紧耦合算法

(5)

式中:δr、δv和δψ分别是位置、速度和方向误差向量,ωen是地理坐标系相对于地球坐标系的旋转角速度矢量,ωie是地理坐标系相对于惯性坐标系的旋转角速度矢量,ωin是地理坐标系相对于惯性坐标系的旋转角速度矢量,f是加速度计测量的比力矢量。加速度误差向量和陀螺误差向量ε近似于随机游走过程模型。

2.2 非线性滤波的最优化思想

获得k时刻的观测量Zk后,状态量的后验概率密度函数(PDF)可以由式(6)求得:

(6)

即滤波更新的过程等价于最大化后验PDF的优化问题。由式(6)知其等价于

(7)

采用Newton-Raphson算法处理代价函数V(x)所述的非线性无约束最优化问题有:

xi+1=xi-[2V(xi)]-1V(xi)

(8)

(9)

(10)

代入式(9)并利用矩阵求逆引理得

(11)

由于EKF 未考虑泰勒展开后的高阶项,所以其线性化后的非线性量测方程为:

zk=Hkxk|k-1+vk+ξk

(12)

式中:ξk为非线性截断误差,存在

(13)

(14)

(15)

2.3 迭代更新优化

在非线性滤波的迭代更新过程中,代价函数V(x)的下降受量测方程非线性的影响较大,适当的选择步长控制系数αi可以加速并确保V(x)下降。本项目采用BLS优化步长的策略,定义步长S和步长下降量ΔV为:

(16)

基于BLS更新过程是在r∈(0,0.5)、β∈(0,1)条件下计算αi=αiβ直至V(xi+αiSi)≤V(xi)+γαiV(xi)TSi,停止更新得到αi,式(14)所示的Newton迭代过程描述如图1所示。

由图1知迭代更新的xi+1并没有反馈到模型预测过程中,即迭代过程只考虑非线性量测似然方程的SLR,因为非线性滤波器的多数问题都发生在量测更新阶段。由于时间更新过程受模型不确定性影响较大,有文献报道指出在预测误差的协方差阵Pk|k-1中添加正矩阵ΔQ可以增加滤波器的稳定性,因此将迭代过程反馈到时间更新阶段将增大预测过程对系统模型不确定性的敏感性。同时,考虑到组合导航系统方程的非线性较弱,通过使用闭环校正的滤波结构可以减小预测过程的不确定性。

图1 步长优化的迭代量测更新

3 实验仿真

为分析 BLS-ICKF算法的有效性分别基于数值仿真和车载实验进行验证,值得注意的是文献[11]中的IEKF和文献[14]中的IUKF 在迭代过程中等价于固定步长控制系数αi(例如取αi=1)

图2 αi=1时的迭代量测更新

3.1 数值仿真

图3 优化αi的迭代量测更新

由图2可知,αi=1时ICKF和IEKF的迭代更新效果并无区别,而LBS-ICKF在迭代3次后得到V(x)为0.503,x为0.94,其较IEKF得到的0.810和1.10有明显改善,由于LBS 速度非常快且易实现,将其用于步长控制系数的优化切实可行。值得注意的是当初始状态值的误差减小时,如将x0增大10倍,两种方法具有相似的性能,这主要由于量测方程的非线性较弱,EKF能满足弱非线性量测似然方程的估计需求。

3.2 车载试验

陆地车辆导航应用中姿态角的可观测性较弱,迭代量测更新等价于在当前时刻获得额外的观测量,从而改善弱可观测性状态的估计精度。车载实验设备如图4所示,采用差分方案提高GPS输出导航参数精度,IMU由闭环干涉型光纤陀螺(IFOG)和MEMS加速度计组成,其中IFOG零偏稳定性小于1 °/h,加速度计零偏稳定性小于1 mgn,参考轨迹为一套NovAtel SPAN系列产品输出。GNSS/INS组合滤波器采用状态误差模型和闭环校正结构,因此系统方程中的非线性误差可以忽略不计。

图4 场地实验设备

需要说明的是,在试验中利用差分GPS作为组合导航系统的位置基准,其定位误差小于1 m,系统的结果与其比较得到位置误差。表1和表2给出了GPS/INS紧耦合和纯惯性的导航精度的比较结果。从表中可以看出GPS紧耦合具有很高的精度。

表1 GPS紧耦合最大位置误差

表2 纯惯性最大位置误差

图6 航向角和俯仰角的迭代更新

图5为行车轨迹,其中前半程车辆保持为“8”字形运动,后半程为平稳的直线行驶过程。仿真结果发现迭代停止阈值ε的选择需根据具体的对象经验性的调整,且当迭代次数大于3时滤波效果改善较小,因此本文手动设置迭代3次。如图6所示,BLS-ICKF较CKF提高了航向角和俯仰角的估计精度,当姿态角状态的可观测性较好时(频繁转弯)迭代量测更新性能优势不明显,当车辆直线行驶时迭代量测更新有较好的改进效果。

图5 场地实验轨迹

图7为东向速度的迭代更新结果,可以发现其精度并没有得到改善,北向速度估计也得到类似的结果。姿态误差以及IMU误差属于不可直接观测的状态量,其误差校正量的估计是通过系统模型耦合实现,即在时间更新过程利用估计误差协方差矩阵Pk-1|k-1的非对角线元素实现不可直接观测状态误差的校正。

图7 东向速度的迭代更新

BLS-ICKF和CKF的性能对比如表3所示。表3为BLS-ICKF和CKF这两种方法的时间消耗和滤波结果的均方误差(10次仿真结果的均值),从表中可以发现BLS-CKF牺牲较小的时间复杂度显著,大大改善了车辆航向角的估计精度,但是迭代更新无法减小速度估计误差。

表3 BLS-ICKF和CKF的性能对比

4 结论

基于Backtracking线搜索实现了一种改进的迭代CKF算法,提出了一种简化的迭代量测更新框架并将其应用GNSS/INS紧组合中。数值仿真和跑车实验结果表明,BLS-CKF在初始误差较大时较常规的迭代UKF和迭代EKF有更快的收敛速度,EKF和 CKF在处理弱非线性的量测更新中具有相似的性能。将BLS-CKF应用于存在弱可观测状态量的车载导航状态估计,BLS-CKF牺牲较小的时间复杂度得到航向角误差约为CKF估计的 41%。下一步工作将基于状态估计误差的检测,建立迭代更新方法的约束模型,并针对控制系数。

[1] Julier S J,Uhlman J K,Durrant-Whyte H F. A New Approach for Filtering Nonlinear Systems[C]//Proceedings of the American Control Conference. Washington,USA:IEEE,1995:1628-1632.

[2] Arasaratnam I,Haykin S. Cubature Kalman Filters[J]. IEEE Trans Autom Control,2009,54(6):1254-1269.

[3] Crassidis J L. Sigma-Point Kalman Filtering for Integrated GPS and Inertial Navigation[J]. IEEE Trans Aerosp Electron Syst,2006,42(2):750-756.

[4] Wendel J,Metzger J,Moenikes R,et al. A Performance Comparison of Tightly Coupled GPS/INS Navigation Systems Based on Extended and Sigma Point Kalman Filters[J]. Navigation:Journal of the Institute of Navigation,2006,53(1):21-31.

[5] 侯当云,吕伟杰. 基于偏差迭代的干扰源定位算法[J]. 传感技术学报,2015,28(12):1818-1822.

[6] Zhao Y W. Performance Evaluation of Cubature Kalman Filter in a GPS/IMU Tightly-Coupled Navigation System[J]. Signal Processing,2016,119:67-79.

[7] Morelande M R,Garcia-FernndezF. Analysis of Kalman Filter Approximations for Nonlinear Measurements[J]. IEEE Transactions on Signal Processing,2013,61(22):5477-5484.

[8] 刘栓,宋喜忠. 基于可信度估计的迭代滤波的WSNs数据融合算法[J]. 电子器件,2016,39(4):810-815.

[9] Gustafsson F,Hendeby G. Some Relations between Extend and Unscented Kalman Filters[J]. IEEE Transactions on Signal Processing,2012,2(60):545-555.

[10] 庞鸿锋,潘孟春,王伟,等. 基于高斯牛顿迭代算法的三轴磁强计校正[J]. 仪器仪表学报,2013,34(7):1507-1511.

[11] Skoglund M A,Hendeby G,Axehill D. Extended Kalman Filter Modifications Based on an Optimization View Point[C]//18th International Conference of Information Fusion,2015.

[12] 韩萍,干浩亮,何炜琨,等. 基于迭代中心差分卡尔曼滤波的飞机姿态估计[J]. 仪器仪表学报,2015,36(1):187-193.

[13] 宋宇,孙富春,李庆玲. 移动机器人的改进无迹粒子滤波蒙特卡罗定位算法[J]. 自动化学报,2010,36(6):851-857.

[14] Zhan R H,Wang J W. Iterated Unscented Kalman Filter for Passive Target Tracking[J]. IEEE Tran on Aerospace and Electronic Systems,2007,43(3):1155-1163.

[15] Ito K,Xiong K Q. Gaussian Filters for Nonlinear Filtering Problems[J]. IEEE Trans Autom. Control,2000,45(5):910-927.

[16] García-FernndezF,Svensson L,Morelande M R,et al. Posterior Linearization Filter:Principles and Implementation Using Sigma Points[J]. IEEE Transactions on Signal Processing,2015,63(20):5561-5573.

[17] Xiong K Q,Zhang H Y,Chan C W. Performance Evaluation of UKF-Based Nonlinear Filter[J]. Automatica,2006,42:261-270.

ImprovedIteratedCubatureKalmanFilterandItsApplicationtoTightlyCoupledGlobalNavigationSatelliteSystem/InertialNavigationSystem

HUAXuehui*,CHENDali

(Foshan Polytechnic Institute,Foshan Guangdong 528000,China)

To improve the stability and accuracy of nonlinear filter used in tightly coupled GNSS/INS,a novel iterated cubature Kalman filter(CKF)is proposed based on backtracking line search(designated as BLS-ICKF). First,a generalized iterated framework is developed,and a damped Newton method is used to improve filtering accuracy with large initial state error. Moreover,a simplified iterated update structure is proposed to improve the robustness of prediction process for integrated system. Numerical simulations and field test reveal that,compared with CKF,BLS-ICKF improves the heading accuracy by 59% in terms of mean square error,and has a faster convergence rate than traditional iterated filtering method.

integrate navigation;iterated filtering framework;cubature Kalman filter;backtracking line search

10.3969/j.issn.1005-9490.2017.05.026

2016-08-04修改日期2016-09-24

TP273;TH89

A

1005-9490(2017)05-1185-06

化雪荟(1976-),女,甘肃景泰人,硕士,高级实验师,主要从事应用电子设计、移动通信技术、传感器应用方向的研究,huaxuehui_vip@163.com;

陈大力(1975-),男,吉林四平人,副教授,主要从事工业控制、电子设计方面研究。