赵洪松，缪玲娟，邵海俊

（1. 北京理工大学 自动化学院，北京 100081；2. 中国人民解放军94032部队，甘肃 武威 733003）

二阶段H∞滤波在捷联惯导/里程计组合导航系统中的应用

赵洪松1,2，缪玲娟1，邵海俊1

（1. 北京理工大学 自动化学院，北京 100081；2. 中国人民解放军94032部队，甘肃 武威 733003）

针对一类包含未知常值偏置的线性系统的状态估计问题，比如捷联惯导/里程计组合导航系统，由于扩展未知常值偏置为系统状态，导致系统状态维数比较高。当应用 H∞滤波进行估计时，就会引起滤波算法的计算量大大增加，进而会影响其估计性能。为了解决这个问题，将 H∞滤波问题转化为Krein空间下的求解某种二次型的最小值问题，而求解该最小值可以使用Kalman滤波来解决。因此，通过采用二阶段Kalman滤波理论，在Krein空间下建立了二阶段H∞滤波算法，将H∞滤波中的高维矩阵计算解耦成并行的低维矩阵计算，有效地解决了高维矩阵计算所带来的数值计算问题，降低了计算量，并且该算法有利于硬件并行运算设计，为进一步提高算法执行速度提供了可能。通过在捷联惯导/里程计组合导航系统中的仿真实验，验证了算法的有效性。

H∞滤波；二阶段Kalman滤波；状态估计；Krein空间；Riccati方程

系统模型准确和随机干扰信号统计特性精确已知是标准Kalman滤波有效应用的基础[1]。而实际系统往往存在着模型不确定性和随机干扰信号统计特性不能够准确已知的情况，这些是导致标准Kalman滤波性能下降甚至引起滤波发散的主要因素[1-3]。针对实际系统中存在的模型以及外界干扰不确定性，H∞滤波使得从干扰输入到滤波误差输出的H∞范数最小化[4-5]。H∞滤波与标准Kalman滤波不同，其对干扰信号的统计特性不做要求，只要求其为能量有界信号[4-7]。因此，在一些随机干扰信号统计特性未知的情况下，与标准Kalman滤波相比，H∞滤波具有更好的适用性。

H∞滤波在算法结构上与标准Kalman滤波十分相似。一些学者将H∞滤波与标准Kalman滤波在Krein空间下联系起来，将H∞滤波问题转化为求Krein空间下二次型的最小值问题，而此最小值恰好通过Krein空间下的标准kalman滤波计算得到[8-10]。因此，H∞滤波与标准Kalman滤波存在相同的问题，即高状态维数会大大增加滤波算法的计算量。当H∞滤波用于高维状态系统的状态估计时，由于滤波计算量大，滤波实时性会降低，滤波性能也会受到不同程度的影响。

二阶段Kalmna滤波（two-stage Kalman filter, TKF）是用于解决由于扩展系统未知常值偏置为状态而导致Kalman滤波计算量增大的问题[11]，后来被推广到系统偏置为未知随机偏置的情况[12]。TKF通过将标准Kalman滤波解耦分成两个低维的滤波器（即 bias-free estimator, BFE 和 bias estimator, BE），来分别估计系统状态和系统未知偏置。这样就将标准Kalman滤波中的高维矩阵的计算变成了一系列低维矩阵的计算，解决了高维矩阵所引起的计算困难问题；同时TKF中的两个滤波器可以并行执行，因此其适用于硬件并行运算设计，进一步提高算法的运行速度[11]。

针对一类包含未知常值偏置的线性系统的状态估计问题，在Krein空间下通过应用TKF算法的原理，对普通H∞滤波进行解耦，分解成两个低维状态的滤波过程，构成二阶段H∞滤波算法（two-stage H∞filter, THF），用以解决普通H∞滤波由于高维状态所导致的计算量大而影响滤波实时性的问题。

1 问题的描述

对于离散时变线性系统

2 Krein空间下的H∞滤波

如文献[8]和[9]中所述，系统(2)的H∞滤波问题可以转化为Krein空间下的Kalman滤波问题。由文献[9]的引理1可知，式(3)等价于

且对于所有的0≤i≤k，

2.1 Krein空间下的系统状态模型

为了引入Krein空间下的系统状态模型，将式(4)重新写成如下形式：

根据文献[8]中的引理6和引理7，引入如下形式的Krein空间线性系统：

2.2 H∞滤波算法

定理1 （H∞滤波器）对于给定的γ＞0，如果满行秩，满足式(3)的滤波器存在，当且仅当对所有的k，有

成立，其中Pk满足如下的递推Riccati方程：

式中，

如果上述条件满足，则一个可能的H∞滤波器给定如下：

注：定理1给出的H∞滤波器实际为次优H∞中心滤波器[9]。

证明：定理1在Krein空间下的推导过程，如下：

通过应用Krein空间下的Kalman滤波算法，对应于线性系统(6)的新息为

显然，式(22)即为式(10)和(11)。证毕。

3 二阶段H∞滤波

TKF算法通过将Kalman滤波算法解耦成两个低维滤波算法并行执行，用以解决由于扩展未知常值偏置为状态量所引起的高维Kalman滤波算法计算量大的问题[11]。文献[12]对TKF进行了扩展，使其适用于未知偏置为随机未知偏置，而不再仅限于常值未知偏置。H∞滤波算法与Kalman滤波算法存在相同的问题，对于包含未知常值偏置为状态量的高维系统来说，计算量大，算法实时性会受到影响，因而滤波精度也将会受到影响[11]。为了解决这个问题，根据TKF算法的原理，提出了THF算法。

定理 2 (THF算法) 对于给定的γ＞0，满足式(3)的滤波器存在，当且仅当对所有的k，有

上述TKF算法与2.2小节中的Kalman滤波完全等价[11]。因此，当选择

时，TKF算法与2.2小节中的Kalman滤波算法相同，使得的最小值大于零，即TKF能够代替2.2小节中的Kalman滤波算法构成H∞滤波算法。此处，通过Krein空间下的TKF算法推导出与定理1等价的THF算法。

由式(42)、(43)和(45)可计算得到：

② THF将H∞滤波中的Riccati方程(8)解耦成两个低维的Riccati方程(25)和(26)，有效地解决了高维矩阵所引起的数值计算问题，减小了计算量[13]。

③ THF的算法结构与TKF相似，能够并行执行，分别计算状态估计和常值偏值估计因此其算法结构适用于硬件的并行运算设计，为进一步提高算法的运行速度提供了可能。

4 算法验证

4.1 验证背景

为了验证THF的有效性以及与H∞滤波的等价性，将THF和H∞滤波分别应用在捷联惯导(SINS)/里程计组合导航系统中，对比其导航结果。文献[13]给出了基于b系下量测的捷联惯导/里程计组合导航系统的误差模型，为了便于THF应用，将其误差模型等价地变成如下形式：

4.2 仿真验证

仿真初始条件设置如下：

陀螺随机常值零偏0.02 (°)/h；加速度计随机常值偏值1×10-4g；里程计初始刻度系数误差为 0.2%；SINS安装误差角均为0.5°；SINS初始对准水平姿态误差角1′、航向误差角10′；初始经纬度和高度误差均为10 m。陀螺噪声和加速度计噪声分别为如下两种情况：

(a) 陀螺噪声为白噪声，噪声均方差为0.02 (°)/h，加速度计噪声为白噪声，噪声均方差为1×10-4g；

(b) 陀螺噪声为由白噪声（均方差为0.02 (°)/h）驱动的二阶马尔科夫过程，加速度计噪声为由白噪声（均方差为1×10-4g）驱动的二阶马尔科夫过程。即均为有色噪声。

在仿真实验中，滤波算法始终将陀螺和加速度计的噪声当作白噪声处理，即在过程噪声为有色噪声的时候，其仍被当作白噪声，以此检验当系统模型不准确的时候滤波算法的估计性能。另外，实验结果仅给出捷联惯导/里程计组合导航系统的导航定位误差，即纬度误差（δL）、经度误差（δλ）和高度误差（δh），以此来说明滤波算法的性能。

4.2.1 静基座仿真实验

在静基座条件下，分别对比THF、H∞滤波（HF）和TKF算法，实验结果如图1和图2所示。

由图1和图2可以看出，在陀螺和加速度计噪声分别为白噪声和二阶马尔科夫过程条件下，应用THF和HF滤波的静基座导航结果相同，从而验证了THF滤波算法的有效性及其与HF滤波算法的等价性。

图1 条件(a)下的静基座定位误差Fig.1 Static-base positioning errors in the condition of (a)

由图1可以看出，在白噪声条件下，TKF的定位结果要略优于THF的定位结果；而由图2可以看出，在有色噪声条件下，THF的定位结果明显优于TKF。上述结果说明，在系统噪声模型比较准确的情况下，TKF的估计性能比THF略优，此时，TKF与Kalman滤波相同具有最小方差估计最优性；而当系统噪声模型不准确时，与TKF滤波相比，THF滤波具有更强的鲁棒性，其表现出更好的估计性能。

图2 条件(b)下的静基座定位误差Fig.2 Static-base positioning errors in the condition of (b)

4.2.2 动基座仿真实验

仿真时间为20 min，车辆首先静止100 s，然后开始运动，轨迹如图3所示，其中车辆机动主要是航向转弯、水平加速、减速，最大速度为20 m/s，总路程约为17.5 km。

在动基座条件下，分别对比THF、HF和TKF算法，实验结果如图4和图5所示。由图4和图5可以看出，与静基座定位结果相似，在陀螺和加速度计噪声分别为白噪声和二阶马尔科夫过程条件下，应用THF和HF滤波的动基座导航结果相同，从而进一步验证了THF滤波算法的有效性及其与HF滤波算法的等价性。

图3 车辆运动轨迹Fig.3 Vehicle Trajectory

图4 条件(a)下的动基座定位误差Fig.4 Moving-base positioning errors in the condition of (a)

图5 条件(b)下的动基座定位误差Fig.5 Moving-base positioning errors in the condition of (b)

同样与静基座定位结果相似，运动基座在白噪声条件下，TKF的定位结果要优于THF的定位结果，如图4所示；而在有色噪声条件下，THF的定位结果明显优于TKF，如图5所示。从而进一步说明，在系统噪声模型比较准确的情况下，TKF具有最小方差估计最优性，其具有更好的估计精度；而当系统噪声模型不准确的情况下，THF滤波要比TKF滤波具有更好的鲁棒性，其估计性能明显优于TKF。

4.2.3 算法运行效率检验

由于THF和HF算法较为复杂，不易直接分析算法计算量。采用在相同的计算机运行条件下，分别进行12 000次的导航滤波解算，对比THF和HF运行所需的时间，如表1所示。

表1 运行时间对比Tab.1 Comparison on running times

由表1可知，THF滤波算法的运行效率明显高于HF滤波算法。值得说明的是，表1中的数据是在普通PC机环境下（Intel i3 双核2.93 GHz处理器，2GB内存）运行得到的；而当在一些运行能力有限的工作环境中（比如SINS计算机主频远低于PC机），两种算法的运算速度差异将愈发明显。在相同的硬件条件下，与HF滤波算法相比，THF滤波算法可以采用更高的滤波频率，提高滤波的实时性。

5 结 论

针对在包含未知常值偏置的线性系统中应用H∞滤波进行状态估计的问题，在Krein空间下，应用TKF理论，推导并建立了THF算法，使得H∞滤波中的高维矩阵计算变成低维矩阵计算，大大减小了计算量。THF算法具有与TKF相同的并行计算结构，适合于硬件的并行设计，从而可以进一步提高THF算法的运行速度。理论推导和仿真实验结果均证明了THF算法与H∞滤波算法在滤波性能上的等价性，且THF算法具有更好的执行效率。此外，与TKF相比，当系统模型不准确的时候，THF具有更好的鲁棒性。

（References）：

[1] 徐定杰, 贺瑞, 沈锋, 等. 基于新息协方差的自适应渐消卡尔曼滤波器[J]. 系统工程与电子技术, 2011, 33(12): 2696-2699. Xu Ding-jie, He Rui, Shen Feng, et al. Adaptive fading Kalman filter based on innovation covariance[J]. Systems Engineering and Electronics, 2011, 33(12): 2696-2699.

[2] 钱华明, 葛磊, 彭宇. 多渐消因子卡尔曼滤波及其在SINS初始对准中的应用[J]. 中国惯性技术学报, 2012, 20(3): 287-291. Qian Hua-ming, Ge Lei, Peng Yu. Multiple fading factors Kalman filter and its application in SINS initial alignment[J]. Journal of Chinese Inertial Technology, 2012, 20(3): 287-291.

[3] Gao W X, Miao L J, Ni M L. Multiple fading factors Kalman filter for SINS static alignment application[J]. Chinese Journal of Aeronautics, 2011, 24(4): 476-483.

[4] Shi P, Luan X L, Liu F. H∞Filtering for discrete-time systems with stochastic incomplete measurement and mixed delays[J]. IEEE Transactions on Industrial Electronics, 2012, 59(6): 2732-2739..

[5] Shi J, Miao L J, Ni M L, et al. Optimal robust fault-detection filter for micro-electro-electromechanical system-based inertial navigation system/global positioning system[J]. Control Theory and Applications, 2012, 29(2): 254-260.

[6] 赵长钟, 栾小丽, 刘飞. 基于有限频段的 Markov跳变系统有限时间H∞滤波[J]. 控制理论与应用, 2015, 32(3): 406-412. Zhao Chang-zhong, Luan Xiao-li, Liu Fei. Finite-time H-infinity filtering for Markov jump systems in finite frequency domain[J]. Control Theory & Applications, 2015, 32(3): 406-412.

[7] Dong H L, Wang Z D, Ding S X, et al. Event-based H∞filter design for a class of nonlinear time-varying systems with fading channels and multiplicative noises[J]. IEEE Transactions on Signal Processing, 2015, 63(13): 3387-3395.

[8] Hassibi B, Sayed A H, Kailath T. Linear estimation in Krein spaces-partⅠ: theory[J]. IEEE Transactions on Automatic Control, 1996, 41(1): 18-33.

[9] Hassibi B, Sayed A H, Kailath T. Linear estimation in Krein spaces-partⅡ: applications[J]. IEEE Transactions on Automatic Control, 1996, 41(1): 34-49.

[10] Shen B, Ding S X, Wang Z D. Finite-horizon H∞fault estimation for uncertain linear discrete time-varying systems with known inputs[J]. IEEE Transactions on Circuits and Systems-Ⅱ: Express Briefs, 2013, 60(12): 902-906.

[11] Ignagni M B. An alternate derivation and extension of Friedland’s two-stage Kalman estimator[J]. IEEE Transactions on Automatic Control, 1981, AC-26(3): 746-750.

[12] Hsieh C S. On the optimality of two-stage Kalman filtering for systems with unknown inputs[J]. Asian Journal of Control, 2010, 12(4): 510-523.

[13] 赵洪松, 缪玲娟, 沈军. 捷联惯导/里程计高精度组合导航算法[J]. 兵工学报, 2014, 35(4): 433-440. Zhao Hong-song, Miao Ling-juan, Shen Jun. High accuracy algorithm for SINS/Odometer integrated navigation system[J]. Acta Armamentarii, 2014, 35(4): 433-440.

Application of two-stage H∞filter in SINS/Odometer integrated navigation system

ZHAO Hong-song1,2, MIAO Ling-juan1, SHAO Hai-jun1
(1. School of Automation, Beijing Institute of Technology, Beijing 100081, China; 2. Unit 94032 of PLA, Wuwei 733003, Gansu, China)

For the state estimate of linear systems with unknown constant biases, such as SINS/Odometer integrated navigation system, the unknown biases are extended to be system states, which often make the state dimension significantly increased. When the H∞filter is used to estimate the system states, the computations required by the H∞filter may be significantly increased, hence the performance of the H∞filter would be degraded. In order to solve this problem, the H∞filtering problem is transformed into the problem of calculating the minimum value of certain quadratic forms in Krein space, and the Kalman filtering algorithm can be used to calculate the minimum value. Thus, by using the two-stage Kalman filtering theory, a two-stage H∞filtering algorithm is developed in Krein space. In this algorithm, the high dimensional matrix operations are decoupled into parallel low dimensional matrix operations, which effectively solve the problem of numerical calculations caused by high-dimensional matrix operations and reduce the computation amount. This algorithm is suitable for the parallel computing hardware design, which makes it possible to further improve the execution speed of this filtering algorithm. Simulation results of the SINS/Odometer integrated navigation system verify the effectiveness of this algorithm.

H∞filter; two-stage Kalman filter; state estimation; Krein space; Riccati equation

V249.3

：A

2015-05-14；

：2015-09-24

国家自然科学基金（61153002）；总装备部预先研究项目（51309030104）

赵洪松（1983—），男，博士研究生，从事惯性导航及组合导航研究。E-mail：3120100354@bit.edu.cn

联 系 人：缪玲娟（1965—），女，教授，博士生导师。E-mail：miaolingjuan@bit.edu.cn

1005-6734(2015)05-0615-09

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