一种新的自适应UKF算法及其在组合导航中的应用

胡高歌，高社生，赵 岩

（西北工业大学 自动化学院，西安 710072）

一种新的自适应UKF算法及其在组合导航中的应用

胡高歌，高社生，赵 岩

（西北工业大学 自动化学院，西安 710072）

为了提高组合导航系统的滤波精度，提出一种带噪声统计估计器的自适应UKF滤波算法。该算法根据协方差匹配原理，利用UKF滤波算法的残差序列与新息序列，在线估计、调整系统过程噪声和量测噪声的统计特性，提高UKF的自适应能力，克服了标准UKF在系统噪声统计未知或不准确情况下滤波精度下降甚至发散的问题。将提出的算法应用于SINS/BDS组合导航系统进行仿真验证，并与标准UKF和抗差UKF进行比较，结果表明，提出的自适应UKF得到的水平位置误差和天向误差分别在以内，滤波性能明显优于标准UKF与抗差UKF，提高了组合导航系统的解算精度。

SINS/BDS组合导航；Kalman滤波；自适应UKF；协方差匹配

在组合系统导航定位中，高精度的滤波算法对导航定位的解算精度有重要影响[1-2]。卡尔曼滤波是常用的导航滤波算法，它要求系统的状态方程与量测方程均为线性，而在捷联惯导(SINS)/北斗(BDS)组合导航系统的直接法滤波中，由于系统状态方程具有非线性形式，因此传统的卡尔曼滤波算法已不再适用[3-5]。为了解决非线性系统的滤波解算问题，人们提出了扩展卡尔曼滤波(Extended Kalman filter，EKF)，其基本思想是将非线性系统模型进行Taylor展开，略去二阶以上的高阶项，得到非线性系统的线性化模型，然后对线性化后的系统模型采用卡尔曼滤波算法进行计算，因此EKF是一种次优滤波。EKF虽然在非线性滤波中得到了广泛应用，但该算法仍然存在一些局限性。当动力学系统的非线性特性较强时，略去Taylor展开式的高阶项，将会引起较大的线性化误差，从而导致EKF的滤波误差增大甚至发散。此外，在EKF的滤波计算中，雅可比矩阵的求取复杂、计算量大，在实际应用中很难实施。

为了克服EKF的局限性，一些学者提出了Unscented卡尔曼滤波(Unscented Kalman filter，UKF)算法。UKF的核心思想是采用 Unscented变换(Unscented Transformation，UT)对非线性系统状态的后验概率密度函数进行近似[6]。与EKF相比，UKF以二阶以上精度近似高斯非线性系统状态的后验均值与方差，具有实现简单、滤波精度高以及收敛性好等优点。然而，UKF要求精确已知系统噪声的先验统计信息，未知或不准确的系统噪声统计会导致滤波精度下降甚至发散[7-8]。文献[9]在UKF的基础上提出了SPRHKF (Sigma point based receding horizon Kalman filter)，该算法对模型不确定性及瞬时干扰误差具有鲁棒性。然而，与 UKF比较SPRHKF收敛性较差。文献[10]通过最小化UKF残差协方差之和的方法，来求取噪声的统计特性，但是该算法计算量大，实时性较差。文献[6]通过引入比例因子调节滤波增益的方法，提出一种抗差UKF，该算法可以抑制量测异常对滤波精度的影响，然而该算法中比例因子的选取具有一定的经验性，不能从根本上克服UKF的上述缺陷。

为了提高组合导航系统的滤波精度，本文提出一种新的自适应UKF滤波算法。该算法根据协方差匹配原理，利用UKF滤波算法的残差序列与新息序列，在线估计、调整系统过程噪声和量测噪声的统计特性，提高UKF的自适应能力，克服了标准UKF在系统噪声统计未知或不准确情况下滤波精度下降甚至发散的问题。将提出的算法用于SINS/BDS组合导航系统进行仿真验证，结果表明，当系统噪声统计不准确时，提出的自适应UKF滤波性能明显优于标准UKF和抗差UKF，能显著提高组合导航的滤波精度。

1 一种新的自适应UKF滤波算法

考虑非线性离散系统：

式中，Qk为非负定矩阵，Rk为正定矩阵，δkj为Kronecker-δ函数。

1.1 标准UKF

UKF滤波算法通过UT变换近似计算系统状态向量的后验均值与方差，能够以较高的精度处理非线性系统的状态估计问题。对于非线性系统(1)，标准UKF滤波算法如下：

① 初始化

② Sigma点计算与时间更新

式中，a∈R为调节参数，通常设定为一个小的正值；为矩阵nPk-1均方根的第i列；权值选取为

③ 量测更新

1.2 提出的自适应UKF

针对标准 UKF在系统噪声统计未知或不准确情况下滤波精度下降甚至发散的问题，基于协方差匹配技术，利用 UKF滤波算法的残差序列与新息序列，设计一种带有系统噪声统计估计器的自适应UKF滤波算法。

1.2.1 系统噪声统计估计

定义新息与残差序列分别为：

将式(1)中的量测方程代入(14)得：

定义：

容易得知：

由于量测误差与状态估计误差具有正交性，因而新息协方差矩阵可计算如下：

由式(21)有：

利用新息向量有限的样本来计算其协方差矩阵：

其中N为估计窗口宽度。

将(23)代入(22)，得到系统量测噪声协方差矩阵的估计值：

由式(14)(15)可得：

对式(25)右乘其转置，有：

由新息序列与残差序列的不相关性，对(26)求期望可以得到：

与(23)类似，残差向量的协方差可近似为：

将式(7)(23)和(28)代入式(27)，可以得到如下矩阵方程：

1.2.2 自适应UKF算法的实现

对于非线性系统(1)，提出的自适应 UKF算法可以在非线性系统滤波解算的同时实现对系统噪声统计的估计，具体步骤如下：

① 初始化

② 时间更新

预测协方差为：

③ 量测更新

量测协方差矩阵为：

④ 更新系统噪声统计特性

2 SINS/BDS组合导航系统模型

在SINS/BDS组合导航系统的直接法滤波中，以捷联惯导力学编排方程和姿态误差方程作为组合导航系统的状态方程，以北斗接收机输出的速度和位置信息作为组合导航系统的量测。

2.1 组合导航系统状态方程

取导航坐标系为东北天(E-N-U)地理坐标系，SINS的力学编排方程和姿态误差方程分别为

式中，fE、fN、fU为东北天向的比力量测值；δVE、δVN、δL、δh在实际计算中可由SINS输出的速度、位置减去BDS对应的输出来近似获得；陀螺常值漂移εbE、εbN、εbU通常用随机常数来描述，即：

SINS/BDS组合导航系统的状态向量X(t)选取为。结合式(37)-(39)，可得SINS/BDS组合导航系统状态方程：

式中，f(·)为非线性函数，为系统噪声。对式(40)进行离散化处理，可得离散化的系统状态方程

2.2 系统量测方程

取BDS输出的载体速度信息vEBD、vNBD、vUBD和位置信息LBD、λBD、hBD作为量测Z，即

根据之前选取的系统状态向量X与量测Z，建立SINS/BDS组合导航系统的量测方程：

3 仿真实验及分析

将提出的自适应UKF算法应用于SINS/BDS组合导航系统中，并与标准UKF和抗差UKF进行仿真比较。设运载器的初始位置为东经109°，北纬34°，高度1200 m；初始速度300 m/s，方向正北。陀螺常值漂移为0.01(°)/h，随机游走；加速度机常值误差10-4g，随机游走。BDS的水平位置误差均方根为5m，高度误差均方根8 m，速度误差均方根0.05 m/s。SINS初始位置误差为10 m，初始速度误差0.2 m/s，初始姿态误差1′。SINS采样周期为0.01 s，BDS采样周期1 s，仿真时间为3600 s。

为了比较标准UKF、抗差UKF和提出的自适应UKF在系统噪声统计不准确时的滤波性能，三种滤波器中选取的噪声统计初值均与实际值存在较大偏差，具体为

标准UKF、抗差UKF和提出的自适应UKF计算得到的定位误差如图1～3所示。由仿真结果可以看到，在200～3600 s时间段内，采用标准UKF计算得到的水平位置误差在[-14.3 m，+13.7 m]内，天向误差在[-15.5 m，+16.2 m]内。抗差UKF能够在一定程度上抑制异常量测对滤波精度的影响，得到的定位精度相比标准UKF有所提高，分别为：水平位置误差[-9.8 m，+10.6 m]，天向误差[-12.7 m，+14.0 m]。提出的自适应UKF可以自适应地更新系统过程噪声与量测噪声的统计特性，因而得到的定位精度最高，其水平位置误差在[-6.2 m，+6.4 m]内，天向误差在[-9.8 m，+8.6 m]内。

表1给出了三种滤波算法200～3600 s时间段内定位误差的平均误差(mean error)与均方根误差(root mean square error，RMSE)。由表1可以看出，提出的自适应 UKF的平均误差与均方根误差均明显小于标准UKF与抗差UKF，表明当系统噪声统计不准确时，提出的自适应 UKF滤波算法可以有效的提高滤波精度，减小SINS/BDS组合导航直接法滤波的定位误差。

图1 标准UKF定位误差Fig.1 Position error of standard UKF

图2 抗差UKF定位误差Fig.2 Position error of robust UKF

图3 提出的自适应UKF定位误差Fig.3 Position error of the proposed adaptive UKF

表1 SINS/BDS组合导航系统误差比较Tab.1 Error comparison for SINS/BDS integration

4 结 论

为了提高组合导航系统的滤波精度，针对标准UKF滤波性能依赖于系统先验噪声统计的缺陷，提出一种新的自适应UKF滤波算法。该算法能够在线地估计和调整系统过程噪声与量测噪声的统计特性，克服了标准UKF在系统噪声统计未知或不准确情况下滤波精度下降甚至发散的问题，提高了UKF的自适应能力。

将该算法应用于 SINS/BDS组合导航系统进行仿真验证，结果表明，当系统噪声统计不准确时，提出的自适应UKF滤波性能明显优于标准UKF和抗差UKF，有效地提高的SINS/BDS组合导航系统的定位精度。

（References）：

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

[2]Gao S S，Zhong Y M，Li W.Robust adaptive filtering method for SINS/SAR integrated navigation system[J].Aerospace Science and Technology，2011，15(6):425-430.

[3] 汪秋婷，胡修林.基于 UKF 的新型北斗/SINS 组合系统直接法卡尔曼滤波[J].系统工程与电子技术，2010，32(2)：376-379.WANG Qiu-ting,HU Xiu-lin.Improved Kalman filtering algorithm for passive 2BD/ SINS integrated navigation system based on UKF[J].Systems Engineering and Electronics,2010,32(2):376-379.

[4]杨波，秦永元，柴艳.UKF 在 INS/GPS 直接法卡尔曼滤波中的应用[J].传感技术学报，2007，20(4)： 842-846.YANG Bo,QIN Yong-yuan,CHAI Yan.Application of UKF in Direct Method of Kalman Filter for INS/GPS[J].Chinese Journal of Sensors and Actuators,2007,20(4):842-846.

[5]Wang X L.Fast alignment and calibration algorithms for inertial navigation system[J].Aerospace Science and Technology,2009,13(4-5):204-209.

[6]Soken H E,Hajiyev C.Pico satellite attitude estimation via robust unscented Kalman filter in the presence of measurement faults[J].ISA Transactions,2010,49(3):249-256.

[7]Hu G G,Gao S S,Xue L.A novel adaptive unscented Kalman filter[C]// 2012 Third International Conference on Intelligent Control and Information Processing.IEEE,2012:497-502.

[8]薛丽，高社生，胡高歌.自适应 Sage-Husa 粒子滤波及其在组合导航中的应用[J].中国惯性技术学报，2013，21(1)：84-88.XUE Li,GAO She-sheng,HU Gao-ge.Adaptive Sage-Husa particle filtering and its application in integrated navigation[J].Journal of Chinese Inertial Technology,2013,21(1):84-88.

[9]Cho S Y,Choi W S,Robust positioning technique in low-cost DR/GPS for land navigation[J].IEEE Transactions on Instrumentation and Measurement,2006,55(4):1132-1142.

[10]Song Q,Han J D.An adaptive UKF algorithm for the state and parameter estimation of a mobile robot[J].Acta Automatica Sinica,2008,34(1):72-79.

Novel adaptive UKF and its application in integrated navigation

HU Gao-ge，GAO She-sheng，ZHAO Yan
(School of Automatics，Northwestern Polytechnical University，Xi’an 710072，China)

This paper presents a novel adaptive UKF with noise statistic estimator for the purpose of improving the filtering accuracy of integrated navigation systems.The covariance matching technique is employed in the proposed algorithm，and the innovation and residual sequences are used to estimate and adjust the covariance matrices of the process and measurement noises online.The proposed algorithm enhances the adaptive capability of the UKF and overcomes the limitation of the standard UKF，otherwise the filtering solution will be deteriorated or even divergent as the system noise statistics are unknown or inaccurate.The proposed algorithm is applied to the SINS/BDS integrated system for simulation in comparison with the standard UKF and robust UKF.The simulation results demonstrate that the horizontal position error and vertical error obtained by the proposed adaptive UKF are within [-6.2 m，+6.4 m]and[-9.8 m，+8.6 m]，respectively.The performance of the proposed algorithm is significantly superior to that of the standard UKF and robust UKF，leading to improved calculation precision of the integrated navigation system.

SINS/BDS integrated navigation;Kalman filtering;adaptive UKF;covariance matching

U666.1

A

1005-6734(2014)03-0357-05

10.13695/j.cnki.12-1222/o3.2014.03.015

2013-12-18；

2014-04-08

国家自然科学基金（61174193）；航天科技创新基金（CASC201102）

胡高歌（1988—），男，博士研究生，从事组合导航与非线性滤波算法研究。E-mail：hugaoge1111@126.com

联 系 人：高社生（1956—），男，教授，博士生导师，研究方向为导航、制导与控制。E-mail：gshshnpu@163.com