胡 彬，杨新民，王胜红

（1.南京理工大学 江苏 南京 210094；2.淮海工业集团 山西 长治 046000）

基于降阶卡尔曼滤波算法在组合导航上的应用

胡 彬1，杨新民1，王胜红2

（1.南京理工大学 江苏 南京 210094；2.淮海工业集团 山西 长治 046000）

本文在简单介绍了卡尔曼滤波基本原理基础上，基于分段线性系统理论以及SOM方法对系统状态可观性和可观度的分析，采用降阶卡尔曼滤波算法，合理剔除不可观或观测度较低的状态因子，对高阶系统进行降阶设计。采用Visual Studio 2010语言开发环境，对常规卡尔曼滤波和降阶卡尔曼滤波进行了算法仿真，通过对前后误差波形的分析比较，结果表明降阶后的滤波系统继承了传统卡尔曼滤波高精度的优点，同时，算法阶数的降低，显著减少了导航计算机的计算负担，实时性得到了显著增强，更易于数字化实现，具有重要实际意义。

惯性导航；组合导航；卡尔曼滤波；数字仿真

工程中实性是个必须考虑的问题，尤其是在组合导航系统中，要求系统具有高精度、高实时性的机动能力。在导航计算中，应用卡尔曼滤波算法对数据处理时，由于采用数学迭代计算，计算时间由状态维数n决定，每一步迭代的计算量都与n3正比[1]。显然21阶误差状态给导航计算机带来了很大的计算负担，工程实现困难，难以满足组合导航高实时性的要求，在设计组合导航计算机速度和复杂度方面都不允许。

由于SINS/GPS系统卡尔曼滤波的初始误差协方差很容易满足正定性条件，而且系统的状态转移矩阵又是满秩的，因此系统完全可控，滤波的有效性就取决于系统的可观测行性。关于降阶滤波模型的研究，前人给出了很多研究方法，例如Model方法，Lyapunow函数方法，Perturbation方法等等[2]。文献[2]中提出了一种时变动态系统可观测性矩阵的奇异值分解分析方法，文献[3]又进行了改进，本文是应用分段线性系统可观性理论和SOM分析法[4]，对系统状态可观测性和可观测度的分析，抛开不可观测或观测度较低的系统状态因子，从而得到一个只有12阶的状态变量降阶模型。采用Visual Studio 2010语言开发环境设计编写了常规卡尔曼滤波和降阶滤波的组合导航程序，模拟设计飞行轨迹，最后通过计算机仿真验证了降阶后的模型仍能满足组合导航高精度的要求，并且实时性得到了显著的增强。

1 组合导航滤波结构和滤波算法

1.1 常规卡尔曼滤波算法

卡尔曼滤波是一种线性最小方差估计，它能够从一系列不完全且包含噪声的量测值中估计动态系统的状态，用于实时融合动态的低层次冗余多传感器数据，是实现组合导航系统的核心技术，由于采用数学迭代的运算方式，非常适合计算机数据运算。主要有如下特点[5-6]：

1）卡尔曼滤波的估计准则是估计的均方误差最小，即

2）卡尔曼滤波是无偏估计，即估计误差的均值为零。

3）卡尔曼滤波具有连续型和离散型两类算法。离散型算法可直接在数字计算机上实现，在计算上一般采取递推形式，离散系统的数学模型可以用下式表示:

其中，X是n维系统状态向量；Z为m维量测向量；φ为n×n维系统矩阵；Γ为n×r维系统噪声矩阵；H为m×n维量测矩阵；卡尔曼滤波要求{Wk}和{Vk}是互不相关且均值为零的白噪声序列[7]。

离散型卡尔曼滤波方程组如下：

1.2 降解卡尔曼滤波算法

在设计组合导航系统卡尔曼滤波器的过程中，必须保证滤波模型的可控性和可观性，这样设计出的滤波器才是一致稳定的，滤波结果的可靠性才能得到保证[8]。文献[2-3]总结并给出了组合导航系统同样可以采用分段线性系统理论以及采用SOM分析方法进行系统的可观测性分析的结论，并给出了一种适合于分析全程系统状态的可观测度方法。下面对基于奇异值的分解的系统状态可观测度分析方法做简要阐述。

定理：对于任意一个m×n阶矩阵A，若Rank（A）=r＜max（m，n），则存在两个正交矩阵 Um×m和Vn×n，使

其中，σ1≥σ2≥…≥σr≥0称为矩阵 A 的奇异值，V2和V1分别是 N（A）和 N（A）┴的标准正交基，N（A）为矩阵 A 的零空间，N（A）┴为 N（A）的正交补空间[9-10]。

对于 SOM 矩阵，将可观测矩阵 Qs（r）i=[Q1Q2…Q]T进行奇异值分解：

其中：

U=[u1，u2，…，um]，V=[v1，v2，…，vm]均为正交矩阵。

上式表明，Yj为 X0在[σ1v1，σ2v2，…，σrvr]张成的子空间上的投影变换，所以以需要r个测量值才能唯一的确定 Xj0，如果 σr＞0，则表明 Rank（Qs（r）j）=n，即系统是随机可观测的[11]，此时：

当存在 σi+1=σi+2=…=σr=0 时 Rank（Qs（r）j）＜n，即系统是不完全随机可观测的，Xj0的某些状态是不能从观测量Yj中估计出来的。此时，可利用奇异值分解法构造出可观测子空间及不可观测子空间的标准正交基，并由此可以分析出哪些状态是可以估计的，哪些状态是不可以估计的[12]。

由上式可知，Xj0是由r个列向量相加得到的。对于Xj0的某一分量，各向量中对应分量的绝对值最大的那个分量，包含了最多的的信息，对于的贡献最大，而此列向量中包含相应的因子，此奇异值σi越大，其倒数越小，则测量值σi对的变换越敏感，也就是说，的变换可以由测量值yi中更明显的反映出来，所以，次奇异值在一定程序上反映了状态分量的可观测程序。奇异值σi越大，的可观性越好[13]。

由于计算中存在误差，用奇异值分解法计算的奇异值时，理论上为零的奇异值可能不为零，而是一个很小的数。此时需根据计算机的精度选取一个界限值ζ，对于小于ζ的奇异值均认为是零奇异值，大于ζ的便认为是非零的奇异值。非零奇异值的个数就是可观测矩阵的秩。由以上叙述可知，基于奇异值的系统可观测性分析方法的优点是，在确定系统可观测性的同时，也可以确定系统可观测状态的可观测度[14]。

2 数字仿真及分析

2.1 飞行轨迹仿真的设定

为了验证组合导航系统性能，设计了一组飞行轨迹：设计飞机的初始位置为：北纬32.05°，东经118.766 7°，高度 0m；飞机的初始速度为 0m/s，航向120°，飞行任务有上升、加速、下降、左拐、右拐、静止等，飞行时间为3 700s。具体飞行轨迹如图1所示。

图1 飞行轨迹图

2.2 常规卡尔曼滤波仿真分析

采用位置、速度组合模式，常规卡尔曼滤波仿真误差曲线如图2、图3、图4所示。

图2 常规卡尔曼滤波组合的位置误差曲线

图3 常规卡尔曼滤波组合的速度误差曲线

图4 常规卡尔曼滤波组合的姿态角误差曲线

2.3 降解卡尔曼滤波仿真分析

基于分段线性系统理论及采用SOM分析方法对松耦合SINS/GPS系统进行分段，求得系统可观测性矩阵的秩为11，系统存在4个完全不可观测状态。15阶组合导航滤波器中4个完全不可观测的系统状态分别是航向角误差φz、y轴上的陀螺随机漂移εby，加速度计x轴上和z轴上的零位误差由于航向角为导航参数，因此不能剔除，故将其余3个不可观测的状态变量得到12阶的简化滤波方案，滤波效果如图5～图7所示。

图5 降阶卡尔曼滤波组合的位置误差曲线

图6 降阶卡尔曼滤波组合的速度误差曲线

图7 降阶卡尔曼滤波组合的姿态误差曲线

3 结论

由图2～4与图5～7对比看出：简化的滤波方案位置和速度误差与原滤波算法相当，姿态误差比原滤波方案滤波效果稍差。实质上降阶设计总是一种次优设计[15]，但由于精度波动的幅度在可以接受的范围，完全满足组合导航的精度要求。同时数学模型阶数的降低，使卡尔曼滤波计算时间减少了大约20％，简化的滤波方案大大减少了矩阵迭代给导航计算机的计算量，提高了组合导航系统实时性，因此简化的滤波方案具有较好的理论和实用价值。

[1]秦永元，汪叔华.卡尔曼滤波与组合导航原理[M].3版.西安：西北工业大学出版社，2014.

[2]胡小毛，刘飞.MINS/GPS全组合系统可观测性分析[J].中国惯性技术学报，2011，19（1）:1-4.

[3]周卫东，蔡佳楠.GPS/SINS超紧组合导航系统可观测性分析[J].北京航空航天大报，2013，39（9）:1-6.

[4]严涛，王跃钢，杨波.SINS/DR组合导航系统可观测性研究[J].现代防御技术，2012，40（3）:1-5.

[5]罗建军，马卫华.组合导航原理及应用[M].西安：西北工业大学出版社，2012.

[6]王新龙，李亚峰，纪新春.SINS/GPS组合导航技术[M].北京：北京航空航天大学出版社，2014.

[7]雷浩然.弹载SINS/GNSS组合导航系统研究[D].南京：南京理工大学，2014.

[8]朱莉.旋转式捷联惯导系统晃动基座自对准方法研究[D].北京：北京工业大学，2015.

[9]张贤达.矩阵分析与应用[M].北京：清华大学出版社，2013.

[10]廖安平.矩阵论[M].北京：清华大学出版社，2013.

[11]杨建峰.捷联惯导系统初始对准可观测性分析.[J]航天控制，2014，32（1）:1-4.

[12]高叶叶，李华滨.基于观测方程的捷联系统工具误差可观测性分析[J].航天控制，2014，32（3）:1-5.

[13]马朋，张福斌.基于条件数的多自主水下航行器协同定位系统可观测度分析[J].兵工学报，2015:36（1）:1-6.

[14]吴松羽，王可东.低精度INS/GPS组合导航大方位失准角初始对准方案[J].全球定位系统，2014：39（6）:1-5.

[15]袁克非.组合导航系统多源信息融合关键技术研究[D].哈尔滨：哈尔滨工程大学，2012.

The application of reduced order Kalman filter algorithm in integrated navigation

HU Bin1，YANG Xin-min1，WANG Sheng-hong2
（1.Nanjing University of Science and Technology，Nanjing 210094，China；2.Huai Industrial Group，Changzhi 046000，China）

This paper briefly introduce the basic principle of Kalman filter and analyze the observability and the degree of observability of system based on the piecewise linear system theory and the SOM method.Using the reduced order Kalman filter algorithm to reasonably eliminate the state factors which are not observable or low observable and make a reduced-order design for the high-end system.Using the reduced order Kalman filter algorithm to reasonably eliminate the state factors which are not observable or low observable and make a reduced-order design for the high-end system.At the same time，the reduction of the degree， which has the important practical significance， significantly reduces the computational burden of navigation computer and enhances the real-time so that the algorithm can be more easily realized digitally.

inertial navigation； integrated navigation； Kalman filter； digital simulation

TN961

：A

：1674－6236（2017）15-0098-04

2016-06-07稿件编号：201606058

胡 彬（1990—），男，江苏连云港人，硕士。研究方向：组合导航算法。