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

張德州, 劉文鍇, 熊曉峰, 馮黨, 宋書(shū)學(xué)

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

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

1.1 漸消濾波的基本準則

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

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分別為觀(guān)測矩陣和觀(guān)測向量,則卡爾曼濾波的迭代解xk/k為:

(3)

(4)

(5)

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

1.2 漸消因子的估計方法

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

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

(6)

(7)

(8)

(9)

Vk=Hkxk,k-1-zk。

(10)

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

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

(11)

(12)

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

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

(13)

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

(14)

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

(15)

(16)

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

zk=[ΔrΔv]T。

(17)

實(shí)際上,松耦合GNSS/INS組合導航系統中的觀(guān)測方程為線(xiàn)性方程,因此其量測更新過(guò)程可以表示為:

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

(18)

(19)

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

(20)

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

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

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

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

圖1 方案①的定位誤差

圖2 方案②的定位誤差

圖3 方案③的定位誤差

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

圖4 方案④的定位誤差

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

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

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

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

圖6 方案①的定位誤差

圖7 方案②的定位誤差

圖8 方案③的定位誤差

圖9 方案④的定位誤差

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

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

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

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

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

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

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