基于馬氏距離的非線性自適應狀態(tài)估計算法

張德州, 劉文鍇, 熊曉峰, 馮黨, 宋書學

(1.河南省地球物理空間信息研究院,河南 鄭州 450009; 2.河南省地質物探工程技術研究中心,河南 鄭州 450009;3.華北水利水電大學,河南 鄭州 450046; 4.南水北調(diào)中線建管局渠首分局,河南 南陽 473000)

針對上述問題,本文在工程實踐的基礎上提出了一種基于漸消因子的改進自適應濾波器。根據(jù)假設檢驗的結果在每個歷元上進行漸消濾波或魯棒估計。利用GNSS/INS組合導航系統(tǒng)采集的實測數(shù)據(jù),對所提算法進行測試,并與常規(guī)濾波器的解算結果進行對比分析,以驗證該算法的有效性。

1 漸消濾波

1.1 漸消濾波的基本準則

在滿足高斯分布假設的情況下,卡爾曼濾波具有良好的性能。但是,若模型誤差較大,脫離了高斯分布假設,則可能導致濾波器發(fā)散。針對動態(tài)模型誤差,研究人員提出利用漸消濾波器[14]來限制卡爾曼濾波器的“記憶長度”。對于動力學方程:

xk=Φk,k-1xk-1+wk,

(1)

先驗狀態(tài)估值xk,k-1為:

xk,k-1=Φk,k-1xk-1。

(2)

式中:xk和xk-1分別為k和k-1歷元的狀態(tài)向量;Φk,k-1為狀態(tài)轉移矩陣;wk為狀態(tài)噪聲矩陣。

假設Hk和zk分別為觀測矩陣和觀測向量,則卡爾曼濾波的迭代解xk/k為:

(3)

(4)

(5)

與傳統(tǒng)卡爾曼濾波器的協(xié)方差矩陣相比,漸消濾波器中xk,k-1的協(xié)方差矩陣膨脹了Sk倍,更加注重當前測量信息的作用。因此,由于漸消因子的存在,前一歷元帶來的狀態(tài)模型誤差得到了很好的減弱。由此也可以看出,當前測量信息可靠與否,直接影響著漸消因子的效果。

1.2 漸消因子的估計方法

漸消濾波器的重點工作是構造合適的漸消因子。理論上,當誤差異常時,應放大漸消因子Sk。研究人員提出了一種具有最優(yōu)漸消因子的濾波器,并給出了簡化的漸消因子[14]:

Sk=max{1,tr(Nk)/tr(Mk)};

(6)

(7)

(8)

(9)

Vk=Hkxk,k-1-zk。

(10)

除采用單因子漸消濾波器外,還開發(fā)了用于漸消濾波器的漸消矩陣[12]。與單因子漸消濾波器不同,漸消矩陣濾波器能夠根據(jù)狀態(tài)向量元素的可觀測性在多個數(shù)據(jù)通道中進行調(diào)整。漸消矩陣可表示為:

Sk=diag(s1,s2,…,si,…,st,1,…,1,…,1),

(11)

(12)

在公式(11)中,s1、s2、…、st能夠自適應地進行估計,其他元素均賦值為1。

2 基于馬氏距離和漸消因子的改進自適應狀態(tài)估計

漸消濾波器中更加重視當前測量信息[11],為了達到理想的濾波效果,當前的測量信息必須可靠。然而,很少有文獻討論漸消濾波器中當前測量信息不可靠的情況。實際上,漸消濾波器中的單漸消因子和漸消矩陣都是基于測量結果得到的預測殘差向量來構造的?？梢?漸消濾波器的性能與測量值的精度密切相關。因此,應適時、合理地使用漸消因子/矩陣,制定針對不可靠觀測信息的有效處理策略。若過程噪聲和測量噪聲符合高斯分布,則離散線性隨機狀態(tài)空間模型可表示為:

(13)

m維觀測值的概率密度函數(shù)ρ(zk)可表示為:

(14)

為控制異常觀測誤差的影響,Rk需自適應進行估計與更新,以降低異常觀測量的權重或者刪除異常觀測值。一般地,先驗狀態(tài)向量中的元素是相關的[11]。因此引入雙因子協(xié)方差膨脹參數(shù)λij[15],即:

(15)

(16)

3 GNSS/INS組合導航系統(tǒng)模型

近年來,GNSS/INS組合導航系統(tǒng)已成為動態(tài)導航定位領域的熱點[16-17]。GNSS和INS的集成主要分為3種模式,即松耦合、緊耦合以及超緊耦合。其中,松耦合系統(tǒng)實施方便,成本較低,應用較為廣泛,因此本文采用松耦合模式。對于GNSS/INS各種集成系統(tǒng),應針對非線性問題實現(xiàn)非線性濾波。

zk=[ΔrΔv]T。

(17)

實際上,松耦合GNSS/INS組合導航系統(tǒng)中的觀測方程為線性方程,因此其量測更新過程可以表示為:

xk,k=xk,k-1+Kk(zk-Hkxk,k-1),

(18)

(19)

Pk,k=Pk,k-1-KkHkPk,k-1。

(20)

顯然,狀態(tài)估計值與測量值密切相關,不可靠測量值可能會造成不可估量的影響。然而,在實際中不可靠的測量是不可避免的。因此,對異常測量值必須進行謹慎處理。漸消因子/矩陣和抗差估計方法已在GNSS/INS組合導航系統(tǒng)中得到應用。針對漸消因子對濾波效果的影響,可采用基于馬氏距離和協(xié)方差膨脹因子構建的判斷指標來提高濾波效果。

4 試驗分析

為驗證所提算法的有效性,本文設計了不同的濾波算法數(shù)據(jù)處理試驗。以地面車輛作為測試載體,車輛上裝備有GNSS接收機和慣性測量單元(Inertial Measurement Unit,IMU)。同時,另一臺GNSS接收機安裝在地勢較高處作為基準站。利用地面車輛在真實條件下采集數(shù)據(jù)。在測試系統(tǒng)中,衛(wèi)星截止高度角設置為10°,GNSS采樣頻率為1 Hz,INS采樣頻率為100 Hz,以商用軟件提供的雙差緊耦合結果作為參考。

在GNSS/INS集成系統(tǒng)的數(shù)據(jù)融合中,數(shù)據(jù)融合解算的時間間隔為1 s。試驗中,分別采用初始數(shù)據(jù)和帶有異常觀測值的數(shù)據(jù)測試了不同算法的性能。因此,設計并實施了兩種試驗情形。每一種情形由4種方案組成,同時將每種算法的結果與參考值之差作為解算誤差。對比單個漸消因子和漸消矩陣發(fā)現(xiàn),漸消矩陣能夠同時在多個數(shù)據(jù)通道中調(diào)整協(xié)方差矩陣,具有更好的性能,增強了抵御模型失配的能力,因此本文采用漸消矩陣進行試驗。對于本文的所有算法,協(xié)方差的初始值都是根據(jù)經(jīng)驗確定的,因此模型參數(shù)存在一定的統(tǒng)計偏差。4種方案為:①傳統(tǒng)卡爾曼濾波器;②漸消矩陣濾波器;③接受零假設的漸消矩陣漸消濾波器;④在接受零假設時采用漸消矩陣濾波器,否則利用協(xié)方差膨脹因子對異常觀測值進行序列更新。

試驗1:本試驗中利用GNSS/INS組合系統(tǒng)采集的數(shù)據(jù)分別采用上述4種方案進行處理。各算法的定位誤差如圖1—4所示。

圖1 方案①的定位誤差

圖2 方案②的定位誤差

圖3 方案③的定位誤差

如上所述,每種方案都是基于初始測量值實現(xiàn)的。初始測量值是在良好的觀測條件下采集的,測量值中存在少量的異常值。因此,由初始測量值解算產(chǎn)生的定位誤差主要是由模型偏差帶來的。對比圖1—4可以明顯看出:方案①算法在X、Y、Z方向上的誤差幅值要比其他算法的大,說明傳統(tǒng)卡爾曼濾波算法的性能有待提高;后面3種算法有效抑制了模型不確定性引起的濾波器發(fā)散,提高了濾波器的穩(wěn)定性。理論上,方案②和方案③的主要區(qū)別在于采用漸消矩陣的時機。對于初始測量值,零假設在大多數(shù)歷元上都被接受,因此方案②和方案③的結果是相似的。除了零假設被接受的歷元外,方案④算法在其他歷元中均采用了協(xié)方差膨脹因子,從理論上弱化了異常測量的影響。因此,圖4所示算法的誤差幅值比其他3種算法的誤差幅值小。

圖4 方案④的定位誤差

為便于表達各算法的性能,計算了各算法的均方根誤差。圖5給出了各算法在X、Y、Z3個方向上的均方根誤差(RMSE)。

圖5 各方案的定位均方根誤差

從圖5可以看出,方案①比其他3種算法的RMSE都要大得多,說明傳統(tǒng)卡爾曼濾波算法的穩(wěn)定性和魯棒性還需要進一步加強。比較方案②算法和方案③算法的RMSE值發(fā)現(xiàn),前者的效果更好,模型誤差的影響比異常測量值的影響更顯著。在方案③算法的基礎上,本文所提出的改進算法能夠拒絕原假設的歷元附加協(xié)方差膨脹因子。顯然,圖5中方案④算法的RMSE值比其他算法的RMSE值要小,說明有更高的解算精度。由于模型誤差的影響減弱,所有漸消濾波算法的RMSE都小于CKF算法的,并且方案④算法在精度上優(yōu)于其他算法。

試驗2:本試驗人為加入漸變及突變的定位異常值,以檢驗各算法的穩(wěn)定性和抗差性,從而構造基于初始數(shù)據(jù)的擾動數(shù)據(jù)。4種算法均基于擾動數(shù)據(jù)實現(xiàn),各算法的定位誤差如圖6—9所示,圖10描述了各算法在X、Y、Z3個方向上的均方根誤差。

圖6 方案①的定位誤差

圖7 方案②的定位誤差

圖8 方案③的定位誤差

圖9 方案④的定位誤差

圖10 各方案的定位均方根誤差

相對較大的異常觀測值數(shù)量是影響濾波性能的主要因素。分析圖6—9以及圖5和圖10發(fā)現(xiàn),所有算法的誤差振幅都比試驗1中的振幅大,說明所有算法都受到了測量異常值的顯著影響。由于方案①和方案③算法沒能有效地處理測量異常值,圖6和圖8顯示的測量異常值影響比圖7和圖9中的更明顯。方案③算法在接受零假設且模型誤差得到良好控制的情況下采用漸消矩陣,其性能優(yōu)于方案①算法。在方案②中,當接受零假設時,漸消矩陣表現(xiàn)良好,而當拒絕零假設時,漸消濾波器變得不穩(wěn)定。對比圖6和圖7可以看出,在某些歷元,方案②算法的振幅甚至比方案①算法的還要大。方案④算法考慮了模型誤差和測量異常值,并在相對合理的時機采用漸消矩陣。在圖9中,3個方向的誤差振幅都比其他算法的誤差振幅小,說明方案④算法有更好的性能。但對于連續(xù)變化的異常觀測值可能會出現(xiàn)數(shù)值太小而導致的檢驗失敗。因此,在圖9的某些歷元,方案④算法的性能仍然會受到連續(xù)變化的異常觀測值的影響。

5 結論

本文針對異常觀測值研究了漸消因子/矩陣的自適應狀態(tài)估計問題,得到如下結論:

1)傳統(tǒng)卡爾曼濾波算法難以有效抵抗異常觀測值的影響,其濾波魯棒性還需要進一步加強。

2)基于漸消矩陣的自適應濾波器能夠控制模型誤差的影響,但當測量數(shù)據(jù)中存在異常值時,濾波器的性能可能會顯著下降,本文的數(shù)據(jù)試驗結果也驗證了這一點。

3)利用異常觀測值殘差構造出的漸消矩陣將導致濾波性能變差,不合理地使用漸消濾波將帶來相反的效果。因此,漸消濾波在工程應用中應采用適時及合理的方式。

4)通過異常探測提高了漸消濾波器的穩(wěn)定性,有效地控制了模型誤差和測量異常值的影響。