一种新型自适应模型预测组合滤波在在线标定中的应用

徐晓苏，吴 梅，张 涛，李 瑶，王捍兵

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

随着光纤陀螺技术的飞快发展，在航海领域光纤捷联惯导系统也逐步得到应用。目前通过工艺改进来提高光纤陀螺精度的成本越来越高，因此为提高惯性系统的导航精度，通过软件对捷联惯性器件误差进行在线建模及补偿具有现实意义。采用在线标定方法提高导航精度是当前导航领域研究的热点之一。

目前，在线标定一般采用卡尔曼滤波（Kalman Filter, KF）和扩展卡尔曼滤波（Extended Kalman Filter,EKF）算法进行参数估计。KF和EKF算法不但要求模型准确，而且要求系统噪声和量测噪声均为高斯白噪声，这种做法会降低状态估计精确度，影响滤波器的最优性，当系统是时变的且噪声统计信息未知时，甚至可能导致滤波估计发散[1]。高精度滤波算法对提高标定精度有重要影响，因此近年来许多学者展开了各种先进滤波方法在在线标定中的应用研究，其中：无迹卡尔曼滤波（Unscented Kalman Filter, UKF）是一种典型的非线性变换估计方法，滤波精度比KF和EKF算法优越，但需要计算大量统计样本，计算量过大，难以在工程实际中应用[2-3]；粒子滤波存在运算实时性不高、状态初始概率如何选取的问题，使其距离工程应用还有一定差距[4]；Sage-Husa自适应滤波是近几年发展起来的一种针对不确定系统过程噪声方差阵和量测噪声方差阵的经典算法[5-6]；模型预测滤波是一种基于最小模型误差准则的实时非线性滤波算法，能够实时对模型误差进行估计，进而修正系统模型，并且克服了将模型误差和系统噪声假设为高斯白噪声的局限[7-8]。

文献[9]提出了一种抗差自适应模型预测滤波算法，并将其应用于组合导航系统中，仿真结果表明该算法能够有效抑制系统模型误差及观测异常对导航解的影响。文献[10]提出了一种CCD星敏感器辅助陀螺的在线标定方法，采用乘性误差四元数构造量测量，运用了MPF+EKF的组合滤波算法，仿真结果表明，标定精度和速度明显优于传统方法，但是这篇文献仅研究了陀螺的相关误差参数，未涉及加速度计的相关误差参数。

本文在研究模型预测滤波和 Sage-Husa自适应滤波算法优缺点的基础上，提出一种新的MPF-AKF算法。该算法利用MPF算法实时估计系统模型误差，对系统模型进行修正，提高状态方程的准确度，克服了将模型误差假设为高斯白噪声的局限性，利用AKF算法减小量测噪声对在线标定结果的影响。这种组合滤波算法能够提高状态估计精度，降低状态变量维数，提高系统实时性。系统仿真实验进一步表明，这种方法在提高导航精度的同时满足了工程应用对实时性的要求。

1 模型预测滤波与自适应卡尔曼组合滤波

1.1 模型预测滤波原理分析

模型预测滤波算法的基本思想是通过比较量测输出与预测输出来估计相应的模型误差，进而修正滤波器状态，实现对真实状态量的估计。在非线性预测滤波器中，状态估计和输出估计由初始模型和待标定的模型误差给出：

离散的量测方程可由下式表示：

将输出方程(3)进行泰勒级数展开，如下式：

式中，

根据式(4)和式(8)可得到模型误差的求解公式：

1.2 Sage-Husa自适应卡尔曼滤波

带遗忘因子的Sage-Husa自适应卡尔曼滤波与传统卡尔曼滤波基本方程的差异主要在于，系统过程噪声和量测噪声的均值向量不为零。同时实时更新过程噪声方差阵和量测噪声方差阵，即：

在离散型卡尔曼滤波五个方程的基础上增加如下公式：

Sage-Husa自适应卡尔曼滤波算法虽然在计算滤波增益时考虑了实际的测量值，但是这将会导致计算量增大，工程实用性较差，所以在Sage-Husa自适应卡尔曼滤波算法的基础上进行简化，将系统过程噪声和量测噪声的均值向量设为零，将过程噪声方差阵设为常数，同时取消了的计算[10]。

1.3 MPF-AKF组合滤波流程

Sage-Husa自适应卡尔曼滤波结合模型预测滤波得到系统状态的一步预测状态量为：

最优滤波值为：

偏差为：

一步预测误差方差阵为：

新的信息为：

采用 Sage-Husa自适应卡尔曼滤波方法对量测噪声方差阵进行动态估计：

卡尔曼滤波增益阵为：

系统的估计误差方差阵为：

2 基于MPF-AKF的在线标定方法设计

2.1 惯性器件误差模型

由于安装误差在实验室标定后基本不随器件的老化发生变化，因此这里考虑的惯性器件误差包括器件常值误差和刻度因数误差。

建立包含常值误差和刻度因数误差的光纤陀螺输出误差模型：

建立包含常值误差和刻度因数误差的加速度计输出误差模型：

2.2 在线标定系统模型

2.2.1 系统状态方程

选取东北天地理坐标系为导航坐标系，选取SINS导航误差为状态量，包括速度误差、姿态误差和位置误差，建立状态方程为：

2.2.2 系统量测方程

以计程仪提供的水平速度和平台罗经提供的航向角为高精度外部信息源，将 SINS输出的水平速度、航向角与外部信息中的水平速度、航向角的差值作为量测量，建立量测方程，其中 SINS误差方程表示的是计算机中的导航坐标系与真实导航坐标系之间的平台误差角、、，所以建立航向角误差与平台误差角之间的关系为：

那么，系统量测方程可表示如下：

2.3 可观测性

根据文献[11]，以俯仰为例，进行可观测性说明。

对式(33)进行积分可得：

式(34)表明了陀螺误差在俯仰时的可观测性。

其他不同机动方式下惯性器件误差的可观测性如表1所示。

表1 惯性器件误差在不同机动方式的可观测性Tab.1 Observability of IMU errors in different maneuvers

3 仿真验证及分析

首先设计载体的行驶轨迹，尽量模拟真实运动环境，综合设计了静止、匀加速、匀速、上抬、俯仰、转弯时伴有横滚等多种机动方式。假设初始位置为东经118.78，北纬32.06，初始速度为0 m/s，初始航向角、俯仰角和横滚角均为0(随机设定)。利用 Matlab仿真生成行驶轨迹，如图1所示。

基于图1所示轨迹，对本文中所研究的在线标定方法进行仿真验证。设光纤陀螺的常值误差为0.1 (°)/h，随机游走为0.001(°)/h1/2，刻度因数误差为 100×10-6；加速度常值误差为 50 μg，随机游走为，刻度因数误差为 100×10-6；系统的速度量测噪声为0.1 m/s，姿态量测噪声为0.1°。仿真结果如图2~3和表2~3所示。

图2 陀螺仪标定结果Fig.2 Calibration results of gyroscopes

图3 加速度计标定结果Fig.3 Calibration results of accelerometers

由图 2~3和表 2~3可以看出，采用MPF-AKF组合滤波方法标定后，陀螺常值误差在0.001 (°)/h级别上，加速度计的常值误差小于0.4μg，陀螺的刻度因数误差小于6×10-6，加速度计的刻度因数误差小于5×10-6，标定结果理想，且均收敛于设定值。与传统EKF滤波相比，采用MPF-AKF组合滤波使得收敛速度更快，收敛精度更高，稳定性更好。

表2 陀螺误差模型参数估计结果Tab.2 Parameter estimation results of gyro error model

表3 加速度计误差模型参数估计结果Tab.3 Parameter estimation results of accelerometer error model

根据已设定的轨迹设置不同的外部条件进行仿真分析。对照组中系统的速度量测噪声为1 m/s，姿态量测噪声为1°，仿真结果如图4~5所示。

由图2~5可以看出，相较于传统的EKF滤波方法，速度和姿态量测噪声较大时，采用MPF-AKF组合滤波方法标定，所有IMU误差的标定值均与设置值相差较小，标定精度和收敛速度受外部信息精度影响较小。

图4 对照组陀螺仪标定结果Fig.4 Calibration results of contrasted gyroscopes

图5 对照组加速度计标定结果Fig.5 Calibration results of contrasted accelerometers

4 结 论

为了提高在线标定的精度，本文采用“速度+航向角”匹配方式，并采用MPF-AKF组合滤波算法进行在线标定的仿真实验。仿真结果表明，本文提出的MPF-AKF组合滤波算法在理论上是可行的，滤波稳定，能够估计出误差模型中的参数，满足标定精度要求，收敛速度和滤波精度均明显优于EKF，达到预期效果，同时MPF算法使系统维数降低，减少了计算时间。

（

）：

[1] Wu X J, Wang X L. A SINS/CNS deep integrated navigation method based on mathematiccal horizon reference[J]. Aircraft Engineer and Aerospace Technology, 2011, 29(8): 26-34.

[2] Moradi E. Design a low price and high performance sensor network using constrained-based unscented Kalman filter[C]//International Conference on Control Automation and Systems.2010: 124-129.

[3] 谷丰, 房建成. 非线性卡尔曼滤波方法的实验比较[J]. 决策与控制, 2014, 29(8):1388-1393.Gu F, Fang J C. Experimental investigation and comparison of nonlinear Kalman filters[J].Control and Decision, 2014, 29(8): 1388-1393.

[4] Sawabe W. Application of particle filter to autonomous navigation system for outdoor environment[C]//2008 SICE Annual Conference.2008: 93-96.

[5] Narasimhappa M, Rangababu R. A modified Sage-Husa adaptive Kalman filter for denoising Fiber Optic Gyroscope signal[C]//2012 Annual IEEE India Conference. 2012: 643-1283.

[6] 薛丽, 高社生. 自适应Sage-Husa粒子滤波及其在组合导航中的应用[J]. 中国惯性技术学报, 2013, 21(1): 84-88.Xue L, Gao S S. Adaptive Sage-Husa particle filtering and its application[J]. Journal of Chinese Inertial Technology, 2013, 21(1):84-88.

[7] Crassidid J L, Markley F L. Predictive filtering for nonlinear system[J]. Journal of Guidance Control and Dynamics, 1997, 20(3):566-572.

[8] Sanaz M, Javad P, Mohanmmad R J, et al.Nonlinear model predictive control of a pH neutralization process based on Wiener-Laguerre model[J]. Chemical Engineering Journal, 2009, 146(3): 328-337.

[9] 高社生, 宋飞彪, 姜微微. 抗差自适应模型预测滤波及其在组合导航中的应用[J]. 中国惯性技术学报, 2011, 19(6): 701-705.Gao S S, Song F B, Jiang W W. Robust adaptive model predictive filtering algorithm and application to integrated navigation[J].Journal of Chinese Inertial Technology, 2011,19(6): 701-705.

[10] 高伟, 林星辰. CCD星敏感器辅助光纤陀螺在线标定技术[J]. 系统工程与电子技术,2012, 34(8): 1680-1683.Gao W, Lin X C. Ecperimental investigation and comparison of nonlinear Kalman filters[J].Control and Decision, 2014, 29(8): 1388-1393.

[11] 孙伟强, 张礼伟. 激光陀螺惯组系统级标定方法[J]. 中国惯性技术学报, 2016, 24(1): 9-13.Sun W Q, Zhang L W. Systematic calibration method for RLG inertial measurement unit[J].Journal of Chinese Inertial Technology, 2016,24(1): 9-13.