強跟蹤CKF及其在慣導系統初始對準中的應用

郭士犖，許江寧，李 峰

（海軍工程大學 導航工程系，武漢 430033）

強跟蹤CKF及其在慣導系統初始對準中的應用

郭士犖，許江寧，李 峰

（海軍工程大學 導航工程系，武漢 430033）

容積卡爾曼濾波（CKF）是常用的慣性導航系統（INS）初始對準算法。針對在模型失配和觀測噪聲干擾情況下常規容積卡爾曼濾波出現精度下降甚至發散的問題，提出了一種自適應漸消濾波算法，引入多重漸消因子對預測誤差協方差陣進行調整。設計了基于濾波殘差序列統計特性的濾波狀態2χ檢驗條件，檢測濾波器故障并確定是否引入漸消因子，使漸消因子的引入時機更加合理，有效增強了算法的自適應性。仿真試驗表明，新算法可以有效提高初始對準精度及魯棒性。

容積卡爾曼濾波；慣性導航系統；初始對準；自適應漸消濾波；χ2檢驗

慣性導航系統（INS）在導航過程中不依賴于任何外部信息，是一種自主式的導航系統，其導航過程就是要對慣性測量單元（IMU）的輸出進行基于積分的導航解算。在此之前要確定積分初值，即對慣導系統進行初始對準。初始對準是慣性導航的重要階段，其精度和速度決定了慣導系統的工作性能[1-2]。卡爾曼濾波（KF）是高斯過程遞推的最優估計，具有實時性好、精度高等優點，因此KF及其擴展算法是目前較為常用的INS精對準算法。

針對大失準角下的非線性初始對準問題，非線性擴展卡爾曼濾波（EKF）[3-5]、無跡卡爾曼濾波（UKF）[6-7]及容積卡爾曼濾波（CKF）[8-11]得到了廣泛關注和應用。EKF濾波技術通過當前狀態線性展開來近似非線性系統，引入了高階截斷誤差，且計算雅可比矩陣比較繁瑣。UKF與CKF的濾波過程類似，利用具有權值的采樣點來近似狀態變量的均值，并對下一時刻的系統狀態進行預測。但相比而言，CKF的理論推導更加嚴謹，且使用的采樣點更少，文獻[12]證明，三維及以上非線性系統更適用CKF作為濾波方法，其穩定性及濾波精度都高于UKF。

卡爾曼濾波器的精度依賴于系統模型與噪聲統計的準確性[13]。針對系統不確定性會影響濾波精度的問題，文獻[14]提出利用標量漸消因子調節量測噪聲或先驗協方差，在觀測噪聲異常時增強濾波器魯棒性。針對標量漸消因子對多變量跟蹤能力較差的問題，文獻[15][16]提出了基于多重漸消因子的強跟蹤濾波（STF）算法，并通過強迫殘差序列協方差估計值與理論值相等來確定漸消因子的數值，提高了不同濾波通道的調節能力。文獻[17]將多重漸消因子STF算法應用到 SINS初始對準問題中，有效提高了復雜干擾環境下的對準精度。文獻[18][19]進一步將 STF算法應用到CKF中，在CKF的時間和量測更新方程中引入單漸消因子，增強了CKF的魯棒性。

傳統STF通過強迫殘差序列正交或強迫殘差序列協方差估計值與其理論值相等的方式來確定漸消因子的取值，并以漸消因子大于1作為其引入條件，這種引入條件具有一定的隨機性，導致濾波結果不夠穩定。文獻[8]證明，即使在系統正常工作的條件下，傳統STF算法也會導致超過16%的概率判定系統異常，從而產生大于1的漸消因子。針對這一問題，本文提出了基于殘差序列統計特性的濾波狀態的χ2檢驗條件，對濾波器工作狀態進行檢驗，并以一定置信度條件下的檢驗異常作為漸消因子的引入條件，使得漸消因子的引入時機更加合理，增強了STF算法的自適應性。本文結合三階球面-相徑積分的CKF算法，設計了基于多重漸消因子的強跟蹤容積卡爾曼濾波算法（STCKF）并將其應用到捷聯式慣導系統（SINS）的非線性初始對準問題中。仿真結果表明，本文提出的改進算法可以有效提高慣導初始對準精度，性能優于傳統CKF算法。

1 慣導系統非線性誤差模型

1.1 SINS誤差微分方程

選取東-北-天地理坐標系為導航坐標系n，右-前-上體坐標系為載體坐標系 b，載體姿態由姿態轉移矩陣表示。經過初始粗對準后SINS計算得到的導航坐標系為n′，其偏離n系的歐拉角為初始姿態失準角。若初始姿態失準角為大角度，則SINS姿態誤差方程具有非線性，只能進行非線性精對準。設載體真實速度為，SINS解算得到的計算速度為定義慣導系統速度誤差為在此直接給出非線性條件下的 SINS速度與姿態誤差方程：

式中：表示慣導解算得到的變量x的計算值，δx表示慣導解算得到的值與真實值之間的誤差，即δx=x～-x；為單位矩陣；為SINS加速度計輸出的比力，為加速度計的輸出誤差，在此主要考慮加速度計零偏；為地球自轉角速率在n系的投影，為n系相對地球坐標系（e系）的自轉角速度在n系的投影，分別表示各自的計算值，其計算方法為

其中，wie為地球自轉角速率，且為載體所處位置的經、緯度坐標；為陀螺輸出誤差，在此主要考慮陀螺漂移。

其中，

1.2 建立非線性誤差模型

其中，0為適當維數的零向量或矩陣。

在載體運動的情況下可以通過全球衛星定位系統（GPS）、多普勒計程儀（DVL）等提供速度基準，得到速度誤差觀測量。在系泊狀態下，載體對地無線運動，因此以慣導輸出的速度作為速度誤差觀測量，即：

2 容積卡爾曼濾波算法

CKF的核心思想是基于貝葉斯估計和最小方差準則，利用三階球面-相徑容積的近似策略對非線性高斯系統狀態的后驗均值、協方差和互協方差進行近似[7]。其算法實現步驟為：

時間更新：

1）假設k-1時刻的狀態估計及估計均方差已知。首先通過Cholesky分解Pk-1：

2）計算容積點

3）通過狀態方程傳遞容積點

4）對k時刻的狀態進行預測

5）估計k時刻的預測均方誤差

量測更新：

1）對Pk/k-1做Cholesky分解

2）計算容積點：

3）通過觀測方程傳遞容積點

4）估計k時刻的觀測預測值

5）估計自相關協方差

6）估計互相關協方差

7）計算濾波增益

8）k時刻的狀態估計

9）k時刻的估計均方誤差

3 基于多重漸消因子的自適應強跟蹤CKF算法

3.1 多重漸消因子

基于漸消因子的STF算法的核心是通過正交性原理在線求取最優濾波增益。正交性原理是卡爾曼濾波理論中的一個重要定理，即在系統模型完全準確，不存在模型誤差時，卡爾曼濾波器輸出的殘差序列是均值為零的高斯白噪聲，因此這些殘差序列處處正交。當濾波異常時，通過在線調整濾波增益陣使殘差序列正交，可獲得優于卡爾曼濾波的估計結果，當濾波正常時，STF便退化為普通的卡爾曼濾波。

濾波器中每一步預測的濾波殘差為

在濾波最優的情況下，其協方差理論值滿足

而殘差序列協方差的估計值C?k為[16]

傳統STF算法認為，如果濾波殘差協方差的計算值大于其理論值，則可以說明濾波器已經出現異常。此時可以通過調整的取值改變強迫殘差協方差的理論值與計算值相等，使濾波器恢復到正常狀態。下面推導Sk的計算方法：

若量測矩陣Hk滿足

其中，λi為對角陣λm×m的第i個對角元素，和分別為矩陣Nk和Jk第i行i列的對角元素。

3.2 基于χ2檢驗的濾波器故障檢測

從以上推導過程可以看出，傳統多重漸消因子STF通過強制濾波殘差協方差的理論值與計算值相等來計算的，并且在漸消因子大于1時STF算法發揮作用。而實際上漸消因子是否大于1并不能準確代表濾波器當前是否處于穩定狀態。文獻[8]證明，即使在系統正常工作的情況下，STF算法也會導致超過16%的概率誤判系統異常，從而產生大于1的漸消因子。這就有可能造成在并不需要引入漸消因子的時候引入了漸消因子，一方面增加了濾波器計算量，另一方面也會影響濾波結果的穩定性。

針對傳統漸消濾波中漸消因子的引入條件過于寬松的問題，本文提出了一種基于濾波殘差序列統計特性的檢驗條件。已知在濾波穩定的情況下，殘差序列的統計特性滿足

即濾波殘差εk的分布為0均值，方差為的正態分布；γk的分布為m自由度的χ2分布，m為量測的維度。因此可以利用χ2分布上側分位點的性質來進行濾波穩定性的判斷。例如當量測維度m=2時，選取分位點ζ=9.210，則有

也就是說，在濾波正常的情況下，γk大于ζ的概率只有1%。根據假設檢驗原理，若γk＞ζ ，則在99%的置信度下可以認為濾波異常。以此作為漸消因子引入的判斷條件，即首先進行正常CKF濾波并實時計算γk，若滿足γk＞ζ，則引入漸消因子，進入STCKF濾波。

4 仿真分析與驗證

用本文提出的 STCKF濾波算法進行靜基座下的對準實驗，仿真參數設置如下：

慣導系統所處位置為(30.58°N, 114.24°E)。陀螺常值漂移為 0.01 (°)/h，角隨機游走系數為0.001 (°)/h1/2。加速度計的常值偏置為 1×10–4g，測量白噪聲為1×10–5g。設采樣頻率為100 Hz，對準時間為900 s。選用 1.2節中建立的慣導非線性誤差模型為初始對準濾波模型。以慣導水平速度誤差作為觀測量，在無線運動狀態下慣導輸出的速度即為速度誤差。

假設慣導系統處于系泊狀態，在風浪的影響下，慣導姿態作周期變化：

其中，θ、φ、ψ分別為俯仰角、橫滾角和航向角。設初始失準角為[10°,30°,50°]，其他初始條件設置如下：

為驗證算法有效性，假設觀測量是在受噪聲干擾的初始對準環境下。通常情況下，慣導系統初始對準時的速度觀測量可以由GPS或DVL等外部基準提供。考慮到環境干擾等因素，速度基準會受到非高斯測量噪聲的影響，此時速度誤差觀測量也會受到同樣的誤差干擾。為了模擬這一情況，人為對速度誤差觀測量施加噪聲為濾波器觀測方程使用的觀測量，VD為測速設備輸出的噪聲。應該注意到，VD實際上是由GPS或DVL等測速設備引入到濾波器觀測方程中的，

其中，wD為零均值的高斯白噪聲。已知量測維度通過查表選取分位點即99%的檢驗置信度。分別使用傳統CKF對準算法及本文提出的STCKF算法進行對準仿真，對準結果如圖1～3所示。

圖1～3中橫坐標表示對準時間，縱坐標表示對準得到的失準角與真實失準角的差值，即對準誤差δxφ、單位為度(°)。表示本文提出的STCKF的對準誤差，表示傳統CKF的對準誤差。從圖中可以看出，在觀測噪聲VD的干擾下，傳統CKF對準結果出現比較大的振蕩，特別是航向對準出現發散，最終航向對準誤差為4.08°，而本文提出的STCKF算法具有更好的魯棒性，在噪聲干擾的情況下仍具有很好的穩定性和濾波精度，最終的航向對準誤差為0.03°。

圖2 橫滾角對準誤差Fig.2 Alignment errors of roll angle

圖3 航向角對準誤差Fig.3 Alignment errors of yaw angle

本文通過χ2檢驗進行濾波器故障檢測以確定漸消因子的引入時機：在濾波過程中利用式(18)實時計算kγ，在γk大于ζ時才進入STCKF。一方面使漸消因子的引入時機更加合理，減少濾波器的計算量；另一方面減少漸消因子的不必要引入，使濾波結果更加穩定。

為了進一步說明算法優勢，分別使用傳統 CKF（“算法1”）、直接引入式STCKF（“算法2”）以及基于χ2檢驗的STCKF（“算法3”）進行20次對準試驗，記錄每次試驗的姿態對準誤差，計算其均值作為對比的依據。以xφ為例：

表1 對準試驗姿態誤差均值Tab.1 Mean of attitude errors in alignment experiments

表1所示為三種算法在20次對準實驗中的姿態誤差均值，可以看出：算法2與算法3都可以明顯提高初始對準精度，證明基于多重漸消因子的強跟蹤CKF算法在抗干擾能力上的優勢；與算法2相比，算法3的對準精度有所提高，這是因為通過χ2檢驗進行濾波器的故障檢測，使得漸消因子的引入時機更加合理，濾波器穩定性更好。

5 結束語

為提高三階球面-相徑容積卡爾曼濾波器的抗干擾能力，分析了強跟蹤濾波器的基本原理，提出了基于多重漸消因子的強跟蹤容積卡爾曼濾波。在此基礎上，針對傳統漸消卡爾曼濾波中漸消因子的引入條件過于寬松的問題，提出了基于χ2分布檢驗的濾波器故障檢測方法，檢驗濾波器的工作狀態是否出現異常，以確定是否需要引入漸消因子。

將提出的濾波算法應用到捷聯慣導系統非線性初始對準應用中。仿真試驗表明，在復雜觀測噪聲干擾的情況下，改進的濾波算法可以有效保證濾波結果的魯棒性，提高慣導系統的初始對準精度。

（References）：

[1]Li J S, Xu J N, Chang L B, et al. An improved optimal method for initial alignment[J]. Journal of Navigation,2014, 67(4): 727-736.

[2]徐曉蘇, 周峰, 張濤, 等. 基于四元數自適應卡爾曼濾波的快速對準算法[J]. 中國慣性技術學報, 2016, 24(4):454-459.Xu Xiao-su, Zhou Feng, Zhang Tao, et al. Initial alignment algorithm for SINS based on quaternion adaptive Kalman filter[J]. Journal of Chinese Inertial Technology,2016, 24(4): 454-459.

[3]Hsu Y L, Wang J S, Chang C W. A Wearable inertial pedestrian navigation system with quaternion-based extended Kalman filter for pedestrian localization[J]. IEEE Sensors Journal, 2017, 17(10): 3193-3206.

[4]Zhou W D, Hou J X, Liu L, et al. Design and simulation of the integrated navigation system based on extended Kalman filter[J]. Open Physics, 2017, 15(1): 182-187.

[5]Fang J C, Yang S. Study on innovation adaptive EKF for in-flight alignment of airborne POS[J]. IEEE Transactions on Instrumentation & Measurement, 2011, 60(4):1378-1388.

[6]Chang Lu-bin, Hu Bai-qin, Li An, et al. Strapdown inertial navigation system alignment based on marginalised unscented Kalman filter[J]. IET Science, Measurement &Technology, 2013, 7(2): 128-138.

[7]Li W L, Wang J L, Lu L Q, et al. A novel scheme for DVL-aided SINS in-motion alignment using UKF techniques[J]. Sensors, 2013, 13(1): 1046-1063.

[8]葛磊. 容積卡爾曼濾波算法研究及其在導航中的應用[D]. 哈爾濱: 哈爾濱工程大學, 2013.Ge Lei. Research on cubature Kalman filter and its application for navigation[D]. Harbin: Harbin Engineering University, 2013.

[9]Zhang L, Yang C, Chen Q W, et al. Robust H-infinity CKF/KF hybrid filtering method for SINS alignment[J].IET Science, Measurement & Technology, 2016, 10(8):916-925.

[10]Ran C Y, Cheng X H, Wang L. SINS alignment using velocity matching and simplified cubature Kalman filter[J].Applied Mechanics and Materials, 2014, 615: 255-258.

[11]Cui B B, Chen X Y, Xu Y, et al. Performance analysis of improved iterated cubature Kalman filter and its application to GNSS/INS[J]. ISA Transactions, 2017, 66: 460-468.

[12]孫楓, 唐李軍. Cabature卡爾曼濾波與Unscented卡爾曼濾波估計精度比較[J]. 控制與決策, 2013, 28(2):303-308.Sun Feng, Tang Li-jun. Estimation precision comparison of cubature Kalman filter and unscent Kalman filter[J].Control and Decision, 2013, 28(2): 303-308.

[13]Chang Guo-bin, Liu Ming. M-estimator-based robust Kalman filter for systems with process modeling errors and rank deficient measurement models[J]. Nonlinear Dynamics, 2015, 80(3): 1431-1449.

[14]Chang Guo-bin, Liu Ming. An adaptive fading Kalman filter based on Mahalanobis distance[J]. Proceedings of the Institution of Mechanical Engineers, Part G: Journal of Aerospace Engineering, 2015, 229(6): 1114-1123.

[15]Geng Yan-rui, Wang Jin-ling. Adaptive estimation of multiple fading factors in Kalman filter for navigation applications[J]. GPS Solution, 2007, 12(4): 273-279.

[16]高偉, 李敬春, 奔粵陽, 等. 基于多重漸消因子的自適應卡爾曼濾波器[J]. 系統工程與電子技術, 2014, 36(7):1405-1409.Gao Wei, Li Jing-chun, Ben Yue-yang, et al. Adaptive Kalman filter based on multiple fading factors[J]. Systems Engineering and Electronics, 2014, 36(7): 1405-1409.

[17]錢華明, 葛磊, 彭宇. 多漸消因子卡爾曼濾波及其在SINS初始對準中的應用[J]. 中國慣性技術學報, 2012,20(3): 287-291.Qian Hua-ming, Ge Lei, Peng Yu. Multiple fading Kalman filter and its application in SINS initial alignment[J]. Journal of Chinese Inertial Technology, 2012, 20(3): 287-291.

[18]徐曉蘇, 田澤鑫, 劉義亭, 等. 基于改進型 CKF的SINS初始對準方法[J]. 華中科技大學學報, 2016,44(1): 81-86.Xu Xiao-su, Tian Ze-xin, Liu Yi-ting, et al. Alignment method of initial SINS based on improved CKF[J].Huazhong Univ. of Sci. & Tech, 2016, 44(1): 81-86.

[19]張龍, 崔乃剛, 王小剛, 等. 強跟蹤-容積卡爾曼濾波在彈道式再入目標跟蹤中的應用[J]. 中國慣性技術學報, 2015, 23(2): 211-218.Zhang Long, Cui Nai-gang, Wang Xiao-gang, et al.Strong tracking-cubature Kalman filter for tracking ballistic reentry target[J]. Journal of Chinese Inertial Technology, 2015, 23(2): 211-218.

Strong tracking cubature Kalman filter for initial alignment of inertial navigation system

GUO Shi-luo, XU Jiang-ning, LI Feng
(Department of Navigation, Navy University of Engineering, Wuhan 430033, China)

The filtering accuracy of the cubature Kalman filter is tend to decrease or even diverse when there are disturbances of inaccurate model and/or observation noise. To solve this problem, an improved adaptive fading filter is proposed, which introduce multiple fading factors to adjust the covariance matrix of the prediction errors. A chi-square test method is designed to check the filter’s fault and determine at what time the fading factors are introduced, thus the introduction of the fading factor is more reasonable, and the algorithm’s adaptability is enhanced. Simulation and experiment are made for the nonlinear initial alignment of SINS suffered from complex observation noise interference, and the results show that the proposed algorithm can effectively improve the accuracy and robustness of the initial alignment.

cubature Kalman filter; inertial navigation system; initial alignment; adaptive fading filter;chi-square test

1005-6734(2017)04-0436-06

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

U666.1

A

2017-04-13；

2017-07-26

國家自然科學基金（41574069）；國家自然科學基金（61503404）；國家自然科學基金（41404002）

郭士犖（1991—），男，博士研究生，從事慣性技術及應用研究。E-mail: hg_guoshiluo@163.com

聯 系 人：許江寧（1964—），男，教授，博士生導師。E-mail: xujiangning@hotmail.com