基于卡方检验的自适应鲁棒CKF组合导航算法

熊 鑫，黄国勇，王晓东

(1.昆明理工大学信息工程与自动化学院，云南 昆明 650500；2.云南省矿物管道输送工程技术研究中心，云南 昆明 650500)

0 引言

捷联惯性导航系统(SINS)和全球卫星导航系统(GPS)广泛应用于飞机和车辆载体的定位与姿态估计[1]。扩展卡尔曼滤波(EKF)算法和无迹卡尔曼滤波(UKF)算法是目前导航中两种常用的算法[2-3]，但却存在一定的局限性。容积卡尔曼滤波(CKF)算法[4-5]，是近年来提出的一种新型非线性滤波算法，具有更好的实时性和滤波精度。但将CKF应用于SINS/GPS组合导航系统，最大的困难是难以获得系统模型和观测模型完整的先验统计信息。由于难以准确描述惯性系统的运动规律，建立的系统模型通常具有近似性，作为系统模型主要先验信息的过程噪声方差对滤波精度有着重要的影响[6-7]，以及观测卫星失锁，观测模型和动力学模型间存在的误差，也会使系统可能发散[8-9]。为同时降低过程噪声不确定性和观测值异常对系统的影响，文献[10]通过对误差开窗拟合的方式来解决观测异常和噪声不确定的影响，但该方式会占用大量存储资源不利于实时的处理。文献[11]提出通过两次卡方检验对系统的判断，在标准CKF，抗差滤波，和自适应滤波进行选择，由于它至多只对一个参数进行了调节，并不能处理观测异常和噪声不确定同时存在的情况。文献[12]通过利用Doyle-Stein条件构造的卡尔曼滤波对动力学模型误差不敏感这一现象，来分离观测异常与模型误差，但是该方法只用于受控系统。针对组合导航系统在过程噪声统计特性不确定和观测值异常时鲁棒性差的问题，本方法将鲁棒性估计与自适应算法结合，提出了基于卡方检验的自适应鲁棒CKF组合导航算法(CTAR-CKF)。

1 CKF算法

对于INS/GPS组合导航非线性离散系统建立如下：

(1)

对于非线性系统(1)，CKF的计算如下：

步骤1 初始化

(2)

步骤2 时间更新

对误差协方差Pk-1/k-1进行Cholesky分解：

(3)

计算容积点：

(4)

式(4)中，i为容积点序号。

通过系统函数传播容积点：

(5)

计算状态一步预测和对应的协方差：

(6)

(7)

步骤3 量测更新

Cholesky因式分解：

(8)

计算新的容积点：

(9)

通过量测函数传播容积点：

Zi,k/k-1=h(Xi,k/k-1)

(10)

K时刻的观测预测值：

(11)

残差ηk和自相关协方差Pzz,k/k-1：

(12)

(13)

互相关协方差Pxz,k/k-1：

(14)

步骤4 状态更新

K时刻滤波增益：

(15)

K时刻状态估计值：

(16)

K时刻状态误差协方差估计值：

(17)

2 基于卡方检验的自适应鲁棒CKF

2.1 卡尔曼滤波的正交性原理

在卡尔曼滤波理论中，当系统的模型确定且不存在模型误差时，卡尔曼滤波输出的残差序列为零均值的高斯白噪声，故这些残差处处正交，这就是卡尔曼滤波的正交性[13]。

根据卡尔曼滤波的正交性原理，残差马氏距离平方服从卡方分布，造如下卡方检验统计量：

(18)

因此可以通过计算残差序列的马氏距离的平方，进而利用假设检验原理对系统进行评估，如果系统状态突变或者存在观测异常时，会使得残差的马氏距离的平方不再是自由度为n的卡方分布。

2.2 过程噪声统计特性自适应估计

引入卡方检验，对系统进行评估并通过预设的模糊逻辑函数计算相应的过程噪声加权值，利用加权值求取过程噪声的估计值，从而达到自适应调整过程噪声统计特性的目的。

当系统出现状态突变时，过程噪声的统计特性的不确定性增加，会导致滤波器的状态估计值偏离系统的状态值。此时可通过对过程噪声统计协方差矩阵Qest的调整，调节滤波增益，减少过程噪声统计特性不确定性对滤波结果的影响。当过程噪声统计特性不确定性增加时，Mk1会增加，反之，Mk1则会减小。则可通过设定检测阈值的上界mmax和下界mmin来表示系统不确定性的程度，mmax和mmin分别取置信概率为99%和90%时所对应的卡方检验值，查表可知道mmax=11.35，mmin=6.25。定义如下模糊逻辑函数，用于计算Qest：

(19)

Mk1为k时刻第一次卡方检验的检验值，初始时刻Q0，Qmin取正常情况下过程噪声统计特性的协方差矩阵，Qmax一般取Qmin的整数倍。设定协方差矩阵的上界Qmax和下界Qmin是为了在特大机动的情况下不会因为过大的卡方检验值而得到过大的过程噪声协方差矩阵，导致滤波发散[15]。

2.3 量测噪声增强因子的自适应估计

当系统出现观测异常时，卡尔曼滤波的残差序列幅值增大，此时量测的权值需要降低，可以在式(13)中引入增强因子φk[16]：

(20)

并使下式成立：

(21)

利用公式，利用牛顿迭代求取φk，即：

(22)

(23)

(24)

这就保证了在测量粗差出现时增大了Rk。

2.4 算法实现

考虑非线性离散系统(1)，CTAR-CKF的主要计算过程如下：

步骤1 根据式(2)初始化状态估计值和协方差。

步骤2 时间更新。根据式(3)—式(7)计算状态预测值及其协方差阵。

步骤3 量测更新。根据式(8)—式(12)，式(20)，式(14)计算自相关协方差和互相关协方差。

步骤4 状态更新。根据式(16)，式(17)计算状态估计及其协方差阵。

步骤5 利用卡方检验对系统模型进行评估，利用式(19)更新过程噪声方差估计值，若检验值小于6.25，则直接跳到步骤7。

步骤6 再次利用卡方检验，判断是否出现量测异常，并利用式(24)更新噪声增强因子。

步骤7 令xk-1=xk/k，Pk-1=Pk/k，k=k+1,返回步骤2，进行下一次循环迭代。

3 仿真分析

3.1 仿真设置

SINS/GPS组合导航系统采用SINS系统为主系统解算得到姿态，速度，位置信息，并通过GPS系统的信息定期对SINS进行反馈修正。建立的状态方程为：

xk=f(xk-1)+ωk-1

(25)

式(25)中，f(·)为非线性函数，由SINS的力学编排和误差方程得到。ωk-1为系统过程噪声向量，xk为15维的状态向量：

xk=[δrx,δry,δrz,δvx,δvy,δvz,δψx,δψy,δψz,εx,εy,εz,Δx,Δy,Δz]

(26)

式(26)中，δrx,δry,δrz为三轴位置误差，δvx,δvy,δvz为三轴速度误差，δψx,δψy,δψz为三轴姿态角误差，εx,εy,εz为三轴陀螺仪零漂，Δx,Δy,Δz三轴加速度计零偏。加速度计和陀螺仪的误差建模为一阶马尔科夫过程。量测方程建立如下：

(27)

ZSINS为SINS解算得到的三轴速度和三轴位，ZGPS为GPS输出的三轴速度和三轴位置。仿真中GPS采样周期为1 s，SINS的采样周期为0.02 s，滤波周期为1 s,仿真其他参数设置如表1所示。

载体在0～77 s进行了滑跑、加速拉起、爬高、改平飞行，75～200 s为平直飞行，200～220 s进行了一次大机动转弯，221～440 s为平直飞行，441～600 s进行了连续的机动转弯。飞行轨迹如图1所示。为了验证算法的鲁棒性和自适应性，在100～105 s和220～225 s加入了20 m的量测粗差。这样就存在如下三种噪声情形，条件1,在100～105 s只存在量测异常；条件2,210～215 s系统存在较大机动，系统过程噪声统计特性的不确定性增加且存在量测异常；条件3,441～600 s系统只存在较大机动。

表1 仿真参数

图1 飞机飞行轨迹Fig.1 Trace of the airplane

3.2 仿真结果及分析

图2为本算法的两次卡方检验结果。第一次检验时由于未引入自适应滤波和鲁棒滤波相当于标准CKF下的检验值，对比两图可知，图1在系统存在大机动和量测粗差时，具有较大的卡方检验值，因此CKF的卡方检验值能检测到系统的不准确程度；而图2中将本文算法引入过程噪声自适应修正后，由大机动引起的系统模型不准确程度得到了相应的改善，对应处的卡方检验值均在置信概率为99%时所对应的卡方检验值之下，只有在存在量测粗差的时刻系统模型不准确度仍较大。因此将置信概率为99%时所对应的卡方检验值作为第二次检验的阈值，对于大于此阈值的时刻应继续进行鲁棒滤波以消除粗差的影响。

图2 卡方检验值Fig.2 Value of Chi-square test

分别采用CKF，鲁棒CKF和本文所提出CTAR-CKF对组合导航系统进行仿真，结果如图3—图5所示。三种算法在直线飞行时都具有较好的滤波精度，但CKF算法在载体存在较大机动和观测值异常时滤波精度会有明显的下降，鲁棒CKF虽能较好的处理观测值异常的情况，却在系统强非线性机动时，过程噪声统的计特性的变化会使鲁棒CKF的滤波精度会较低，本文所提出的CTAR-CKF算法不仅可以分别处理系统存在量测粗差和强非线性运动时的情况，且在过程噪声统计特性变化和存在观测值异常时仍能保持较好的滤波精度。

图3 姿态误差Fig.3 Estimation error of attitude

图4 速度误差Fig.4 Estimation error of velocity

图5 位置误差Fig.5 Estimation error of position

对比三种算法的均方根误差(RMSE)，如表2—表4所示。在三种噪声条件下，本方法都能保持着较好的滤波精度，在条件1时，CKF算法的三维位置误差达19.6 m，算法CTAR-CKF和鲁棒CKF的位置精度仍能保持在5.8 m和6.4 m，CKF三维速度误差精度达0.35 m/s，算法CTAR-CKF和鲁棒CKF速度精度分别为0.215 m/s和0.206 m/s。在条件2时，CKF的位置精度为18.3 m，算法鲁棒CKF的位置精度下降，达到了11.9 m但算法CTAR-CKF仍能保持在4.6 m。算法CKF的速度精度为0.66 m/s，算法CTAR-CKF和鲁棒CKF速度精度分别为0.49 m/s和0.15 m/s。在条件3时，算法CKF和鲁棒CKF的位置精度分别达到了21.2 m和15.3 m,速度误差精度分别达到了0.48 m/s和0.44 m/s，算法CTAR-CKF的位置误差和速度误差精度仍能保持在4.5 m和0.21 m/s。

表2 条件1中不同算法的精度比较(RMSE)

表3 条件2中不同算法的精度比较(RMSE)

表4 条件3中不同算法的精度比较(RMSE)

表5 解算时间

由表5可知，本文算法运行时间略长于CKF算，相较与鲁棒CKF在解算时间上并没有明显的增加，但当系统存在较大机动，系统不确定性增加时，本文提出的算法比鲁棒CKF却具有更好的滤波精度。

综上所述，本文算法能有效抑制过程噪声变化和观测异常对系统的影响，在不同噪声条件下能得到优于CKF和鲁棒CKF算法的滤波精度，同时计算量同鲁棒CKF相似，略高于CKF，但相对于CKF提高的系统精度，增加的计算量是可以接受的。

4 结论

本文提出了基于卡方检验的自适应鲁棒CKF组合导航算法。该算法首先第一次引入卡方检验对系统进行评估，并根据卡方检验值和预设的模糊逻辑函数对过程噪声统计特性进行调节；然后，再次利用卡方检验对观测异常进行判断，并通过增强因子对量测噪声的统计特性进行调节。仿真结果表明，算法能有效抑制过程噪声变化和观测异常对系统的影响，且在噪声正常和观测值充足的情况下也同样适用。