徐晓苏，仲灵通

（1．微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2．东南大学仪器科学与工程学院，南京 210096）

Kalman滤波作为一种线性、无偏且误差方差最小的最优估计算法，广泛应用于组合导航领域，具有算法简单、易于工程实现的特点，但算法精度依赖准确的噪声统计特性[1]。在实际应用中，由于传感器量测环境通常复杂多变，导致量测噪声的统计特性难以准确获取且具有时变性。以惯导/多普勒（SINS/DVL）组合导航为例，多普勒测速仪的量测噪声统计特性会随着水下环境的变化而变化，单一固定的滤波器参数在系统长时间运行过程中必将会导致滤波精度的下降[2,3]。

Sage-Husa自适应滤波是一种常用的量测噪声统计特性估计方法，其在利用观测数据进行递推滤波的同时，通过噪声估计器实时估计和修正噪声的统计特性，从而改善滤波精度，但存在噪声统计特性估计非正定以及估计滞后的问题[3,4]。

交互式多模型滤波算法对于处理模型参数不确定情况下的估计问题有着其他方法所不具备的效果，但估计性能很大程度上依赖于模型集[5]。对于固定结构的多模型算法，为了能够覆盖到系统所有可能的模式，模型集要设计的足够大，但并不是所有系统模式都是完全已知的，若增大模型集，则会导致过度的模型竞争反而影响估计精度。另外交互式多模型算法的模型转移概率矩阵通常固定不变，难以准确快速地反映实际模型的切换[6]。

文献[7]采用Sage-Husa自适应滤波方法辅助交互式多模型算法，首先通过前者给出量测噪声统计特性的粗略值，再以此为中心对称地得到模型集，进行交互式多模型估计，但其滤波精度仍依赖Sage-Husa自适应滤波且计算量增大。

另外，因声学信号在水下会受到各种干扰，多普勒测速仪的量测数据不可避免会带有一些异常值（野值）[8,9]，且量测噪声多呈厚尾分布[10]，为抑制量测粗差对滤波精度的影响，通常采用一些鲁棒抗差滤波算法。文献[2]提出了一种混合交互式多模型滤波算法，主滤波器采用基于残差2χ检测的鲁棒滤波以提高算法抗差性。文献[11]则是构建基于马氏距离算法的调节因子来修正量测噪声方差阵。上述算法虽然一定程度抑制了野值干扰，但其仍需人为提前设计好模型集以覆盖实际噪声统计特性。

本文针对上述问题，改进传统交互式多模型算法，引入基于M估计的抗差Kalman滤波，提出了一种基于M估计的抗差自适应多模型组合导航算法。该算法突破传统交互式多模型算法定结构的限制，凭借模型集自适应调整策略，模型集能够自适应地朝着当前真实模型方向调整，以快速估计量测噪声统计特性；利用模型概率信息对模型转移概率矩阵进行实时修正，增强匹配模型的作用；通过基于M估计的抗差Kalman滤波算法，提高滤波抗差能力。以SINS/DVL组合导航为例，通过仿真和长江试验对本算法进行了验证，结果表明本算法有效降低了量测噪声统计特性时变及量测粗差对滤波精度的影响，提高了组合导航精度。

1 交互式多模型算法

交互式多模型算法首先需要设计一个含有多个模型的模型集来描述系统可能的模态，其中每一个模型对应一个滤波器，经输入交互后所有滤波器并行工作，根据某种假设规则，利用各滤波器中的残差信息以及残差协方差信息等，得到当前时刻各模型与真实系统模型匹配的概率，最后根据模型概率大小，加权融合各滤波器估计得到系统估计[12,13]。

假设随机线性离散系统的状态方程和量测方程分别为：

其中，Xk，Zk分别为k时刻的状态向量和量测向量，Φk,k-1为k-1时刻到k时刻的状态一步转移矩阵，Wk-1为k-1时刻的系统噪声向量，Gk,k-1为k-1时刻到k时刻的系统噪声输入矩阵，Hk为k时刻的量测矩阵，Vk为k时刻的量测噪声向量。

假设模型集为 M= ｛m1,m2,···,mn｝，模型个数为n，模型间转换遵循Markov过程，并设Markov模型转移概率矩阵为 Π= [πij]n×n，πij为模型mi到模型mj的Markov转移概率，Π通常由先验知识给定，满足行和等于1，且主对角占优。模型mi在k-1时刻为匹配模型的概率称为模型概率，记为根据各模型概率和各模型间Markov转移概率，可计算各模型混合概率为：

概括来说，交互式多模型滤波过程包括输入交互、模型滤波、模型概率更新和融合输出四个部分。

（1）输入交互

首先利用各模型混合概率计算各滤波器混合初始状态及其均方误差阵：

（2）模型滤波

根据各个模型采用的滤波算法，分别进行模型滤波，得到每个模型当前时刻的状态估计及其均方误差阵。这里以滤波器j标准Kalman滤波为例阐述过程。

计算状态一步预测及其均方误差阵：

计算新息：

计算滤波增益阵和新息方差阵：

计算状态估计及其均方误差阵：

（3）模型概率更新

由各滤波器的新息和新息方差阵，计算各模型的似然函数值：

采用贝叶斯假设检验方法进行模型概率更新：

（4）融合输出

最后，各滤波器估计值与对应模型概率加权融合，输出联合状态估计及其均方误差阵：

2 基于M估计的抗差Kalman滤波

M估计即广义极大似然估计，是一种常用抗差估计方法[14-16]，其不同于最小二乘估计采用残差平方和最小作为准则函数，而是定义了如式(16)的准则函数：

式中函数ρ(·)为一适当选择的连续凸函数，为k时刻新息的第i个元素，m为量测量的维数。为进一步优化上述准则函数的鲁棒性，常引入一个鲁棒尺度调节参数使新息标准化，则改写式(16)准则函数为：

其中MAD表示中位数绝对偏差，其定义为：

对式(17)相对于被估计量X求导并令其为零，则有：

以上即为M估计原理。因此，M估计在Kalman滤波中的应用可以表现为引入矩阵ωk作为新息的加权阵，即如式(22)所示。

这样便得到基于M估计的抗差Kalman滤波算法（简称MKF）。通过引入加权矩阵对新息进行限制修正，能够有效降低量测粗差对滤波精度的影响，增强滤波抗差能力。

选取的权函数不同，所得到的抗差效果也会存在差异，常用的权函数有：Huber权函数、丹麦法权函数和IGG系列法权函数等。

3 基于M估计的抗差自适应多模型滤波算法

本文在交互式多模型算法基础上，采用基于M估计的抗差Kalman滤波算法，提出模型集自适应和模型转移概率自适应策略，构建了一种基于M估计的抗差自适应多模型滤波算法，该算法采用三个滤波器的交互式多模型滤波结构，原理框图如图1所示。

图1 基于M估计的抗差自适应多模型组合导航算法原理框图Fig.1 Diagram of robust adaptive multiple model integrated navigation algorithmbased on M-estimation

3.1 模型集合自适应

针对量测噪声统计特性时变的问题，采用3个不同的量测噪声方差阵构建初始模型集分别对应时刻1滤波器1，2，3的量测噪声方差阵。

一次滤波结束后，模型概率经过更新，将当前时刻对应模型概率最大的量测噪声方差阵作为下一时刻量测噪声方差阵估计值：

其中，j等于模型概率矩阵kμ中对应最大值的位置序号，即k时刻对应模型概率最大的量测噪声方差阵，为k+1时刻量测噪声方差阵估计值。

因对应的模型概率最大，所以其与真实量测噪声统计特性最匹配，故将其作为k+1时刻量测噪声方差阵估计值。同时再以此估计值为中心，对称扩展得到新的量测噪声方差阵模型集

图2 模型集自适应调整示意图Fig.2 Diagram of model set adaptive adjustment

另外，为了避免模型集频繁重复调整，可以设定一概率调整阈值D，即当max(μk)＞D时，模型集才进行上述调整更新。同时，为了更加准确快速地自适应跟踪真实量测噪声方差阵，常数η设置的不宜过大或过小，可以选择设置一较大ηb和一较小ηs，并分别对应一较大bD和一较小Ds。因为当量测噪声突变时，滤波器1或3对应的模型概率会激增，如果常数η设置的过小，将会导致模型集调整的较慢，则此时常数η可以选择设置的较大一些，而当量测噪声变化较小时，模型概率变化幅度也会较小，此时常数η可以选择设置的较小一些，以更准确地跟踪量测噪声变化。

本算法突破了定结构多模型的限制，模型集能够自适应地朝着当前真实模型方向变化，从而快速跟踪估计量测噪声方差阵，具有很好的自适应性。与Sage-Husa自适应滤波辅助的多模型方法相比，本算法计算量更小，不需在多模型交互的同时再构建自适应滤波器，同时不存在噪声统计特性估计非正定的问题。

3.2 抗差Kalman滤波

针对量测存在粗差的问题，采用基于M估计的抗差Kalman滤波算法，权函数采用如式(25)所示的具有淘汰段的IGGⅢ权函数。其中，即k时刻滤波器j中标准化新息的第i个元素，k0和k1为抗差临界值，通常k0取1.0～1.5，k1取2.5～8。IGGⅢ权函数是一种三段式权函数，拥有正常段、可疑段和淘汰段，能够充分根据观测数据的实际情况对粗差进行分类，并根据不同粗差进行合适处理，是一种较好的自适应抗差估计方案。

在抗差Kalman滤波过程中，当求得式(7)新息后，将新息标准化：

然后构建加权矩阵对新息进行加权处理：

其中，为k时刻滤波器j的修正新息序列。

采用修正新息序列进行后续滤波计算以及模型概率更新中的各模型的似然函数值计算：

采用抗差Kalman滤波算法能够有效降低量测粗差对滤波精度的影响，增强多模型滤波算法的抗差性能。

3.3 模型转移概率自适应

针对标准交互式多模型算法采用固定模型转移概率矩阵难以准确快速反映实际模型切换的问题，采用模型概率信息对模型转移概率矩阵进行实时自适应修正。

模型概率反映了当前时刻该模型与真实系统模型的匹配程度，模型概率越大，表明该模型与真实模型越匹配。而模型概率变化率可以很好反映该模型与真实模型匹配程度的变化以及模型切换的变化趋势，若模型概率变化率大于零，则表明该模型与真实模型更加匹配，同时模型集将朝着该模型方向移动调整，另外，模型集中其他模型向该模型转移的概率也应增大，从而使得在输入交互中该模型所占的比重增大，反之其他模型向该模型转移的概率应减小[17]。

其中，Πk为k时刻模型转移概率矩阵，分别为k时刻和k-1时刻模型mi到模型mj的模型转移概率，为模型转移概率修正因子，为模型概率变化率，，λ为转移速度调节系数，λ＞0，以更加灵活地调节模型转移概率变化。

修正后的模型转移概率需归一化处理：

通过上述利用模型概率信息对模型转移概率矩阵进行实时修正的方法，可以有效增强匹配模型的作用，削弱非匹配模型的影响。

4 SINS/DVL组合导航系统建模

建立SINS/DVL组合导航系统状态方程：

其中，X为状态向量，φx，φy，zφ分别为SINS各轴向失准角，分别为东、北、天三个方向速度误差，δL，δλ，δh分别表示纬度误差、经度误差和高度误差，εx，εy，εz分别表示各轴向陀螺常值漂移，∇x，∇y，∇z分别表示各轴向加速度计零偏，δK表示DVL标度因子误差，W为系统噪声向量，F为系统矩阵，F阵可参考文献[16]。采用SINS载体系速度与DVL测得的经安装偏角转换后的载体系速度的差值作为量测量，建立SINS/DVL组合导航系统量测方程：

5 仿真验证

1）量测噪声自适应跟踪性能验证

为验证本文算法的量测噪声自适应跟踪性能，以SINS/DVL组合导航系统为例，分别采用标准Kalman滤波（简称KF）、Sage-Husa自适应滤波（简称Sage-Husa）以及文献[7]所提出的自适应交互式多模型滤波（简称AIMM）与本文算法进行MTALAB仿真对比。

载体仿真运动轨迹如图3所示。载体设定的初始位置为东经118 °，北纬32 °，仿真时间为4000 s。惯性测量单元（IMU）的输出频率为200 Hz，陀螺仪的常值零偏为0.02°/h，随机游走为，加速度计的常值零偏为100μg，随机游走为50μg。DVL的输出频率为1 Hz，底跟踪测速精度为0.4%±1cm/s，并假设DVL数据已标定补偿。组合计算周期为1 s。

图3 载体运动轨迹仿真图Fig.3 Simulation diagram of vehicle trajectory

DVL的量测噪声设置如下：

其中，R0=diag｛( 0.01m/s)2,(0.01m/s)2,(0.01m/s)2｝。图4给出了DVL量测输出仿真图。

图4 DVL量测输出仿真图Fig.4 Simulation diagram of DVL measurement output

本文算法参数设置方面，其中初始模型集为｛0.92R0,R0,1.12R0｝，初始模型转移概率矩阵：

初始模型概率矩阵μk=[1/3 1/3 1/3]，转移速度调节系数λ=0.5，概率调整阈值Dd=0.98，Ds=0.6，常数ηd=1.1，ηs=1.05。另外三种算法的初始量测噪声方差阵均为R0，Sage-Husa和AIMM算法中的遗忘因子b=0.98。其余参数四种算法设置完全一致。

图5给出了量测噪声均方差估计曲线。从图中可以看出，本文算法与Sage-Husa算法相比，量测噪声跟踪估计速度更快，估计滞后小，同时量测噪声方差阵模型集可以有效覆盖设定值。

图5 量测噪声均方差估计曲线图Fig.5 Curve of mean square deviation of measured noise

图6和图7分别为水平方向速度误差和位置误差的仿真曲线。从图中可以看出，由于KF算法中量测噪声方差阵为定值，当量测噪声发生变化时，KF算法会立即发散，而另外三种算法由于能够自适应调整量测噪声方差阵，都不同程度抑制了量测噪声变化对滤波结果的影响。

图6 水平向速度误差仿真曲线图Fig.6 Simulation curves of horizontal velocity error

图7 水平方向位置误差仿真曲线图Fig.7 Simulation curves of horizontal position error

上述结果表明本文算法能够改善复杂环境下因量测噪声统计特性未知多变引起的滤波精度下降的问题。

2）抗差性能验证

为验证本文算法的抗差性能，在上一部分载体仿真运动轨迹基础上，假设量测噪声服从式(38)所示的混合高斯分布，并在2000s后，每间隔400s添加一突变野值，图8给出了DVL有野值的量测输出仿真图。

图8 DVL量测输出仿真图Fig.8 Simulation diagram of DVL measurement

由于KF、Sage-Husa和AIMM均无抗差性能，则该部分采用KF和MKF与本文算法进行对比。其中，抗差临界值k0=1.5，k1= 5。

图9和图10分别为水平方向速度误差和位置误差仿真曲线。从图中可以看出，KF算法受量测粗差的影响最大，特别是当量测出现突变野值时，滤波出现了明显发散，而本文算法和MKF对量测粗差均有一定的抑制效果，同时因为本文算法的量测噪声自适应性，所以导航精度略优于MKF。

图9 水平方向速度误差仿真曲线图Fig.9 Simulation curvesof horizontal velocity error

图10 水平方向位置误差仿真曲线图Fig.10 Simulation curvesof horizontal position error

6 长江试验验证

为进一步验证本文算法的有效性和优越性，以SINS/DVL组合导航系统为例，采用实验室某次长江试验的数据进行验证。图11给出了该次长江试验的试验船及相关试验设备图。相关试验设备主要有GPS-RTK接收机、惯性测量单元、DVL和导航计算机等。其中，姿态、速度和位置等导航参数参考值由GPS-RTK与IMU松组合来提供。IMU包括三轴光纤陀螺仪和三轴石英加速度计，输出频率为200Hz，其中光纤陀螺零偏稳定性优于0.02°/h，随机游走优于石英加速度计常值零偏优于100μg，随机游走优于50μg。DVL为600 kHz的四波束JANUS配置，其底跟踪测速精度为0.4%±0.5cm/s，输出频率为2Hz。整个试验持续超过2500s，试验过程中GPS信号良好。本次试验验证选取了惯导对准结束后的约2000s数据进行算法验证，图12为试验船的运动轨迹。

图11 试验船及相关试验设备图Fig.11 Diagram of test shipand related equipment

图12 试验船运动轨迹图Fig.12 Trajectory of test ship

图13为DVL的实际量测输出图，从图中可以明显地看出DVL实际量测中存在较多的异常值，在组合导航前需将这些异常值剔除或采用具有抗差性能的滤波算法。

图13 DVL实际量测输出图Fig.13 Diagram of DVL actual measurement

同样，将本文算法与KF、MKF、Sage-Husa和AIMM算法进行对比。组合导航前已完成相关传感器误差标定。其中，本文算法和MKF采用的量测为DVL未经野值剔除的量测输出，其它三种算法采用的量测是经野值剔除后的DVL量测。

参数设置方面，初始模型集为｛0 .92R0,R0,1.12R0｝，R0=diag｛(0.02m/s)2,(0.02m/s)2,(0.02m/s)2｝，概率调整阈值Dd=0.9，Ds= 0.5，抗差临界值k0=1，k1=2.5。组合计算周期为0.5 s。其余参数设置同仿真验证部分一致。

图14和图15分别为水平方向速度误差和位置误差曲线。表1给出了上述四种算法输出结果的均方根误差（RMSE）。

图14 水平方向速度误差曲线Fig.14 Curves of horizontal velocity error

图15 水平方向位置误差曲线Fig.15 Curves of horizontal position error

表1 水平方向速度误差、位置误差的均方根误差Tab.1 RMSE of horizontal velocity error and position error

从上述图表中可以看出，本文算法要明显优于其他四种算法。其中，相比KF算法，东、北向速度误差的均方根误差分别下降了58%、50%，东、北向位置误差的均方根误差分别下降了46%、63%；相比MKF算法，东、北向速度误差的均方根误差分别下降了49%、27%，东、北向位置误差的均方根误差分别下降了24%、62%；相比Sage-Husa算法，东、北向速度误差的均方根误差分别下降了54%、39%，东、北向位置误差的均方根误差分别下降了44%、55%；相比AIMM算法，东、北向速度误差的均方根误差分别下降了44%、36%，东、北向位置误差的均方根误差分别下降了41%、53%。

图16给出了五种算法的水平定位误差曲线对比图。其中，KF算法的最大水平定位误差为52.92 m，MKF算法的最大水平定位误差为48.59 m；Sage-Husa算法的最大水平定位误差为48.19 m；AIMM算法的最大水平定位误差为45.32 m；本文算法的最大水平定位误差为24.48 m，相比上述其他四种算法定位精度分别提升了约53.7%、49.6%、49.2%和45.9%。

图16 水平定位误差曲线Fig.16 Curves of horizontal positioning error

长江试验的结果表明本算法有效降低了量测粗差对滤波精度的影响，改善了复杂环境下因量测噪声统计特性未知多变引起的滤波精度下降的问题，提高了组合定位精度。

7 结 论

本文针对复杂环境下因量测噪声统计特性时变及量测粗差而引起的组合导航精度下降的问题，提出了一种基于M估计的抗差自适应多模型滤波算法。该算法突破了传统交互式多模型算法定结构的限制，凭借模型集自适应调整策略，模型集能够自适应地朝着当前真实模型方向调整，以快速估计量测噪声统计特性；利用模型概率信息对模型转移概率矩阵进行实时修正；融合基于M估计的抗差Kalman滤波算法，提高了滤波抗差性能。以SINS/DVL组合导航系统为例，通过仿真和长江试验对本算法进行了验证，结果表明该算法有效降低了量测噪声统计特性时变及量测粗差对滤波精度的影响，提高了组合定位精度。