強跟蹤CKF及其在慣導(dǎo)系統(tǒng)初始對準(zhǔn)中的應(yīng)用

郭士犖，許江寧，李 峰

（海軍工程大學(xué) 導(dǎo)航工程系，武漢 430033）

強跟蹤CKF及其在慣導(dǎo)系統(tǒng)初始對準(zhǔn)中的應(yīng)用

郭士犖，許江寧，李 峰

（海軍工程大學(xué) 導(dǎo)航工程系，武漢 430033）

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

容積卡爾曼濾波；慣性導(dǎo)航系統(tǒng)；初始對準(zhǔn)；自適應(yīng)漸消濾波；χ2檢驗

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

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

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

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

1 慣導(dǎo)系統(tǒng)非線性誤差模型

1.1 SINS誤差微分方程

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

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

其中，wie為地球自轉(zhuǎn)角速率，且為載體所處位置的經(jīng)、緯度坐標(biāo)；為陀螺輸出誤差，在此主要考慮陀螺漂移。

其中，

1.2 建立非線性誤差模型

其中，0為適當(dāng)維數(shù)的零向量或矩陣。

在載體運動的情況下可以通過全球衛(wèi)星定位系統(tǒng)（GPS）、多普勒計程儀（DVL）等提供速度基準(zhǔn)，得到速度誤差觀測量。在系泊狀態(tài)下，載體對地?zé)o線運動，因此以慣導(dǎo)輸出的速度作為速度誤差觀測量，即：

2 容積卡爾曼濾波算法

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

時間更新：

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

2）計算容積點

3）通過狀態(tài)方程傳遞容積點

4）對k時刻的狀態(tài)進行預(yù)測

5）估計k時刻的預(yù)測均方誤差

量測更新：

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

2）計算容積點：

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

4）估計k時刻的觀測預(yù)測值

5）估計自相關(guān)協(xié)方差

6）估計互相關(guān)協(xié)方差

7）計算濾波增益

8）k時刻的狀態(tài)估計

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

3 基于多重漸消因子的自適應(yīng)強跟蹤CKF算法

3.1 多重漸消因子

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

濾波器中每一步預(yù)測的濾波殘差為

在濾波最優(yōu)的情況下，其協(xié)方差理論值滿足

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

傳統(tǒng)STF算法認為，如果濾波殘差協(xié)方差的計算值大于其理論值，則可以說明濾波器已經(jīng)出現(xiàn)異常。此時可以通過調(diào)整的取值改變強迫殘差協(xié)方差的理論值與計算值相等，使濾波器恢復(fù)到正常狀態(tài)。下面推導(dǎo)Sk的計算方法：

若量測矩陣Hk滿足

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

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

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

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

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

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

4 仿真分析與驗證

用本文提出的 STCKF濾波算法進行靜基座下的對準(zhǔn)實驗，仿真參數(shù)設(shè)置如下：

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

假設(shè)慣導(dǎo)系統(tǒng)處于系泊狀態(tài)，在風(fēng)浪的影響下，慣導(dǎo)姿態(tài)作周期變化：

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

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

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

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

圖2 橫滾角對準(zhǔn)誤差Fig.2 Alignment errors of roll angle

圖3 航向角對準(zhǔn)誤差Fig.3 Alignment errors of yaw angle

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

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

表1 對準(zhǔn)試驗姿態(tài)誤差均值Tab.1 Mean of attitude errors in alignment experiments

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

5 結(jié)束語

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

將提出的濾波算法應(yīng)用到捷聯(lián)慣導(dǎo)系統(tǒng)非線性初始對準(zhǔn)應(yīng)用中。仿真試驗表明，在復(fù)雜觀測噪聲干擾的情況下，改進的濾波算法可以有效保證濾波結(jié)果的魯棒性，提高慣導(dǎo)系統(tǒng)的初始對準(zhǔn)精度。

（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]徐曉蘇, 周峰, 張濤, 等. 基于四元數(shù)自適應(yīng)卡爾曼濾波的快速對準(zhǔn)算法[J]. 中國慣性技術(shù)學(xué)報, 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ǎo)航中的應(yīng)用[D]. 哈爾濱: 哈爾濱工程大學(xué), 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]高偉, 李敬春, 奔粵陽, 等. 基于多重漸消因子的自適應(yīng)卡爾曼濾波器[J]. 系統(tǒng)工程與電子技術(shù), 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初始對準(zhǔn)中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報, 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初始對準(zhǔn)方法[J]. 華中科技大學(xué)學(xué)報, 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]張龍, 崔乃剛, 王小剛, 等. 強跟蹤-容積卡爾曼濾波在彈道式再入目標(biāo)跟蹤中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報, 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

國家自然科學(xué)基金（41574069）；國家自然科學(xué)基金（61503404）；國家自然科學(xué)基金（41404002）

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

聯(lián) 系 人：許江寧（1964—），男，教授，博士生導(dǎo)師。E-mail: xujiangning@hotmail.com