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

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

0 引言

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

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

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

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

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

(1)

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

(2)

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

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

(3)

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

(4)

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

(5)

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

(6)

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

2 新的軟卡方檢測(cè)方法

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

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

(7)

(8)

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

(9)

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

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

(10)

(11)

(12)

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

3 仿真驗(yàn)證

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

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

(13)

其中

3.2 仿真條件

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

圖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)測(cè)量1 Hz。再進(jìn)行慣性導(dǎo)航解算和慣導(dǎo)/衛(wèi)導(dǎo)組合導(dǎo)航Kalman濾波解算,仿真過程中注入的各種誤差如表1所列。

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

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

(14)

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

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

圖3 衛(wèi)導(dǎo)高度測(cè)量誤差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)的量測(cè)噪聲增大,組合導(dǎo)航的誤差也相應(yīng)變大,該段誤差的量級(jí)與全程衛(wèi)導(dǎo)誤差方差都設(shè)置為(10 m)2的結(jié)果是基本一致的;在600～800 s之間狀態(tài)估計(jì)基本不受衛(wèi)導(dǎo)量測(cè)噪聲野值的影響,濾波器具有良好的野值隔離效果,與只存在小方差(1 m)2時(shí)的量測(cè)噪聲一樣,始終保持較高的濾波精度。

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

(b) 速度誤差

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

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

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

4 結(jié)論

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