M估计的强跟踪SVD-UKF算法在组合导航中的应用

池传国，黄国勇，孙 磊

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

组合导航系统中，观测数据异常是导致滤波系统发散的原因之一。卫星导航系统中系统输出数据不可避免会出现野值数据或粗差，与惯性导航系统数据融合滤波后仍不可避免异常数据对系统输出的影响。卫星导航数据异常通常分为3类：单粗差、多粗差与缓慢增长误差。卫星导航系统中，缓慢增长误差常见于接收机钟差问题，在松组合方式的组合导航系统中无法消除。卫星导航数据的单粗差和多粗差异常会导致组合导航滤波系统鲁棒性降低[1]。

强跟踪SVD-UKF算法中，强跟踪滤波框架建立在新息正交的基础上，利用正常新息数据实现对实际状态的强跟踪，其并不能判断带有单粗差或多粗差的新息序列的健康程度。一旦异常数据污染了新息序列，强跟踪滤波框架强迫新息残差序列正交后导致多重渐消因子变化，引起估计结果的偏差，降低估计精度并可能导致滤波系统的发散。抗差估计是常用的解决卫星信号野值数据问题的手段之一[2]。当滤波系统中粗差的影响不可避免时，通过适当的手段来避免粗差对状态估计值的影响。抗差估计是通过等价权来影响新息序列，以降低新息序列在出现野值时造成对状态估计值的影响。将抗差估计引入强跟踪SVD-UKF算法，在算法进行量测方程更新前对新息序列预处理，剔除新息序列中存在的卫星野值，保证滤波系统的可靠性和鲁棒性[3]。

强跟踪SVD-UKF算法是基于新息序列来判断估计状态是否偏离，故新息序列的信息完整性是强跟踪滤波算法对系统建模及滤波精度保证的要求之一[4]。M估计是抗差估计的一种，其本质是通过等价权函数对新息序列进行加权，根据残差序列来确定新息序列权重。当回归残差较大时，降低新息序列中异常数据对状态估计值影响；当回归残差较小时，增大权重保证新息序列信息完整性。M估计的等价权函数方式有效提高新息数据的可利用率。因此，选择M估计方法用于抑制卫星信号野值问题，能够最大程度的提出卫星信号中的异常数据，并最低程度的影响新息数据的信息完整性，保证强跟踪SVD-UKF算法在新息正交时不受影响[5]。

1 M估计理论

1.1 等价权原理

选取状态向量X=[x1,x2,x3,…,xm]T，引入一组新息序列Z=[z1,z2,z3,…,zn]T，则相应的残差方程为

(1)

在最小二乘估计中，最优线性无偏估计的准则是残差平方和最小，即

(2)

最小二乘估计在新息异常时，新息值会直接影响估计结果的精度。M估计是基于最小二乘的稳健估计，通过加权新息序列来降低异常数据对估计过程的影响。如何选择加权因子直接影响M估计的估计精度。

构造观测加权矩阵B=diag[b1,b2,b3,…,bn]T，使得

(3)

则加权最小二乘估计为

(4)

针对M估计建立如式(5)所示的准则函数

(5)

式中f为适当选择的函数，对准则函数以X(k)求偏导得

(6)

令φ[e(k)i]/e(k)i=ωi，则有

(7)

将结果带入式中，可以得到基于M估计的最小二乘估计

(8)

当等价权矩阵为I时，基于M估计的最小二乘估计降阶到标准最小二乘，故实际上最小二乘估计是M估计的一种特殊形式。等价权矩阵的选择直接影响到M估计的精度，选择不同的等价权函数代替等价权矩阵，能够自适应处理新息数据，以更有效的利用新息数据。

1.2 等价权函数选择

等价权函数的选择是M估计理论的关键。等价权函数的选择需要结合应用对象来进行选择，以保证M估计的精度[6]。结合卫星导航数据的特点，等价权函数的选取目标是：

(1)新息数据在先验模型的正常噪声范围内，经过M估计后仍然保证其有效性；

(2)新息数据超出先验模型的正常噪声范围程度较小，经过M估计后，新息数据包含的有效信息不被受到破坏性影响；

(3)新息数据超出先验模型的正常噪声范围程度较大，经过M估计后，新息数据不会较大程度影响滤波系统的状态估计值。

根据等价权函数的选取目标，可以将等价权分为3类：(1)正常区，不影响新息序列；(2)调整区，通过权因子调整新息序列；(3)拒绝区，剔除新息数据。

根据等价权区划分，选择合适的等价权函数，保证新息数据在正常区能够不影响后续状态估计，在调整区提高新息数据的可利用率，在拒绝区实现对污染数据的抗差[7]。由此来选择等价权函数，解决卫星信号野值问题。

常用的等价权函数有IGG法、丹麦法、Huber法等。根据上面等价权函数的选取原则以及卫星信号数据的特点，选择IGG法作为本文M估计的等价权函数。IGG法相对等价权区将权函数分为3段，在正常区不对新息序列产生任何影响，即等价权矩阵为I，不影响后续估计；在调整区，对新息序列进行降权处理，保证新息数据信息的部分可用；在拒绝区，抛弃不可用新息数据避免估计受到粗差影响[8]。

(9)

其中，c0由系统新息数据统计确定。

1.3 基于M估计的强跟踪SVD-UKF算法

强跟踪SVD-UKF算法由于其新息正交性原理在新息数据误差较大的情况易导致滤波发散。当卫星信号出现野值时，强跟踪滤波框架不可避免的会受到野值的影响。将M估计应用到强跟踪SVD-UKF算法中，在强跟踪SVD-UKF算法迭代前对新息数据进行M估计，通过选择恰当的等价权函数，判断新息数据是否为野值并处理，有效解决了卫星信号野值对强跟踪SVD-UKF算法干扰。等价权函数的选择充分考虑卫星信号数据的基本模型以及强跟踪SVD-UKF算法的内部结构，选择IGG法等价权函数在数据异常时降低数据对强跟踪SVD-UKF算法的干扰，在数据正常时，保证新息数据对于强跟踪SVD-UKF算法的可利用率[9]。

基于M估计的强跟踪SVD-UKF算法在强跟踪SVD-UKF算法框架下，利用M估计对新息数据预处理，数据处理后送入强跟踪SVD-UKF算法得到结果[10]。

将基于M估计的强跟踪SVD-UKF算法应用到组合导航系统中，选择系统状态方程状态向量的选择如下

X(t)=[L,λ,h,VE,VN,VU,φE,φN,φU,εbx,εby,εbz]

(10)

式中,L,λ,h为位置；φE,φN,φU为INS输出的姿态角；VE,VN,VU为速度；εbx,εby,εbz为陀螺零偏。系统的量测向量选择如下

Z=[LB,λB,hB,VEB,VNB,VUB]

(11)

基于M估计的强跟踪SVD-UKF算法的基本流程如下：

(1)初始化。状态向量X以及协方差矩阵P的初值为

(12)

(2)构造Sigma点集

(13)

(3)时间更新

(14)

(15)

(16)

Yi=f(Xi);i=0,1,…,2n

(17)

(4)新息序列M估计处理

(18)

(19)

(20)

(5)多重渐消因子更新

(21)

Ck,i=tr[Nk]/tr[Mii,k]

(22)

(23)

(24)

(25)

(6)量测更新

(26)

(27)

(28)

由上可知，利用M估计理论，对异常新息数据进行“筛选”，保留有用新息，剔除有害新息，提供可靠有效的新息数据输入强跟踪SVD-UKF算法。经过M估计处理后的新息数据对于强跟踪SVD-UKF算法的意义在于滤除卫星信号野值，保留新息有效信息，避免强跟踪SVD-UKF算法因模型不精确导致估计精度的降低[11]。

2 仿真验证

为了验证本文提出算法的有效性，以下通过仿真实验对基于M估计的强跟踪SVD-UKF算法进行验证，并与强跟踪SVD-UKF算法进行对比验证。

假设飞机做机动飞行，飞行轨迹中含有爬升、变速、平飞和转弯等各种飞行状态。飞机的初始位置为东经107.002°，北纬29.498°，高度300 m；初始速度为50 m/s，方向正北。其中，陀螺的常值漂移为0.1 deg/h，加速度计常值误差1.0g～4g，陀螺一阶马尔可夫过程相关时间为3 600 s，加速度一阶马尔可夫过程相关时间为1 800 s。BDS水平位置误差均方根10 m，高度误差均方根20 m，速度误差均方根0.1 m/s。INS初始水平位置误差50 m，高度误差为100 m；初始速度误差0.5 m/s。BDS采样周期1 s，INS采样周期0.02 s，滤波周期1 s，仿真时间1 000 s。

两组算法系统状态向量与量测向量的选择与强跟踪SVD-UKF算法仿真实验相同，在以上仿真模型中，卫星导航位置数据加入异常粗差后进行对比验证算法效果。

(1)卫星单粗差数据异常对比。在仿真模型中加入5组水平位置异常数据，加入时间分别为第200 s、第400 s、第600 s、第800 s、第1 000 s。5组数据分别为：-1 000 m、-1 000 m、-1 000 m；-800 m、-800 m、-800 m；1 000 m、1 000 m、1 000 m；800 m、800 m、800 m；500 m、500 m、500 m。仿真结果如图1和图2所示。

图1 两种算法位置误差比较

图2 两种算法速度误差比较

对比两组算法仿真结果可知，基于M估计的强跟踪SVD-UKF滤波算法在卫星加入异常数据后仍能够不受异常数据干扰，滤波系统鲁棒性增强[12]。其中，强跟踪SVD-UKF滤波算法在遇到卫星野值后无法克服野值干扰，在加入异常数据的时间点均出现较大误差。在遇到异常新息数据后，强跟踪SVD-UKF滤波算法无法判断数据是否异常。由于强跟踪滤波框架的新息正交性，多重渐消因子影响协方差矩阵使系统后验状态估计值倾向于新息数据，使得滤波系统输出估计产生较大误差。由于多重渐消因子利用历史残差数据统计，异常数据对多重渐消因子影响一直存在。故仿真结果中，异常数据对强跟踪SVD-UKF滤波算法的影响随着多重渐消因子慢慢遗忘历史数据渐渐消失。基于M估计的强跟踪SVD-UKF滤波算法在遇到异常数据时，通过M估计辨识出异常后，等价权因子降低了异常数据的影响。在本次仿真中，5组异常数据经过M估计后等价权因子均降为0，使得异常数据并未对滤波系统造成干扰，提高了滤波系统鲁棒性[13]。

(2)卫星多粗差数据异常对比。在仿真模型中第45 s时加入持续100 s均值为0、强度为50的白噪声干扰。仿真结果如图3和图4所示。

图3 两种算法位置误差比较

图4 两种算法速度误差比较

对比两组算法仿真结果可知，基于M估计的强跟踪SVD-UKF滤波算法在卫星加入持续异常数据仍

能保证滤波系统不受干扰。而强跟踪SVD-UKF滤波算法在新息数据持续异常的时间段内亦产生较大误差。基于M估计的强跟踪SVD-UKF滤波算法异常数据残差较大时，等价权因子权值为0，剔除异常干扰；在异常数据残差在调整区时，等价权因子权值根据先验统计降低新息权值，减少异常数据对系统产生的影响[14]。基于M估计的强跟踪SVD-UKF滤波算法在多粗差情况下依然提高了系统鲁棒性[15]。

3 结束语

本文提出一种基于M估计的强跟踪SVD-UKF算法。该算法利用M估计理论，采用等价权因子对异常新息数据进行“筛选”，最大程度保留有效新息，剔除有害新息，避免由卫星信号野值引起的粗差对强跟踪SVD-UKF算法的鲁棒性影响。将该算法应用于组合导航系统仿真实验，在仿真模型中加入单粗差、多粗差数据与强跟踪SVD-UKF算法进行验证，仿真结果表明，基于M估计的强跟踪SVD-UKF算法有效提高了系统在新息数据异常时的鲁棒性，证明了本文方法的可行性和有效性。