一種基于軟卡方檢測的自適應(yīng)Kalman濾波方法

嚴(yán)恭敏, 劉 璠,, 李梓陽, 周 琪

(1. 西北工業(yè)大學(xué)自動化學(xué)院, 西安 710072;2. 中國航空工業(yè)集團公司西安飛行自動控制研究所, 西安 710076)

0 引言

卡爾曼濾波(Kalman filter, KF)是一種利用隨機線性系統(tǒng)狀態(tài)空間模型,通過系統(tǒng)輸出的觀測數(shù)據(jù),對系統(tǒng)狀態(tài)進行最優(yōu)估計的方法,在導(dǎo)航制導(dǎo)與控制、通信以及圖像處理等眾多領(lǐng)域有著廣泛的應(yīng)用。

理論上,只有在隨機線性系統(tǒng)的結(jié)構(gòu)參數(shù)和噪聲統(tǒng)計特性參數(shù)都準(zhǔn)確已知的條件下,KF才能獲得狀態(tài)的最優(yōu)估計。如果實際量測的統(tǒng)計特性與系統(tǒng)建模參數(shù)不匹配,會導(dǎo)致KF狀態(tài)估計精度下降,嚴(yán)重時還可能會引起濾波發(fā)散。卡方檢測(Chi-square test, C2T)方法是一種傳統(tǒng)的量測故障檢測方法[1],在狀態(tài)空間建模準(zhǔn)確的前提下,通過KF的新息及其均方差陣構(gòu)造卡方統(tǒng)計量,可檢測出量測是否存在異常,如果量測正常則進行量測更新,反之,如果量測異常則放棄量測更新。然而,實際應(yīng)用中的系統(tǒng)噪聲參數(shù)往往難以準(zhǔn)確確定,從而使得卡方檢測統(tǒng)計量的閾值設(shè)置成為一大難題。如果卡方檢測閾值設(shè)置太大,則可能將異常量測引入濾波器,降低濾波估計精度;如果閾值設(shè)置太小,則可能排除了一些正常的量測,使得量測利用率下降,同樣也會降低狀態(tài)估計精度。造成這一問題的根源在于傳統(tǒng)卡方檢測的二值化量測異常判斷原則,量測要么被判斷為正常、要么被判斷為異常,再沒有其他任何中間情形。

借鑒Sage-Husa自適應(yīng)濾波方法[2-5](Sage-Husa adaptive KF, SHAKF)或抗差自適應(yīng)濾波方法[6-7],至少可以更精細(xì)地將量測狀況劃分為“正常/信任”“可疑/部分信任”和“異常/丟棄”這三種類別,針對不同的量測類別修改量測噪聲方差陣的大小或賦予不同的量測利用權(quán)重,或者直接根據(jù)KF新息大小計算權(quán)重,實現(xiàn)權(quán)重大小的無縫連續(xù)過渡。但與上述兩種自適應(yīng)濾波的思路不完全相同,本文通過分析傳統(tǒng)卡方檢測的新息權(quán)重二值化利用特點,給出了新息權(quán)重的連續(xù)化利用方法,將權(quán)重的二值化取值改進為連續(xù)化取值,并將其推廣,從而獲得了多維量測的多分量卡方檢測方法。文中將所提新方法稱為軟卡方檢測KF方法(soft Chi-square test KF,SC2TKF),傳統(tǒng)方法可被稱為硬卡方檢測KF方法。顯然,新方法更具普遍性,傳統(tǒng)硬卡方檢測可以看作是新軟卡方檢測方法的一個特例。最后,利用慣導(dǎo)/衛(wèi)導(dǎo)組合進行了軟卡方檢測方法的KF仿真驗證,結(jié)果顯示新方法具有比Sage-Husa自適應(yīng)濾波更好的濾波精度和穩(wěn)定性。

1 傳統(tǒng)的量測故障卡方檢測原理

隨機線性系統(tǒng)的狀態(tài)空間模型記為

(1)

式中,Xk是n×1維的狀態(tài)矢量,Zk是m×1維的量測矢量;Φk/k-1,Γk-1,Hk是已知的系統(tǒng)結(jié)構(gòu)參數(shù),分別稱為n×n維的狀態(tài)一步轉(zhuǎn)移矩陣、n×l維的系統(tǒng)噪聲分配矩陣、m×n維的量測矩陣;Wk-1是l×1維的系統(tǒng)噪聲矢量,Vk是m×1維的量測噪聲矢量,兩者都是零均值的高斯白噪聲矢量序列,且它們之間互不相關(guān),即滿足如下Kalman濾波關(guān)于噪聲的基本假設(shè)條件

(2)

其中,Qk是半正定的,Rk是正定的,δkj為克羅內(nèi)克函數(shù)。

針對系統(tǒng)式(1)的Kalman濾波公式為

(3)

由量測新息及其均方差陣可構(gòu)造統(tǒng)計量[1]

(4)

其中,λk服從自由度為m的卡方分布,即λk～χ2(m)。在量測正常情況下,統(tǒng)計量λk的數(shù)值應(yīng)當(dāng)比較小;而如果量測出現(xiàn)異常,λk將變得較大,量測正常與否一般以某選定的閾值TDm作為判斷門限,即

(5)

這便是Kalman濾波的量測故障卡方檢測原理。在濾波過程中可以計算統(tǒng)計量λk,根據(jù)其大小實時監(jiān)測量測是否異常,進而決定是否進行量測更新,式(3)中的量測及其均方差陣更新方程可改寫為

(6)

顯然,當(dāng)χk=1即λk≤TDm時,進行正常的Kalman濾波量測更新;而當(dāng)χk=0即λk>TDm時,則放棄量測更新,達到隔離異常量測的效果。

2 新的軟卡方檢測方法

傳統(tǒng)卡方檢測方法有效的前提條件是濾波系統(tǒng)模型準(zhǔn)確無誤。在慣導(dǎo)/衛(wèi)導(dǎo)組合導(dǎo)航等實際應(yīng)用中,一般Kalman濾波的系統(tǒng)結(jié)構(gòu)參數(shù)建模會相對比較準(zhǔn)確且穩(wěn)定,而噪聲參數(shù)會由于運動狀態(tài)、建模不完善、系統(tǒng)老化或運行環(huán)境等原因而變化,難以完全精確建模,這樣就會使得卡方檢測閾值TDm難以嚴(yán)格地按理論方法準(zhǔn)確確定。閾值設(shè)置過大會造成故障檢測概率降低,存在將較多的異常值引入濾波量測的風(fēng)險,從而造成濾波誤差增大;閾值設(shè)置過小又會造成故障虛警概率增大,經(jīng)常性的虛警降低了量測利用率,量測修正作用減小也會降低濾波估計精度。當(dāng)然,對于高精度慣導(dǎo)系統(tǒng)而言,隨機性棄用衛(wèi)導(dǎo)定位量測50%以上,比如衛(wèi)導(dǎo)量測即使從1 Hz降為0.1 Hz,對組合導(dǎo)航性能影響也不大;但對于低精度慣導(dǎo)系統(tǒng),衛(wèi)導(dǎo)利用從1 Hz變?yōu)?.5 Hz對組合精度的影響可能就比較顯著了。

傳統(tǒng)卡方檢測方法的結(jié)果是二值化的,非0即1,再無任何中間狀態(tài)。該方法在雷達目標(biāo)探測等領(lǐng)域中主要以有/無為指標(biāo),其應(yīng)用是非常合理的,但是,針對組合導(dǎo)航等場合,在量測信息正常(信任)與異常(丟棄)之間還可能存在大量的中間狀態(tài)(可疑),僅簡單地使用卡方檢測二值化結(jié)果就不太合適了,這也有悖于目前常用的量測自適應(yīng)濾波和抗差濾波等方法的理念。比如,在Sage-Husa量測自適應(yīng)Kalman濾波中,通過量測新息的大小自動調(diào)整量測噪聲方差陣的大小,相當(dāng)于是將所有量測信息都進行了綜合利用,與正常量測相比,量測新息中等大小(可疑)時降低量測權(quán)重,新息明顯異常時量測權(quán)重很小,若權(quán)重趨于0則等效于棄用。論文根據(jù)自適應(yīng)濾波的新息利用特點,將式(6)改進為

(7)

(8)

值得特別指出的是,如果式(1)中量測是多維的,傳統(tǒng)卡方檢測將多維量測視為整體,只要有任何一個分量出現(xiàn)異常,卡方檢測方法也會同時降低其他正常量測分量的新息利用率,這種處理方式不是很合理,為了避免該缺陷,需對各量測分量逐一做卡方檢測。如果在Kalman濾波模型中,各量測分量之間是不相關(guān)的,即量測噪聲均方差陣Rk為對角線矩陣,則采用序貫濾波處理后,各序貫量測更新的卡方檢測就是相當(dāng)于對單個量測分量的逐一處理。如果Rk為非對角線矩陣,不妨記rk(i)為新息rk的第i(i=1,2,…,m)個分量,Ak(ii)為新息均方差陣Ak的第i行i列對角線元素,且記統(tǒng)計量

(9)

不難理解,各統(tǒng)計量λk(i)均服從自由度為1的卡方分布,即有λk(i)～χ2(1)。

參考式(7),推廣和建立由λk(i)構(gòu)造的Kalman濾波量測更新方法,如下

(10)

(11)

(12)

在式(10)中,修改了濾波增益Kk的計算方式,其目的是保證之后均方差陣Pk計算的對稱性,顯然,式(7)是式(10)中ρk各分量都相等時的特殊情形。理論上,ρk可取對稱正定陣,但為了簡便,一般取為如式(11)所示的對角陣。在式(12)中,參數(shù)TD1是自由度為1的卡方統(tǒng)計量λk(i)的故障檢測設(shè)置閾值,當(dāng)取顯著性水平為0.05時有TD1≈3.8。

3 仿真驗證

3.1 慣導(dǎo)/衛(wèi)導(dǎo)組合導(dǎo)航模型

以慣導(dǎo)/衛(wèi)導(dǎo)松組合導(dǎo)航為例進行仿真實驗驗證,系統(tǒng)狀態(tài)15維、量測3維,具體建模如下

(13)

其中

3.2 仿真條件

采用PSINS工具箱軟件模擬一段車載行駛軌跡,軌跡包含靜止、加減速、勻速、轉(zhuǎn)彎、爬坡等階段,總仿真時間約1 000 s,行駛里程約7.5 km。行車速度如圖1所示,軌跡相對位置變化的二維平面圖如圖2所示,圖中標(biāo)識“★”為載車行駛起始點。

圖1 載車行駛速度Fig.1 The vehicular velocities

圖2 載車行駛平面軌跡Fig.2 The vehicular trajectory

根據(jù)軌跡仿真生成慣性傳感器數(shù)據(jù)和衛(wèi)導(dǎo)定位數(shù)據(jù),其中慣性數(shù)據(jù)采樣100 Hz、衛(wèi)導(dǎo)測量1 Hz。再進行慣性導(dǎo)航解算和慣導(dǎo)/衛(wèi)導(dǎo)組合導(dǎo)航Kalman濾波解算,仿真過程中注入的各種誤差如表1所列。

表1 仿真誤差設(shè)置

衛(wèi)導(dǎo)的位置量測誤差(緯、經(jīng)、高,3個分量)均設(shè)置為零均值的高斯白噪聲,正常時間段內(nèi)方差為(1 m)2,以下兩個時間段除外:(1)200～400 s之間的噪聲方差設(shè)置為(10 m)2;(2)600～800 s之間設(shè)置為污染噪聲,以衛(wèi)導(dǎo)高度測量誤差為例,其表示為

(14)

其中,“w.p.”表示“以…概率”(with probability)之意。式(14)表示噪聲δH在正常情況下以90%的大概率服從方差為(1 m)2的高斯分布,視為正常噪聲;而在異常情況下以10%的小概率服從方差為(100 m)2的高斯分布,視為野值噪聲。

一組高度量測誤差仿真的示例如圖3所示,由圖可見,在200～400 s之間高度誤差總體波動變大,模擬噪聲方差變化;而在600～800 s之間個別誤差幅值跳變很大,模擬小概率的野值跳變。

圖3 衛(wèi)導(dǎo)高度測量誤差Fig.3 GNSS altitude measurement error

3.3 仿真結(jié)果

慣導(dǎo)/衛(wèi)導(dǎo)組合導(dǎo)航的姿態(tài)失準(zhǔn)角、速度誤差和位置誤差的結(jié)果如圖4所示,從圖中可以看出:在200～400 s之間隨著衛(wèi)導(dǎo)的量測噪聲增大,組合導(dǎo)航的誤差也相應(yīng)變大,該段誤差的量級與全程衛(wèi)導(dǎo)誤差方差都設(shè)置為(10 m)2的結(jié)果是基本一致的;在600～800 s之間狀態(tài)估計基本不受衛(wèi)導(dǎo)量測噪聲野值的影響,濾波器具有良好的野值隔離效果,與只存在小方差(1 m)2時的量測噪聲一樣,始終保持較高的濾波精度。

(a) 姿態(tài)誤差

(b) 速度誤差

(c) 位置誤差圖4 基于軟卡方檢測的估計誤差Fig.4 State estimation error based on SC2TKF

此外,還對本文新方法(SC2TKF)和傳統(tǒng)Sage-Husa量測噪聲自適應(yīng)濾波(SHAKF)作了對比仿真,進行了50次的蒙特卡洛仿真并統(tǒng)計各導(dǎo)航參數(shù)誤差,結(jié)果表明SC2TKF更加穩(wěn)定,其中緯度誤差的均方根(root mean square,RMS)誤差統(tǒng)計如圖5所示,其他誤差情況類似,不再一一列出。值得注意的是,在SHAKF中還有一個漸消因子需要精心設(shè)置(文中取b=0.5),若漸消因子設(shè)置不合適,對濾波結(jié)果有較大的負(fù)面影響。綜上,新方法無需設(shè)置任何參數(shù),具有使用方便和適應(yīng)性強等優(yōu)點。

圖5 不同濾波方法的緯度誤差比較Fig.5 Comparison of latitude error by using SC2TKF vs SHAKF

4 結(jié)論

自適應(yīng)Kalman濾波的方法很多,廣義上說,能根據(jù)實時量測自動調(diào)整濾波器參數(shù)并獲得良好狀態(tài)估計效果的方法均可稱為自適應(yīng)濾波,比如Sage-Husa自適應(yīng)濾波[2-3]、抗差自適應(yīng)濾波[4,6-7]、強跟蹤濾波[8]以及近年來一系列文獻中根據(jù)變分貝葉斯估計原理給出的RSTKF、MCKF、SSMKF和CERKF等眾多方法[9-13]。在組合導(dǎo)航實際系統(tǒng)中,除基于量測噪聲自適應(yīng)的經(jīng)典Sage-Husa自適應(yīng)濾波比較實用外,其他自適應(yīng)方法理論推導(dǎo)結(jié)果和仿真效果雖然較好,但往往前提假設(shè)過于理想或者參數(shù)設(shè)置繁瑣,難以適用于復(fù)雜高維系統(tǒng),因而少有實際應(yīng)用的報道。繼Sage-Husa自適應(yīng)濾波之后,論文提出的軟卡方檢測Kalman濾波方法有望在組合導(dǎo)航領(lǐng)域成為一種更加實用的自適應(yīng)濾波方法。