组合导航中的噪声在线估计无迹卡尔曼滤波*

崔　丹，吴　楠，孟凡坤

（信息工程大学 导航与空天目标工程学院，河南 郑州 450001）

组合导航中的噪声在线估计无迹卡尔曼滤波*

崔丹，吴楠，孟凡坤

（信息工程大学 导航与空天目标工程学院，河南 郑州 450001）

为了解决大多数实际应用中噪声统计特性未知且时变引起的滤波精度下降甚至发散的问题，在常规无迹卡尔曼滤波算法基础上，提出了一种基于新息的噪声在线估计无迹卡尔曼滤波算法 。该算法利用观测新息的协方差，可同时在线估计系统噪声协方差Q和观测噪声协方差R。将改 进的滤波算法应用 到INS/GPS松耦合组合导航系统，仿真结果表明，该算法对时变噪声统计特性表现出了良好的适应性，滤波器更稳定，定位精度更高。

噪声统计特性；无迹卡尔曼滤波；噪声在线估计；新息；协方差

0　引 言

为了解决强非线性条件下的估计问题，有学者提出了无迹卡尔曼滤波（Unscented Kalman Filter，UKF）算法。UKF算 法采用无迹变换（Unscented Transformation，UT）对一组精确选择的sigma点进行非线性变换，以估计系统状态的均值和协方差。由于不需 要对非线性系统进行线性化，且可以很容易地应用于非线性系统的状态估计，因此UKF方法在许多方面得到了广泛应用［1］。

在使用过程中，常规的UKF系统噪声协方差矩阵Q、观测噪声协方差矩阵R都是准确已知的。但是，在大多 数实际导航系统中，机动载体的噪声统计特性往往是未知且时变的。若假设的噪声统计特性与实际的噪声统计特性偏差较大，则容易导致滤波器发散。为了解决此问题，国内外研究人 员进行了一系列相关研究，提出了各种自适应卡尔曼滤波算法，如Sage-Huge自适应滤波算法、衰减记忆卡尔曼滤波算法、模糊自适应卡尔曼滤波算法［2］等。Sage-H uge［3］自适应滤波算法是由经典Kalman滤波算法和噪声统计估值器组成的次优无偏极大后验估计，可同时估计状态和噪声统计特性，但该算法需保证量测噪声协方差阵的正定性和系统噪声协方差阵的半正定性，否则滤波器容易发散［4］。Fagin和 Sorenson等人提出了衰减记忆滤波算法，利用衰减因子限制卡尔曼滤波器的记忆长度，增加当前量测值的权重来抑制滤波发散，但其滤波精度有所降低［5-7］。通常情况下，衰减记忆卡尔曼滤波采用一个大于1的固定常数作为衰减因子，若系统遇到突发干扰，单纯的衰减因子并不能准确调整所有的状态［8］。文献［9］将经典Kalman滤波算法与模糊逻辑方法相结合，通过设计的模糊推理系统在线实时调整量测噪声方差，但该算法需要预先对机动环境的噪声特性有一定的了解。当实际噪声较强时，噪声特性很难在比值1附近划定隶属度函数［10-11］。

针对时变噪声统计特性造成的滤波发散现象，本文研究了一种改进的无迹卡尔曼滤波算法。通过观测新息的统计特性，利用最后N个观测新息的协方差，同时计算系统噪声协方差Q和观测噪声协方差R，对噪声统计特性进行在线估计，从而达到抑制滤波器发散的目的。在INS/GPS组合导航系统中，采用改进的UKF算法对飞行目标误差状态进行估计，并对其进行仿真。仿真结果表明，改进的无迹卡尔曼滤波器能有效抑制滤波发散，且比传统的卡尔曼滤波器的滤波精度高，对实际工程应用具有一定的参考价值。

1　INS/GPS松耦合组合导航模型

惯性导航系统（Inertial Navigation System，INS）既能提供有效的姿态、角速率和加速度测量，又能输出位置和速度，而且其自主性强，不易受环境、载体机动和无线电干扰，短期精度高且稳定性强。但是，惯导测量存在误差随时间迅速积累的问题。导航精度随时间而发散，不能单独长时间工作，必须不断加以校准。全球定位系统（Global Positioning Systems，GPS）可以提供长时间内误差只有几米的高精度位置输出，其导航定位授时服务可达到全球性、全天候，并且精度高、误差不随时间积累。但是，GPS信号容易受到遮挡或干扰［12］、导航结果的数据更新率低、没有姿态信息的输出，因此不能依赖GPS来提供连续导航参数。

可以看出，INS和GPS的优缺点是互补的。因此，可将二者组合，结合两种技术的优势，实现在两个系统间取长补短的目的，从而提供连续、高带宽、长时和短时精度都较高的、完整的导航参数，提高整个导航系统的鲁棒性。常见的组合导航结构有4种：松耦合、紧耦合、深耦合、超紧耦合。其中，松耦合组合导航是基于速度与位置的组合，其结构简单灵活，易于实现。本文基于松耦合组合导航系统进行研究，选取误差状态进行估计，包含姿态、位置、速度、加速度计零偏及陀螺仪零偏共15个参数。在松耦合组合导航中，不用估计GPS状态，因此状态估计依赖于INS。GPS速度误差的变化可以推出INS的姿态误差和仪表误差，因此使用GPS的位置和速度作为观测参数。系统的观测值包括位置量测差值和速度量测差值，其维数为6。

目标实际运动系统模型通常为非线性，INS/GPS组合导航系统滤波的状态方程与观测方程可写为：

其中：

2　噪声在线估计无迹卡尔曼滤波算法

2.1基于新息的噪声在线估计

常规无迹卡尔曼滤波和标准卡尔曼滤波都属于线性最小方差估计。算法都基于模型，区别在于最佳增益矩阵的求取方法不同［13］。计算最佳增益时，UKF根据观测量和被估计量的协方差阵来确定最佳增益阵。在UKF算法中，状态向量经无迹变换将n维状态向量变换到2n维的并行状态向量，即sigma点。这些sigma点则根据系统方程和量测方程来计算状态估计和误差协方差阵。在导航系统中，UKF系统传播对姿态不确定度大的应用场合很有用；UKF的观测更新对发射器和接收器距离较近的应用场合比较有用。

常规的UKF算法中，噪声统计特性Q和R均为预先设定的高斯白噪声。然而，目标在实际运动过程中Q和R是实时变化的。为了解决此问题，本文利用观测新息对系统噪声协方差Q和观测噪声协方差R进行在线估计。

观测新息矢量包括GPS位置、速度与校正后的INS位置、速度的差值：

计算最后N个观测新息的协方差，N为自适应窗口，取值可变：

利用式（4）可得系统噪声协方差Q和观测噪声协方差R：

给定Q和R的初值后，将式（5）和式（6）分别代入状态估计协方差和观测新息协方差求解中，便能将在线估计的噪声统计特性引入滤波的估计过程中。

下面将改进的UKF算法应用到INS/GPS组合导航系统中。图1给出了改进无迹卡尔曼滤波算法的计算流程框图。

图1　改进无迹卡尔曼滤波算法流程框

2.2基于噪声在线估计的无迹卡尔曼滤波算法

本文采用INS/GPS松 耦合组合导航系统。改进的自适应UKF使用GPS输出的位置和速度参数来估计INS误差，然后利用估计误差对INS导航参数进行校正。经过校正后的INS导航参数，即为组合导航输出。

基于新息的噪声在线估计UKF系统，传播流程的第一步是利用Cholesky求解误差协方差矩阵的均方根Sk：

每个sigma点通过系统模型实现传播：

于是，可计算状态估计及其误差协方差的一步预测：

将式（12）代入式（8）产生新的sigma点，由新的样点可得到观测新息的一步预测。新的sigma点和对应的平均观测新息为：

计算观测新息的协方差：

最后，基于新息的自适应UKF算法增益矩阵、状态向量更新和误差协方差更新为：

3　仿真实验及分析

3.1仿真条件

为了验证本文提出的基于新息的噪声在线估计无迹卡尔曼滤波算法的性能以及其在INS/GPS松耦合组合导航系统中的应用效果，本文在MATLAB开发环境下对改进无迹卡尔曼滤波算法在INS/GPS松耦合组合导航中的应用进行仿真实验。目标初始位置为：大地纬度50.337 4°，经度-3.306 8°，大地高度10 000 m。初始速度为：北向速度-120 m/s，东向速度160 m/s，地向速度0 m/s。初始姿态为：倾斜角-14.036 3°，仰角0°，航向角126.871 5°。目标飞行时间为290 s，传播间隔为0.5 s。在仿真过程中，飞行目标的机载过程包括加速、减速、下降、匀速飞行、转向以及爬升。

在噪声统计特性不确定的情况下，基于新息的自适应无迹卡尔曼滤波器初始参数设置为：

3.2仿真结果及分析

在INS/GPS松耦合组合导航系统中，采用改进的无迹卡尔曼滤波器和传统卡尔曼滤波器进行仿真对比，分别得到相应的位置、速度及姿态误差曲线。图2为飞行目标的运动轨迹，初始北向速度为120 m/s，东向速度为160 m/s，50 s时目标减速到200 m/s，83 s时目标开始做爬升运动，天向速度匀加速至20 m/s，116 s时爬升运动结束，目标保持200 m/s向东匀速运动。图3、图4、图5依次为改进滤波算法与卡尔曼滤波算法对目标位置误差、速度误差、姿态误差的估计曲线。

图2　目标的运动轨迹

图3　改进UKF与Kalman北向、东向、天向位置误差比较

图4　改进UKF与Kalman北向、东向、天向速度误差比较

图5　改进UKF与Kalman北向、东向、天向姿态误差比较

图3、图4、图5中，改进UKF算法对目标的位置误差、速度误差、姿态误差的状态估计结果明显比Kalman滤波算法的状态估计结果稳定。采用Kalman滤波算法的仿真曲线中，位置的北向、东向、天向误差分 别高达22.5 m、17.8 m、11.7 m。而采用本文改进滤波算法得到的北向、东向、天向位置误差最大值分别为2.3 m、6 m、5 m。可见，改进算法的定位精度更高。图4中，滤波器输出稳定后，北向和东向速度误差值有较明显改善，且天向速度误差值也有少许减小。目标运动状态在50 s、83 s、116 s时均有较明显变化，因此Kalman滤波器在50 s、83 s和116 s附近的速度误差有较大波动。而本文提出的改进算法能很好地克服这种由运动轨迹突变造成的影响，速度误差值接近于0且保持稳态。由图5可以看出，Kalman滤波器输出结果中，50 s时，目标倾斜角的突变使得绕北向的姿态误差急剧增大；83 s到116 s目标做爬升运动，导致天向姿态误差呈发散趋势。而本文改进滤波器输出的姿态误差值接近于0且没有明显波动，表现出了良好的平稳性。此外，改进滤波算法对北向姿态误差的改善最明显，东向姿态误差仅稍稍得到改善。对于天向姿态误差，Kalman滤波器的输出是发散的，而改进UKF滤波器的输出是持续稳定的。

通过对上述INS/GPS松耦合组合导航系统中飞行目标的北向、东向、天向位置、速度和姿态误差的比较，可以看出在系统噪声协方差Q和观测噪声协方差R不确定的情况下，本文提出的基于新息的自适应无迹卡尔曼滤波算法比传统的卡尔曼滤波算法稳定性更好、滤波精度更高。

4　结 语

使用常规卡尔曼滤波器对组合导航系统状态进行估计时，需先给定系统噪声和观测噪声的统计特性，但在实际物理过程中，无法确定噪声统计特性。针对此问题，本文提出了一种利用观测新息协方差对噪声统计特性在线估计的自适应无迹卡尔曼滤波算法。此外，描述INS/GPS松耦合组合导航系统模型，介绍改进算法对该系统状态估计的详细过程，对改进滤波算法、卡尔曼滤波算法进行仿真对比实验。仿真结果表明，基于新息的自适应无迹卡尔曼滤波算法克服了由噪声统计特性未知引起的滤波发散问题，且与常规卡尔曼滤波算法相比，改进滤波算法表现出了更好的稳定性，更高的滤波精度。

［1］ 何友，修建娟，关欣等.雷达数据处理及应用［M］.北京:电子工业出版社，2013. HE You，XIU Jian-juan，GUAN Xin，et al.Radar Data Processing with Applications［M］.Beijing:Electronic Industry Press，2013.

［2］ 高青伟，赵国荣，吴芳等.衰减记忆自适应滤波在惯导系统传递对准中的应用［J］.系统工程与电子技术，2010，32（12）:2648-2651. GAO Qing-wei，ZHAO Guo-rong，WU Fang，et al.Application of Adaptive Fading Filter Technique in Transfer Alignment of Inertial Navigation Systems［J］.Systems Engineering and Electro nics，2010，32（12）:2648-2651.

［3］ Sage A P，Husa G W.Adptive Filtering with Unknown Prior Statistics［C］.In Proceedings of Joint Automatic Control Conference，1969:760-769.

［4］ 屈新芬，李世玲，曾超等.渐消记忆自适应滤波在传递对准中的应用［J］.探测与控制学报，2014，36（05）:43-46. QU Xin-fen，LI Shi-ling，ZENG Chao，et al.Application of Adaptive Fading Memory Adaptive Filter in Transferring Alignment［J］.Journal of Detection & Control，2014，36（05）:43-46.

［5］ TIAN Chong，WANG Xing-liang，LU Yan-e.An Adaptive Filter Algorithm for Exponentially-Weighted Multiple Attenuated Memory［J］.Journal of Wuhan University of Technology，2011，33（11）:147-152.

［6］ Long Zhao，Jing Liu.An Improved Adaptive Filtering Algorithm with Applications in Integrated Navigation［C］. IEEE International Conference on Digital Manufacturing &Automation，2010:182-185.

［7］ 袁美桂，严玉国，庞春雷，等.改进的自适应Kalman滤波在GPS/SINS中的应用［J］.空军工程大学学报:自然科学版，2015，16（05）:65-69. YUAN Mei-gui，YAN Yu-guo，PANG Chun-lei，et al.The Application of Improved Adaptive Kalman Filter to GPS/ SINS［J］.Journal of Air Force Engineering University（Natural Science Edition），2015，16（05）:65-69.

［8］ 李杨，胡柏青.基于改进衰减记忆滤波的组合导航算法研究［J］.计算机仿真，2012，29（03）:117-157. LI Yang，HU Bai-qing.Research on Integrated Navigation Algorithm based on Improved Fading-memory Filter［J］. Computer Simulation，2012，29（03）:117-157.

［9］ 徐田来，游文虎，崔平远.基于模糊自适应滤波的INS/GPS组合导航系统算法研究［J］.宇航学报，2005，26（05）:571-575. XU Tian-lai，YOU Wen-hu，CUI Ping-yuan.Research on GPS/INS Integrated Navigation System based on Fuzzy Adaptive Kalman Filtering［J］.Journal of Astronautics，2005，26（05）:571-575.

［10］ 张迎春，李璟璟，吴丽娜等.模糊自适应无迹卡尔曼滤波方法用于天文导航［J］.哈尔滨工业大学学报，2012，44（01）:12-16. ZHANG Ying-chun，LI Jing-jing，WU Li-na，et al.Application of Fuzzy Adaptive Unscented Kalman Filter in Spacecraft Celestial Navigation［J］.Journal of Harbin Institute of Technology，2012，44（01）:12-16.

［11］ Enbo Shi，An Improved Real-time Adaptive Kalman Filter For Low-Cost Integrated GPS/INS Navigation［C］.IEEE International Conference on Measurement，Information and Control，2012:1093-1098.

［12］ 张一，马永奎，张中兆.现代卫星导航信号的发展及特性分析［J］.通信技术，2006，39（01）:28-34. ZHANG Yi，MA Yong-kui，ZHANG Zhong-zhao.Development and Analysis of Modernized Satellite Navigation Signal［J］. Communications technology，2006，39（01）:28-34.

［13］ 秦永远，张洪钺，汪叔华.卡尔曼滤波与组合导航原理［M］.西安:西北工业大学出版社，2015. QIN Yong-yuan，ZHANG Hong-yue，WANG Shu-hua. Theory of Kalman Filter and Integrated Navigation［M］. Xi’an:Northwestern Polytechnical University Press，2015.

崔 丹（1991—），女，硕士研究生，主要研究方向为空间目标信息处理与应用；

吴 楠（1984—），男，博士，讲师，主要研究方向为空间目标信息处理与应用；

孟凡坤（1963—），男，硕士，教授，主要研究方向为空间目标信息处理与应用。

Unscented Kalman Filter based on Online Noise Estimation in Integrated Navigation

CUI Dan， WU Nan， MENG Fan-kun
（Institute of Navigation and Aerospace Target Engineering， Information Engineering University， Zhengzhou Henan 450001， China）

In order to solve the problem of filtering-accuracy decline or even divergent caused by the unknown statistics and time-correlated variation of noise in the most practical applications， an innovationbased adaptive Unscented Kalman Filter algorithm is proposed. This algorithm could simultaneously estimate the system noise covariance Q and the measurement noise covariance R from the covariance of measurement innovation. For the purpose of simulation， the modified filtering algorithm is applied to the loosely-coupled INS/GPS in tegrated navigation system， and the simulation results indicate that the modified algorithm exhibits fairly good adaptability to the ti me-varied noise， and the filter is more stable， the positioning accuracy is even higher.

noise statistics； Unscented Kalman Filter； online noise estimation； innovation； covariance

TN713

A

1002-0802（2016）-10-1306-06

10.3969/j.issn.1002-0802.2016.10.008

2016-06-14；

2016-09-17

data:2016-06-14；Revised data:2016-09-17