基于指数渐消遗忘因子的组合导航自适应滤波算法

曾庆化，赵天钰，赵 宾,2，刘建业，朱小灵

（1. 南京航空航天大学导航研究中心，南京 210016；2. 南京航空航天大学金城学院，南京 211156）

捷联惯性导航系统（Strapdown Inertial Navigation System, SINS）有较强的自主性和抗干扰能力，但误差随时间累积，易发散[1]；全球卫星导航系统（Global Navigation Satellite System, GNSS）可提供高精度的位置、速度信息，且误差不随时间发散，但易受外界环境干扰，定位、测速精度下降[2]。

SINS/GNSS 组合导航系统可以将二者组合在一起，取长补短，从而获得更高精度的导航参数[3,4]。组合导航系统常采用卡尔曼滤波进行信息融合，但卡尔曼滤波要求精确的数学模型以及准确的先验噪声统计特性，在滤波过程中，若噪声特性出现异常，则滤波精度下降甚至发散[5]。

为了解决这一问题，有学者提出了带渐消因子的自适应算法，通过以强迫残差序列正交的方法，确定渐消因子，文献[6][7]在此基础上，设计了具有多渐消因子的卡尔曼滤波器，取得了更好的精度和鲁棒性，文献[8]针对非线性以及观测噪声异常情况，在容积卡尔曼滤波（Cubature Kalman Filter, CKF）的基础上，提出了基于假设检验的滤波器状态检验算法，在合适的条件下引入多重渐消因子，进一步提升了算法的抗干扰能力。在噪声异常情况下，相比于上述自适应算法，Sage-Husa 自适应滤波（Sage Husa Adaptive Kalman Filter, SHAKF）不仅可以提高滤波精度，还能够实时估计出系统的噪声参数[9,10]，文献[11]基于CKF，结合带有衰减指数渐消因子的SHAKF，提高了纵向距离的滤波精度，文献[12]结合联邦滤波和SHAKF，提出了动态加权因子将两种算法进行融合，有效地应用于多源导航系统，文献[13]依据当前时刻的残差信息，在滤波的量测更新中加入渐消因子，提高了滤波的自适应能力，同时引入可变滑窗提高对量测噪声的估计精度，文献[14]根据各导航状态量的可观测度设置渐消因子，调整滤波增益矩阵，降低了状态估计误差。

上述算法在SHAKF 的基础上做出了改进，取得了更好的滤波效果，但都没有针对SHAKF 中的遗忘因子b 展开深入研究，遗忘因子的大小会影响对量测噪声估计的灵敏度和精度，文献[15]通过滑动窗口检测量测噪声方差变化，提出了基于滑动遗忘因子的自适应滤波算法（Sliding Attenuating Factor Adaptive Kalman Filter, SAFAKF），相比于固定遗忘因子SHAKF，该方法可以兼顾跟踪精度和灵敏度，但其收敛速度有待进一步提高，且存在一定的拖尾效应，实际仿真及应用中，导航精度提升不明显。基于此，本文提出了基于指数渐消遗忘因子的自适应滤波算法（Exponential Attenuating Factor Adaptive Kalman Filter, EAFAKF）。首先设计了基于滑动窗口的χ2故障检测方法，当检测到量测信息故障时，设计了基于指数动态变化的遗忘因子，使得算法在快速估计量测噪声矩阵的同时，能够保证其准确性，从而提高滤波精度。

1 组合导航模型

捷联惯性导航系统通过数学平台解算姿态转移矩阵，从而获取姿态信息并进行坐标系的变换。这里给出本文中出现的几种坐标系及其定义。

(2)地球坐标系Oxeyeze（e 系）：原点位于地心，z轴沿地球自转轴方向，x 轴在赤道平面内，与零度子午线相交，y 轴与x 轴、z 轴构成右手坐标系；

(3)载体坐标系Oxbybzb（b 系）：原点与载体质心重合，x 轴沿载体横轴向右，y 轴沿载体纵轴向前，z轴沿载体竖轴向上，即“右前上”坐标系；

(4)导航坐标系Oxnynzn（ n 系）：当地地理坐标系，原点位于载体质心，x 轴指向东，y 轴指向北，z 轴垂直于当地水平面向上，即“东北天”坐标系。

1.1 状态方程

SINS/GNSS 组合导航系统的状态变量为

则状态方程为

式中，状态转移矩阵A 由式(3)所示的微分方程导出。

1.2 量测方程

SINS/GNSS 组合导航系统量测方程为

式中， Zk为量测向量，Hk为量测系数矩阵， Vk为量测噪声向量，近似为白噪声。量测信息选择为捷联惯导系统和全球卫星导航系统的速度和位置差值，具体如下：

其中，vE,I、vN,I和vU,I是捷联惯导计算得到的东向、北向和天向速度，vE,GNSS、vN,GNSS和vU,GNSS是GNSS输出的东向、北向和天向速度； LI、λI和 hI是捷联惯导计算得到的纬度、经度和高度，LGNSS、λGNSS和 hGNSS是GNSS 输出的纬度、经度和高度。

2 基于指数渐消遗忘因子的自适应滤波算法

2.1 基于滑动窗口的χ 2故障检测方法

卡尔曼滤波实际测量值 Zk与量测预测值之间的残差为 εk，其方差为

在滤波工作正常且GNSS 没有发生故障的情况下，残差服从均值为零的高斯分布，即

式中，m 为量测向量的维度。

当GNSS 量测信息异常时，故障检测函数 λk不再服从上述分布，因此，可以通过χ2分布上分位点性质进行故障检测。例如，当量测维度m=6 时，选取上分位点ζ =18.548，通过查表可知P{χ2( m)＞ ζ} =0.5%，在量测信息正常时， λk＞ ζ的概率只有0.5%，则在99.5%的置信度下可认为系统无故障。

上分位点选取过高或过低都会产生误判，因此本文设计了一种基于滑动窗口的故障检测方法，新的故障检测函数表达式如下：

式中，N 为窗口宽度。窗口宽度不宜设置过大，否则会产生较大的时延，检测能力将变弱，经过权衡考虑，窗口宽度选取3～5 为宜。经优化后的故障检测函数能够更加准确地判断出量测信息是否出现异常。

2.2 Sage-Husa 自适应滤波器改进

SHAKF 可以在进行状态估计的同时根据量测输出实时估计出系统的噪声参数，包括系统噪声均值和量测噪声，一般认为系统噪声具有稳定性，且在实际应用中，量测噪声方差阵对滤波影响较为显著，因此通常仅对量测噪声进行估计[16]。

系统状态空间模型如下：

其中

假设滤波过程中量测噪声方差矩阵 Rk未知或发生变化，常规卡尔曼滤波难以保证滤波精度，若能够实时地估计出量测噪声方差矩阵 Rk，可以提高滤波精度。在滤波过程中，量测预测误差公式为： 测噪声 Vk均值为零，可知的均值也为零，且

将上式移项，可得k 时刻量测噪声方差矩阵为

式中，b 为遗忘因子，易知，越陈旧的数据，其加权系数越小，根据式(15)可构建加权系数如下

对量测噪声矩阵 Rk的估计如下：

对 Rk进行渐消记忆指数加权后，可保持一定的自适应能力，且遗忘因子取值越小，对新量测噪声变化的自适应能力越强，但估计结果会跳变得较为剧烈，遗忘因子取值越大，其自适应能力变弱。因此，为了能够在保证较强自适应能力的同时使得对量测噪声矩阵的估计较为平稳，本文提出了可变遗忘因子的自适应滤波方法。

在滤波过程中，当量测噪声发生变化，通过故障检测函数对滤波状态进行判断，若判断出滤波异常，遗忘因子取较小的值以提高对量测噪声矩阵的跟踪速度，随着时间的推移，为了能够更精确地估计出量测噪声矩阵，遗忘因子的取值应适当增大。考虑到遗忘因子的取值应满足0

式中，t0为量测噪声发生变化的时刻，可由故障检测函数得到，m 决定了遗忘因子的初始值，n 可以控制遗忘因子的变化速度。从式(18)可以看出，当量测噪声变化时，遗忘因子取较小的值，随后遗忘因子的值逐渐增大，使得对 Rk的估计更加精确。

假设系统500 s 后发生故障，量测噪声扩大为原来的10 倍，图1 展示了传统SHAKF、SAFAKF 和EAFAKF 遗忘因子取值的变化曲线。遗忘因子的选取会影响对量测噪声的估计，图2 展示了不同算法对量测噪声的估计结果。可以看出，SAFAKF 存在较为严重的拖尾效应，而EAFAKF 在快速估计出噪声变化的同时，还能保证其准确性。

图1 不同算法遗忘因子变化曲线Fig.1 Attenuating factor variation curve of different algorithms

图2 不同算法对量测噪声矩阵估计结果对比Fig.2 Estimation of measurement noise matrixof different algorithms

滤波算法整体流程如图3 所示。

图3 算法流程图Fig.3 Algorithm flow chart

3实验与分析

3.1 仿真实验

仿真参数设置如下：初始经度λ =118.79 °E，纬度L=31.94° N ，水平姿态角为0°，航向角为30°，初始失准角为[0.1′ 0.2′ 3′] ，参考惯导级IMU 性能指标，设置仿真系统传感器精度如表1所示。

表1 仿真参数Tab.1 Simulation parameters

滤波器参数设置如下：

本文设置了包括起飞、爬升、平飞、降落等阶段的飞行轨迹，飞行时间为1940s。飞行过程中，假设在500～600s以及1200～1500s时间段内，GNSS量测噪声扩大10倍。分别采用常规卡尔曼滤波算法、SHAKF、SAFAKF以及EAFAKF进行滤波估计，位置误差曲线如图4所示。

图4 位置误差曲线Fig.4 Position error curves

在500～600s以及1200～1500s时间段内，GNSS量测信息发生故障，从图4中可以看出，EAFAKF的估计精度更高，且当故障结束后，其收敛过程更加迅速。本文针对上述算法各进行了100次蒙特卡洛仿真验证，并比较了上述算法的均方根误差（Root Mean SquareError,RMSE）。

式中，n表示信号序列， Xi表示导航信息真值，由航迹发生器得到，表示对导航信息的估计值。均方根误差可以很好地反映出量测精度，图5给出了100次蒙特卡洛仿真验证均方根误差的箱形图。

图5 位置均方根误差Fig.5 RMSE of position

从图5可以看出，本文提出算法的均方根误差最小，在位置精度方面，EAFAKF相比于KF、SHAKF、SAFAKF分别平均提高了28.33%，21.99%，20.33%。

3.2 跑车实验

为了验证本文算法的有效性，选取实际的系统进行跑车试验，实验装置如图6所示。

图6 跑车测试平台Fig.6 Experimental test platform

根据实测数据，分析了惯性组件（IMU）和GPS的性能指标，具体参数如表2所示。

表2 传感器参数Tab.2 Sensor paramete rs

车载系统搭载RTK，可以提供高精度的位置、速度信息，并将其作为参考值验证组合导航精度。测试路线在校园内进行，历时1450s，运动轨迹如图7所示。

图7 跑车轨迹Fig.7 Trajectory of experimental test

为了验证本文算法的导航精度，设置了两个卫星信号故障区域，在500s-600s以及1100s-1300s两个时间段内对卫星量测信息加入如下故障：误差增大10倍，测速误差从0.2m/s变化为2m/s，定位误差从10m变化为100m。以RTK输出的速度、位置作为参考值，对比了常规卡尔曼滤波算法、SHAKF、SAFAKF以及EAFAKF的导航精度。图8(a)对比了本文算法导航轨迹与参考轨迹的对比结果，图中选取起始点为坐标原点，x 轴正向为正东，y轴正向为正北，图8(b)、图8(c)分别对比了四种算法在500s-600s（图8(a)中AB段）、1100s-1300s（图8(a)中CD段）两段量测信息异常区域的轨迹结果。

在量测信息发生异常时，本文算法可以快速、准确地估计出量测噪声矩阵，从图8可以看出，相比于常规卡尔曼滤波、SHAKF、SAFAKF，本文算法提供的位置信息更加精确。为了量化算法的性能指标，表3给出了整个导航阶段四种算法的位置均方根误差。

图8 轨迹对比Fig.8 Comparison of trajectory

由表3可以看出，整个导航过程，本文算法的位置均方根误差最小，定位精度相比于KF、SHAKF、SAFAKF分别提高了29.50%、38.82%、26.92%。相比于其他三种方法，本文算法能够在量测信息异常时提供更高精度的导航结果。

表3 四种算法位置均方根误差Tab.3 RMSEof position of four algorithms

4 结 论

为了提高量测信息异常时组合导航精度，本文提出了基于指数渐消遗忘因子的自适应滤波算法。在构建故障检测算法判断量测信息是否出现异常的基础上，引入了基于指数函数变化的遗忘因子，有助于实现对量测噪声统计特性的快速准确评估。通过与常规卡尔曼滤波、SHAKF、SAFAKF 的对比分析可知，本文算法在量测异常的仿真和跑车实验中，导航精度均提高20%以上，具有较好的工程应用潜力。