一阶惯性滤波算法程序_基于牛顿插值的抗差卡尔曼滤波算法

引文格式：蔡保杰, 邵雷, 李佳伟, 等. 基于牛顿插值的抗差卡尔曼滤波算法[J]. 导航定位学报, 2020,8(5): 49-56.(CAI Baojie, SHAO Lei,LI Jiawei,et al. Robust Kalman filtering algorithm using Newton interpolation[J]. Journal of Navigation and Positioning, 2020, 8(5): 49-56.)

基于牛顿插值的抗差卡尔曼滤波算法

蔡保杰¹，邵  雷¹，李佳伟²，李正杰¹

(1.空军工程大学 防空反导学院，西安  710051；

2.西安电子科技大学 电子工程学院，西安  710071)

摘要：针对导航系统在运动过程中，卫星导航观测值会受到扰动从而产生较大误差的问题，提出1种基于牛顿插值的抗差卡尔曼(Kalman)滤波算法：通过实验对卡方检验的阈值进行较为精确的选取，以提高对故障观测值检测的准确度；然后利用前面几个时刻观测值组成的牛顿插值函数进行外延，对当前故障观测值实时修正。实验结果表明，该算法能够有效抑制观测值故障带来的影响，相比于使用抗差滤波前，位置误差精度提升约76%，速度误差精度提升约80%，可有效提高组合导航系统的可靠性。

0  引言

导航系统在航空航天和日常生活中应用广泛。导航的方法有很多种，惯性导航和卫星导航的组合导航的方法是最为常用的1种^[1]。在运载体运动过程中，由于所处环境错综复杂，卫星导航的观测值会出现损坏的情况，严重影响导航的精度。 对于观测值故障问题^[2-3]，常用的是传统抗差卡尔曼(Kalman)滤波方法^[4]，文献[5]中构造了1个故障检验公式，与设定的卡方检验阈值相比较，对检验值进行分段处理。文献[6]先用假设检验的方法检测出故障观测值的具体位置，用设定的门限值代替观测噪声值，可以较大程度地减小粗差的影响。文献[7]在一步预测均方误差阵上乘1个渐消因子，进行自适应滤波。文献[8]通过降低滤波增益值来达到修正预测值的效果。文献[9]提出错误预警率和故障探测率的概念，对故障的定位更加精确。文献[10]通过等价权函数法对观测噪声方差阵进行替换，可以实时有效地滤除粗差。 上述方法都能一定程度地减小粗差带来的影响，但也存在不足之处。卡方检验并不能保证百分之百的正确率，适当地选取阈值尤为重要。当检测系统出现误检情况时，用门限值代替正确的观测值也会造成观测值误差相对增大。针对这种情况，本文提出1种基于卡方检验和牛顿插值的抗差滤波方法，利用前几个时刻的观测值组成的牛顿插值多项式进行外延，对当前时刻的故障观测值进行替换，以使组合导航系统稳定性更高，抗差能力更强。

1   组合导航系统建模与常用的抗差滤波方法概述

1.1  组合导航系统建模

导航系统能够测量并解算出运载体的瞬时运动状态和位置，提供给驾驶员或自动驾驶仪，以实现对运载体的正确操纵或控制。随着科学技术的发展，可供导航系统利用的信息源越来越多，导航系统种类也越来越多。捷联惯导(strapdown inertial navigation system, SINS)和全球卫星导航系统(global navigation satellite system, GNSS)所构成的组合导航是近年来导航领域最常使用的1种方法，SINS作为主导航系统，利用惯性器件实时解算出运载体的位置、速度等信息，而GNSS也可以对SINS不断地进行修正，避免了捷联惯导系统误差随时间增大的问题。

1.2  2种常用的抗差滤波方法

 1  等价权函数法

 2 卡方检验法

卡尔曼滤波是1种线性最小方差估计，是1个递推的过程，而在每1步递推的过程中，残差是1个很重要的信息来源，它包含有1步预测误差信息。当导航系统发生故障时，残差序列会有异常变化。利用残差序列的这个特性，可以对故障观测值进行检测。此方法分2步进行，即先对故障进行检测，再对有故障的观测值进行处理。卡方检验的方法在计算上较为复杂，但其检测精度较高，故本文采用卡方检验的方法。

2  基于牛顿插值的抗差卡尔曼滤波算法

2.1  采用卡方检验的故障检测方法

阈值的选取

在实际的运动过程中，载体所处的环境是比较复杂的，对观测值故障的容忍程度也会变大，所以选取的阈值也相对较大。当阈值选取过小时，会把一些正确的观测值当作故障观测值来处理，随着时间的推移会出现累积误差；当阈值选取过大时，被认定为出现故障的观测值就会变得很少，系统几乎检测不到故障，使抗差滤波之前和之后的曲线接近重合。

表1  显著性水平和阈值的关系

由表1可知，显著性水平越小，阈值越大，故障的判定条件就越宽松。本文通过大量的实验分析，发现当显著性水平为0.25~0.01、阈值为5.024~6.635时可以取得较好的效果，如图1所示。卡方检验统计量分布在6.635以下的概率为96.8%，以阈值为6.635作为判定条件可以较为精确地检测到故障，误检的概率较小，所以本文选取6.635作为阈值进行仿真验证。

图1  卡方检验统计量分布

2.2  利用牛顿插值修正观测值

插值法^[17]是利用已知函数某个区间的若干点，构造1个近似函数来近似代替，牛顿插值可以根据计算精度灵活增加插值节点进行递推运算，公式形式对称、结构紧凑，容易编写程序。本文采用的牛顿插值方法是将近似函数进行1个单位的外延。 经典组合导航系统中，每次滤波过程的间隔时间一般是固定的，可以使用等距的插值公式。一般来说，插值节点取得越多，利用牛顿插值构造的曲线与原函数曲线近似程度越高，误差越小，适当增加节点数对计算结果的准确度是有帮助的，但并非插值多项式的次数越高越好。当插值节点增多时，不能保证非节点处的插值精度得到改善，有时反而误差更大。龙格现象表明，大范围内使用高次插值，逼近的效果往往是不理想的。根据大量实验验证，牛顿插值在4阶时精度较高，出现龙格现象的概率也比较小，所以本文将以4阶牛顿插值多项式来进行实验验证。

3  仿真分析及验证

3.1  仿真轨迹生成

本文模拟1条飞机飞行轨迹来验证抗差滤波算法的有效性，飞行轨迹如图2所示，将模拟的轨迹数据送入轨迹发生器，分别得到惯性导航系统(inertial navigation system, INS)和全球定位系统(global positioning system, GPS)的数据，各误差值如表2所示。

图2  飞机飞行轨迹

表2  惯性导航误差

仿真步长为1 s，共用时500 s，为营造与飞机飞行相近的环境，人为地给观测值加入噪声，加入噪声的数值如表3所示。

表3  随机噪声值

为验证本抗差算法的有效性，制造量测不准确的外部环境，仿真在每隔10 s的时刻加入1个噪声放大系数m，使此时刻的噪声含有m倍粗差。

3.2  分析与对比

本文对比使用上述抗差方法和不使用抗差方法的位置和速度误差，如图3、图4所示。

考虑到当GNSS信号受到影响时，INS仍然处于工作状态，分析对比了使用本文提出的抗差方法和仅使用INS的误差，如图5、图6所示。

从图3、图4可以看出，当不使用任何抗差方法时，由于GNSS信号误差非常大，造成观测值发生急剧变化，如表4所示。在使用本文提出的基于牛顿插值的抗差滤波方法后，对GNSS所收到的观测值进行改良，使观测值恢复到正常范围，使位置和速度误差大大减小，如表5所示。 惯性导航误差是随着时间逐渐增大的，没有卫星导航的实时修正，惯性导航方法无论是位置误差还是速度误差都会慢慢增大，与此相比，本文提出的基于牛顿插值的抗差滤波方法能较好地使误差维持在1个较小的范围内，使导航精度得到提高，如图5、图6所示。但不可否认的是，惯性导航在独立工作时间较短时，导航精度仍有一定的准确性。与此相比，由于在导航数据已有误差的前提下又人为加入噪声，致使使用本文提出的抗差方法在短时间内导航精度比只使用惯性导航精度略低，北向速度误差和天向速度误差在个别点处出现较大波动，如图6(b)、图6(c)所示。但北向速度误差值仍在-1.78~0.57 m/s，天向速度误差仍在-0.62~0.59 m/s这样1个较小的范围内，且随着时间的推移，惯性导航误差增大的现象慢慢显现出来。综合对比来看，基于牛顿插值的抗差卡尔曼滤波新算法仍不失为1种有效的抗差滤波方法。

5  结束语

本文对INS/GNSS导航系统在运动过程中GNSS信号会出现故障问题，提出1种基于牛顿插值的抗差卡尔曼滤波算法。该算法通过当前时刻单个观测值的卡方检验方法，通过大量实验选取合适的阈值，对故障观测值进行更精确的定位。用过去几个时刻的观测值组成牛顿插值多项式进行外延，得到当前时刻观测值，给出了基于牛顿插值的抗差卡尔曼滤波方法的具体过程。最后，通过大量的仿真分析进行验证，该算法使误差控制在1个较小的范围内，具有较好的抗差效果。该算法提高了抗差滤波系统的稳定性，计算简单，易于工程实现。

原文有删减，参考文献(略)

基金项目：国家自然科学基金项目(61503408)。

作者简介：蔡保杰(1995—)，男，河北衡水人，硕士研究生，研究方向为组合导航与卡尔曼滤波。

——相关阅读——

陈锐志：室内定位技术已进入了一个新的时代

基于精准地图的室内定位解决方案

声波室内定位系统中基站几何布局探究

室内导航定位技术研究进展与展望