GPS/INS松耦合组合导航的自适应卡尔曼滤波算法研究

周先林，张慧君，和涛,4，李孝辉

GPS/INS松耦合组合导航的自适应卡尔曼滤波算法研究

周先林1,2,3，张慧君1,2,3，和涛1,2,3,4，李孝辉1,2,3

2. 中国科学院 国家授时中心，西安 710600；中国科学院 精密导航定位与定时技术重点实验室，西安 710600；3. 中国科学院大学，北京 100049；4.中国船舶集团有限公司 昆明船舶设备研究试验中心，昆明 650051）

针对常规卡尔曼滤波应用在GPS/INS组合导航时，由于量测数据出现异常值或系统状态模型不准确而造成的滤波精度下降问题，提出了一种基于新息的自适应卡尔曼滤波算法（AKF）。该算法首先通过卡方检验检测出量测异常值，在量测异常值处调整量测噪声方差阵来抑制滤波发散；在此基础上根据新息协方差的计算值与新息协方差的预测值的粗略比率，调整系统噪声方差阵，从而提高整体滤波精度。通过跑车试验，对本文提出的AKF算法进行了验证。试验结果表明：本文提出的AKF算法较常规卡尔曼滤波算法在经度、纬度误差（均方根）上分别降低了67%，34%，在东向速度、北向速度误差（均方根）上分别降低了47%，38%。从而证明了该算法能有效地抑制由量测异常值导致的状态估计误差，防止滤波发散，提高滤波稳定性。

组合导航；自适应卡尔曼滤波；新息；噪声方差

0 引言

惯性导航系统（Inertial Navigation System，INS）是一种不依赖于外界信息的自主式导航系统，不仅提供位置和速度信息，还能提供姿态角信息，具有短时精度高和稳定性好的优点，但是却具有误差随着时间快速累积增长的缺点[1-2]。全球定位系统（Global Positioning System，GPS）具有全天候、高精度导航定位的优点，但是具有信号易受遮挡和干扰的缺点[3-4]。GPS/INS松耦合组合导航既保留了各自解的独立性，又可以结合二者的优点，使组合后的导航精度高于各系统单独工作的精度[5]。通过卡尔曼滤波在位置域将两者融合，可提供载体的连续的位置、速度和姿态解。同时，滤波器输出的误差反馈给惯性测量单元（inertial measurement unit，IMU），可以抑制随时间增长的惯导误差。这种组合导航的方式被广泛应用于国防和军事中，具有极大的应用价值。

常规的GPS/INS组合导航卡尔曼滤波（Kalman filter，KF）需要准确的数学模型以及相对应的系统噪声和量测噪声统计特性预先准确已知[6]。然而在实际应用中，量测噪声和系统噪声的统计特征值很难准确获得。例如，系统噪声方差阵常由陀螺仪和加速度计零偏、速度随机游走（velocity random walk，VRW）、角度随机游走（angle random walk，ARW）等一阶高斯马尔可夫过程参数确定。如果惯导测量单元各项系统噪声指标在出厂时未经过严格标定，则无法获得准确的系统噪声协方差阵。另外，随着应用场景、环境等的变化会使惯导测量误差加大；环境的干扰或遮挡会使GPS信号质量迅速下降，导致量测误差增大。在这些情况下，固定不变的噪声统计特性会导致状态估计误差加大，滤波结果不准确。不准确的状态估计量反馈至惯导测量单元后甚至会引起滤波发散。

针对上述噪声统计特性的不准确问题，可采用自适应的卡尔曼滤波算法[7-10]。在利用测量数据进行滤波递推的同时，判断系统的动态是否发生变化。若有变化则把这种变化看作随机干扰归到模型和噪声中去，对模型参数和噪声统计特性进行估计和修正使之适应系统动态的变化[11-12]。自适应卡尔曼滤波的方法有多种，包括基于精度因子（dilution of precision，DOP）自适应滤波[13]，衰减记忆滤波[14-17]，Sage-Husa自适应滤波[18-20]。其中，基于DOP值自适应滤波只能修正由卫星几何布局变化引起的量测噪声的偏差；衰减记忆滤波在一定程度上能修正模型误差引起的系统噪声；Sage-Huse自适应滤波可以修正一定的噪声统计特性，但当阶数较高时易发散[21]。本文提出一种基于新息的自适应滤波方法（adaptive Kalman filter，AKF），该滤波方法根据新息估计量测噪声和系统噪声，从而间接调整卡尔曼滤波增益抑制滤波发散，提高滤波精度。该方法计算简单且具有实用性。

1 卡尔曼滤波方法及滤波参数影响分析

卡尔曼滤波的系统状态方程和量测方程如下：

2 基于新息的自适应卡尔曼滤波参数调整方法

首先可以利用新息构造卡方检验统计量，对量测数据进行实时异常值检测，并通过新息协方差对进行调整，在此基础上，利用新息协方差构造自适应因子对系统噪声方差阵进行实时估计和调整。具体调整过程如下。

2.1 利用检验检测量测数据异常值

设置适当的分位数，检验拒绝域为

数值在检验拒绝域内说明统计量的实际值与理论值偏离程度过大，的假设不成立，即认为新息的零均值特性丧失，量测量受到异常干扰，出现异常值。

2.2 调整量测噪声方差阵R

当通过2.1节检测到量测数据异常值时，利用新息协方差进行后验估计，在原先的值上加入新息协方差的变化值，对进行上升调整，减小增益矩阵，降低带有异常值的量测数据对滤波的影响，量测噪声协方差调整公式如下：

2.3 修正系统噪声方差阵Q

3 基于新息的自适应卡尔曼滤波算法在GPS/INS松耦合组合导航中的应用

图1 基于新息的GPS/INS松耦合自适应卡尔曼滤波算法流程图

4 车载路测实验及结果分析

本文为了验证基于新息的自适应卡尔曼滤波算法的性能，于2019年7月10日在西安临潼商业街道进行了跑车实验。下面分别给出实验安排和实验结果分析。

4.1 实验安排

本文跑车实验数据采用SBG Ekinox2作为惯性测量单元，使用NovAtel接收机进行GPS（千寻RTK服务）定位，其中，GPS理论定位精度为分米级，其他试验参数如表1所示。

表1 试验设备参数设置

实验将第三方SBG Ekinox 2的组合导航输出结果（理论定位精度为0.02 m）作为参考值，实验分析的各项定位测速误差都是以该参考值为基准得出。将该参考值输入到谷歌地图中，得到跑车真实运动轨迹如图2白色粗线所示。

图2 跑车试验轨迹

整个车载实验过程中，部分时段GPS信号受附近高楼和树木等影响，GPS定位出现异常，图2中用方框标记圈出的区域就属于此种情况，该区域的GPS定位误差如图3所示。由于GPS定位异常会影响组合导航的定位精度，故本文选取方框区域为研究对象，分别用本文提出的AKF算法和常规KF算法对该区域导航数据进行处理，通过比较两种算法的滤波结果来验证本文算法的有效性。

图3 GPS定位结果

4.2 实验结果分析

分别使用AKF算法和KF算法对图2中方框区域所对应的测量数据进行INS/GPS松耦合解算，将解算结果与参考值进行比较，得到经度方向和纬度方向的位置误差对比如图4所示，速度误差对比如图5所示。其中，虚线表示AKF算法滤波结果，实线表示KF算法滤波结果。

图4 KF、AKF位置误差对比

图5 KF、AKF速度误差对比

表2列出了KF算法和AKF算法在方框区域位置误差和速度误差的均值（mean）及均方根（root mean square，RMS），可以算出AKF算法在经度、纬度、东向速度、北向速度误差较KF算法均值上分别降低了39%，20%，95%，50%；均方根分别降低了67%，34%，47%，38%。故，本文提出的AKF算法较KF算法具有更好的导航精度和滤波稳定性。

表2 算法性能比较

注：mean为均值，RMS为均方根。

5 结论

本文以GPS/INS组合导航应用为背景，针对由于系统噪声协方差阵不准确、观测数据突变造成的常规Kalman滤波精度下降问题，提出一种基于新息的自适应Kalman滤波算法。该算法首先利用卡方检验对量测异常值进行识别，再用新息协方差调整量测噪声来抑制异常值；然后，通过自适应因子对系统噪声进行调整来改善系统噪声协方差阵不准确的问题。并通过跑车试验进行验证，实验结果显示：AKF算法在经度、纬度、东向速度、北向速度误差较KF算法均值上分别降低了39%，20%，95%，50%；均方根分别降低了67%，34%，47%，38%，有效地提高了整体滤波精度。相对使用KF算法经度误差峰值达到-19 m，使用AKF算法经度误差峰值能降至-5 m，证明AKF算法能有效地抑制异常值，防止滤波发散。

[1] 王洪先. 陆用惯性导航系统技术发展综述[J]. 光学与光电技术, 2019, 17(6): 77-85.

[2] 杨晓明, 王胜利, 王海霞, 等. 基于EKF的GNSS/SINS组合导航系统应用[J]. 山东科技大学学报(自然科学版), 2019, 38(6): 114-122.

[3] 崔杉, 熊力. 全球卫星导航系统[J]. 电子制作, 2015(16): 47.

[4] 汪勇, 丁金学. 全球卫星导航系统的市场应用前景[J]. 中国航天, 2012(9): 30-36.

[5] 樊宇, 程全. GPS与惯性导航系统的组合应用研究[J]. 制造业自动化, 2015(3): 74-75.

[6] 胡锋, 孙国基. Kalman滤波的抗野值修正[J]. 自动化学报, 1999, 25(5): 692-696.

[7] WU F, YANG Y, CUI X. Application of adaptive factor based on partial state discrepancy in tight coupled GPS/INS integration[J]. Geomatics and Information Science of Wuhan University, 2010, 35(2): 156-159.

[8] WU F M, NIE J L, HE Z B. Classified adaptive filtering to GPS/INS integrated navigation based on predicted residuals and selecting weight filtering[J]. Geomatics & Information Science of Wuhan University, 2012, 37(3): 261-264.

[9] YANG Y, HE H, XU G. Adaptively robust filtering for kinematic geodetic positioning[J]. Journal of Geodesy, 2001, 75(2-3): 109-116.

[10] YANG Y, XU T. An adaptive Kalman filter based on sage windowing weights and variance components[J]. Journal of Navigation, 2003, 56(2): 231-240.

[11] 申逸. Kalman滤波技术在目标跟踪中的应用研究[D]. 长沙: 国防科学技术大学, 2006.

[12] 杨元喜. 动态Kalman滤波模型误差的影响[J]. 测绘科学, 2006, 31(1): 17-18.

[13] 姜庆峰, 桑渤, 潘泉有. 基于DOP值的GPS/INS组合导航滤波方法[J]. 海洋测绘, 2016, 36(1): 59-62.

[14] 田宠, 王兴亮, 卢艳娥. 一种改进的自适应指数加权衰减记忆滤波算法[J]. 武汉理工大学学报, 2011, 33(11): 147-152

[15] 蔡佳, 黄长强, 井会锁, 等. 基于指数加权的改进衰减记忆自适应滤波算法[J]. 探测与控制学报, 2013, 35(4): 21-26.

[16] ZHAO L, LIU J. An improved adaptive filtering algorithm with applications in integrated navigation[C]//IEEE International Conference on Digital Manufacturing＆ automation, 2012: 182-185.

[17] 谭攀, 伍仲南, 康跃耀. 抑制卡尔曼滤波发散的组合导航算法研究[J]. 地理空间信息, 2019, 17(9): 109-112.

[18] SAGE A P, HUSSA G W. Adaptive filtering with unknown prior statistics[C]//Proceedings of Joint Automatic Control Conference. Boulder, USA: ASME, 1969: 760-769.

[19] 鲁平, 赵龙, 陈哲. 改进的Sage-Husa自适应滤波及其应用[J]. 系统仿真学报, 2007(15): 3503-3505.

[20] 徐恩松, 陆文华, 刘云飞,等. 基于卡尔曼滤波的数据融合算法与应用研究[J/OL]. 计算机技术与发展, 2020(5): 1-7.

[21] 付梦印, 邓志红, 闫莉萍. Kalman滤波理论及其在导航系统中的应用[Ｍ]. 北京: 科学出版社, 2010: 111-118．

[22] 袁美桂, 严玉国, 庞春雷,等. 改进的自适应Kalman滤波在GPS/SINS中的应用[J]. 空军工程大学学报(自然科学版), 2015, 16(5): 65-69.

[23] 沈凯, 管雪元, 李文胜. 扩展卡尔曼滤波在组合导航中的应用[J]. 传感器与微系统, 2017, 36(8): 158-160.

Research on adaptive Kalman filter algorithm for GPS/INS loosely coupled integrated navigation

ZHOU Xian-lin1,2,3, ZHANG Hui-jun1,2,3, HE Tao1,2,3,4, LI Xiao-hui1,2,3

(1. National Time Service Center, Chinese Academy of Sciences, Xi’an 710600, China;2. Key Laboratory of Precise Positioning and Timing Technology, Chinese Academy of Sciences, Xi’an 710600, China;3. University of Chinese Academy of Sciences, Beijing 100049, China;4. Kunming Shipborne Equipment Research and Test Center,China State Shipbuilding Corporation Limited, Kunming 650051, China)

Aiming at the reduction of the filtering accuracy when the conventional Kalman filter is used in GPS/INS integrated navigation due to outliers of measurement data or inaccurate system state model, an adaptive Kalman filter algorithm based on innovation is proposed. First, the algorithm detects measured outliers by chi-square test, and adjusts the measurement noise variance matrix at the outliers to suppress filtering divergence. Thereby, the rough ratio between the calculated value and the predicted value of the innovation variance matrix is calculated to adjust the system noise variance matrix and then the overall filtering accuracy will be improved. The AKF algorithm proposed in this paper is verified by a vehicle test. The results show that, the proposed AKF algorithm in this paper reduces the longitude errors and latitude errors (root mean square) with 67% and 34% comparing with the conventional Kalman filter algorithm, and reduces the eastward velocity errors and northward velocity errors (root mean square) with 47% and 38%. It is proved that the algorithm can effectively suppress the state estimation error caused by the measured outliers, and further improve the filtering stability.

integrated navigation; adaptive Kalman filtering; innovation; noise variance

10.13875/j.issn.1674-0637.2020-03-0222-09

2020-01-19；

2020-04-09

基础研究重大项目前期研究专项资助（11703030）

周先林，女，硕士，主要从事多源辅助的GNSS/INS组合导航性能改善研究。