二階段H∞濾波在捷聯慣導/里程計組合導航系統中的應用

趙洪松，繆玲娟，邵海俊

（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