基于多重渐消因子的强跟踪SVDCKF组合导航算法

熊 鑫，黄国勇，王晓东

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

0 引 言

由捷联惯性导航系统(strapdown inertial navigation system,SINS)与北斗卫星导航系统(BeiDou navigation satellite system，BDS)组成的组合导航系统，在飞机和车辆载体的定位与姿态估计中有着广泛的应用[1]，目前扩展卡尔曼滤波(extended Kalman filter, EKF)算法和无迹卡尔曼滤波(unscented Kalman filter, UKF)算法是解决导航中状态估计的主要途径[2-3]，但存在一定的局限性。根据贝叶斯理论以及球面径向规则推导的容积卡尔曼滤波(cubature Kalman filter, CKF)算法是近年来提出的一种新型非线性滤波算法，CKF算法与UKF算法相比，都属于确定采样型滤波算法，但CKF算法估计精度更高，实时性更好[4-5]。然而，任何一种卡尔曼滤波器都要求精确已知系统噪声的先验统计信息，所以在载体状态出现突变，系统过程噪声统计特性存在不确定性的情况下会出现滤波精度下降甚至发散的现象[6-7]。有大量学者对这一问题进行了研究，并提出了基于变分贝叶斯的自适应卡尔曼滤波器[8-9]、基于最大相关熵的卡尔曼滤波器[10]、基于Huber-M估计的滤波器[11]以及基于交互多模的自适应滤波器[12-13]。变分贝叶斯自适应卡尔曼滤波器可以通过分解系统状态和噪声方差的联合后验分布来近似估计时变系统过程噪声方差的统计量，但该滤波器中使用的方差参数设计的动态模型是相对粗糙的，当过程噪声协方差频繁变化时，估计精度会降低；基于最大熵的卡尔曼滤波器能够通过最大化预测误差和残差的熵来抑制较大的系统过程噪声不确定性，但在实际应用中确定内核带宽大小的适当值非常困难，这可能会降低估计性能；基于Huber-M估计的卡尔曼滤波器通过使用M估计器的代价函数来构造加权矩阵，该加权矩阵可以重新调整先验状态估计协方差，从而抑制系统过程噪声不确定性，但其估计的精度有限；基于交互多模的自适应滤波器针对可能的噪声情况建立一组非线性模型来应对噪声的不确定性，但当噪声变化较大时则需要建立更多的模型来匹配，导致难以实现。

文献[14]根据新息正交原理提出的一种强跟踪滤波器，是目前最为可行的一种滤波器。与其他的滤波器相比，强跟踪滤波器(strong tracking filter, STF)在模型参数变化或系统发生突变时具有较强的跟踪能力，且对噪声及初值的统计特性有较低的敏感性。文献[15]将强跟踪理论与CKF算法框架相结合提出了一种强跟踪自适应CKF算法，该算法通过引入单重次渐消因子对预测误差协方差阵进行调整以实现对状态的强跟踪，该算法能一定程度上解决系统过程噪声统计特性不清的问题，但由于无法对每个滤波通道进行不同的调节，不能较好地应用于复杂的多变量系统。

针对BDS/INS组合导航系统在载体存在状态突变时，由于系统过程噪声统计特性的不确定性增加，导致标准CKF滤波精度下降的问题，本文以STF理论为基础，并通过与CKF框架的结合，提出了一种基于多重渐消因子的强跟踪奇异值分解的容积卡尔曼滤波(cubature Kalman filter using singular value decomposition，SVDCKF)组合导航算法。本算法采用卡方检验对系统状态进行检测，以判断是否引入多重渐消因子对预测协方差阵进行调整，并在系统状态的协方差矩阵的分解迭代用数值稳定性更强的SVD分解代替原来的Cholesky分解。最后通过INS/BDS仿真实验，对所提出算法的性能进行评估,并与CKF算法[4]，STCKF算法[15]进行比较，所得结果也表明了本文方法的有效性。

1 SVDCKF算法

对于标准CKF算法，在计算容积点时需对误差协方差矩阵进行Cholesky分解，在进行多次滤波后，由于计算机的截断误差容易导致误差协方差矩阵失去正定性和对称性，使得滤波发散。奇异值分解数值计算的鲁棒性更强，将奇异值分解代替原来的Cholesky分解对误差协方差矩阵进行分解迭代，能有效提高CKF算法的稳定性[16]。

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

(1)

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

步骤1初始化。

(2)

步骤2时间更新。

对Pk-1/k-1进行奇异值分解

(3)

(3)式中，Sk-1为对角矩阵，即

Sk-1=diag[S1,S2,S3,…,Sn]

计算容积点

i=1,2,3,…,2n

(4)

(4)式中：i为容积点序号；

通过系统函数传播容积点

(5)

计算一步预测状态值和对应的协方差值分别为

(6)

(7)

步骤3量测更新。

对Pk/k-1进行奇异值分解

(8)

计算新的容积点

(9)

通过量测函数传播容积点

Zi,k/k-1=HkXi,k/k-1

(10)

k时刻的观测预测值

(11)

残差ηk和自相关协方差Pzz,k/k-1分别为

(12)

(13)

互相关协方差

(14)

步骤4状态更新。

k时刻滤波增益

(15)

k时刻状态估计值

(16)

k时刻状态误差协方差估计值

(17)

2 基于多重渐消因子的强跟踪SVDCKF

相比于标准CKF，SVDCKF保证了误差协方差矩阵的对称性和正定性，提高了滤波迭代的稳定性，但SVDCKF本身仍具有局限性，当系统到达稳态时，滤波增益Kk将趋于最小值，如果系统状态发生突变，会引起残差的增大，但是滤波增益Kk并不会随着残差的增大而增大，仍保持最小值，这会使SVDCKF丧失对突变状态的跟踪能力[17]。为此，本文引入强跟踪滤波的思想来解决这个问题。

2.1 STF原理

强跟踪滤波通过对预测协方差Pk/k-1引入次优渐消因子λ，在线调整滤波增益Kk，强迫残差序列ηk保持正交，使得滤波器在状态突变时仍能保持对真实状态的强跟踪[18]。即有

(18)

(19)

2.2 渐消因子的计算

为了使不同的滤波器通道提供不同的渐消速率，用渐消因子矩阵代替单个的衰落因子。同时，为避免引起矩阵Pk/k-1不对称的问题，改进的预测协方差可表示为

(20)

当滤波器稳定时，残差序列是高斯白噪声，且有

(21)

(21)式中，Vk为滤波器实际输出的残差协方差阵，计算式为

(22)

(22)式中，0<ρ≤1为遗忘因子，通常取ρ=0.95。

定义参数为

(23)

(24)

在导航和定位应用中，量测矩阵通常可以表示为

Hk=[Sm×m0m×(n-m)]m×n

(25)

(25)式中：Sm×m=diag(S1,S2,…,Sm)。

(23)式可表示为

(26)

由(21)，(23)，(24)，(26)式可得

(27)

(27)式中，Nii，Jii为矩阵Nk，Jk中第i行，第i列对角线上的元素，由(27)式可得多重渐消因子的计算方法为

(28)

2.3 系统状态评估

卡尔曼滤波的正交性原理[19]：在卡尔曼滤波理论中，当系统不存在模型误差且滤波稳定时，新息残差序列为零均值的高斯白噪声，且残差保持正交。

根据卡尔曼滤波的正交性原理，残差马氏距离平方服从卡方分布[20]，即

(29)

本文通过卡方检验对系统进行评估，如果存在系统状态突变，残差马氏距离的平方将不再服从自由度为n的卡方分布，作出如下假设。

假设0γ0：系统状态正常。

假设1γ1：系统存在状态突变。

2.4 算法实现

考虑非线性离散系统(1)，MST-SVDCKF的主要计算过程如下。

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

步骤2时间更新。根据(3)—(6)式计算状态一步预测值。

步骤3利用卡方检验对系统模型进行评估，若存在状态异常，则根据(20)式更新状态协方差，否则根据(7)式计算。

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

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

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

3 仿真分析

3.1 仿真设置

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

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

(30)

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

xk=[δrx,δry,δrz,δvx,δvy,δvz,δψx,δψy,

δψz,εx,εy,εz,Δx,Δy,Δz]

(31)

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

Zk=[ZSINS-ZGPS]=Hxk+vk=

[I6×606×9]xk+vk

(32)

ZSINS=[rSINSx,rSINSy,rSINSz,vSINSx,vSINSy,vSINSz]

ZGPS=[rGPSx,rGPSy,rGPSz,vGPSx,vGPSy,vGPSz]

(32)式中：ZSINS为SINS解算得到的三轴速度和三轴位；ZGPS为BDS输出的三轴速度和三轴位置。仿真中BDS采样周期为1 s，SINS的采样周期为0.02 s，滤波周期为1 s，仿真其他参数设置如表1。

载体在0～77 s进行了滑跑、加速拉起、爬高、改平飞行；75～200 s一直为平直飞行；200～220 s进行了一次大机动转弯；221～440 s一直为平直飞行；441～600 s进行了连续机动包括3次转弯和改平飞行以及一次俯冲和改平飞行。飞行轨迹如图1。

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

表1 仿真参数

3.2 仿真结果及分析

分别采用CKF，STCKF和本文所提出MST-SVDCKF对INS/BDS组合导航系统进行仿真，结果如图3—图5。3种算法在直线飞行时都具有较好的滤波精度。在210～220 s和450～600 s期间，载体机动较大，CKF算法的滤波精度明显下降，CKF在载体大机动的情况下，三维速度误差和位置误差分别达到了0.62 m/s和11.25 m；STCKF在检查出状态突变后，可通过实时对状态协方差矩阵的调节，得到优于CKF的滤波精度，其三维速度误差和位置误差分别为0.30 m/s和5.86 m；相比于只能产生相同渐消速率的STCKF，由于MST-SVDCKF可以对不同的状态产生不同的渐消速率，所以，本文所提出的算法具有更强的调节能力，在载体机动较大时，三维速度误差和位置误差仍能保持在0.146 m/s和3.543 m。

图2 采用标准CKF和MST-SVDCKF时的卡方检验统计量Fig.2 Chi-square test value as standard CKF and MST-SVDCKF is used

表2 精度和解算时间比较(RMSE)

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

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

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

表2给出了不同算法在210～220 s，450～600 s的东、北、天的均方误差。可以看出，本文所提出的算法具有最高的滤波精度，在噪声不确定性增加时，仍能保持对真实状态的强跟踪。对比CKF，STCKF，MST-SVDCKF的算法运行时间，虽然本算法的平均解算时间略长，但仍然保持在一个较低的水平，能够满足实际现场应用的实时性要求。

4 总 结

本文提出了一种基于多重渐消因子的强跟踪SVDCKF算法。该算法首先引入SVD分解代替原来的Cholesky分解，解决了协方差矩阵可能非正定的问题，提高了数值计算的稳定性，然后通过卡方检验对系统状态进行评估，当系统出现状态突变时，采用多重渐消因子对代替单一渐消因子对预测状态协方差阵进行调节，使得不同滤波通道具有不同的渐消能力，调节能力更强，可以更好实现对多变量系统状态的强跟踪。将此算法用于SINS/BDS组合导航系统的仿真实验中，结果表明，本文提出的MST-SVDCKF能对系统状态进行判断，并在系统噪声不确定性增加时，自适应地调节预测状态协方差阵。对比CKF和传统的STCKF，本算法调节能力更强，滤波精度更高。