采用卡方檢驗(yàn)和牛頓插值的抗差卡爾曼濾波新算法

蔡保杰,邵 雷,李正杰

(空軍工程大學(xué)防空反導(dǎo)學(xué)院,西安,710051)

導(dǎo)航就是將運(yùn)載體從起始點(diǎn)引導(dǎo)到目的地的技術(shù)或方法，GPS和慣性導(dǎo)航結(jié)合的導(dǎo)航方法在當(dāng)今導(dǎo)航領(lǐng)域應(yīng)用廣泛[1-2]。在組合導(dǎo)航實(shí)際應(yīng)用環(huán)境中，由于建筑物的遮擋或其他隨機(jī)誤差的影響，會(huì)出現(xiàn)觀測(cè)值損壞的狀況，嚴(yán)重影響導(dǎo)航精度。當(dāng)衛(wèi)星接收的信號(hào)受到外界干擾時(shí)，載體狀態(tài)的解算將會(huì)出現(xiàn)粗差，粗差的存在將會(huì)使觀測(cè)噪聲的數(shù)學(xué)特性發(fā)生變化，殘差(新息)將不再服從標(biāo)準(zhǔn)的高斯分布。

對(duì)于觀測(cè)故障問(wèn)題[3]，常用的是傳統(tǒng)抗差卡爾曼濾波方法[4]。文獻(xiàn)[5]中先用假設(shè)檢驗(yàn)的方法檢測(cè)出故障觀測(cè)值的具體位置，再利用抗差因子對(duì)故障觀測(cè)值進(jìn)行降權(quán)處理，用設(shè)定的門限值代替系統(tǒng)噪聲值，可以較大程度的減小粗差的影響。文獻(xiàn)[6]中基于卡方檢驗(yàn)構(gòu)造了一個(gè)檢驗(yàn)函數(shù)，對(duì)不同的檢驗(yàn)函數(shù)值采用不同的公式計(jì)算新的觀測(cè)噪聲方差陣代替故障的觀測(cè)噪聲方差陣。文獻(xiàn)[7]通過(guò)降低濾波增益值來(lái)達(dá)到修正預(yù)測(cè)值的效果。文獻(xiàn)[8]在一步預(yù)測(cè)均方誤差陣上乘一個(gè)漸消因子，進(jìn)行自適應(yīng)濾波。文獻(xiàn)[9]提出了錯(cuò)誤預(yù)警率和故障探測(cè)率的概念，對(duì)故障的定位相對(duì)更加精確。上述方法都能一定程度的減小粗差帶來(lái)的影響，但也存在不足之處。檢測(cè)系統(tǒng)并不能保證百分之百的正確率，當(dāng)檢測(cè)系統(tǒng)出現(xiàn)誤檢時(shí)，用門限值代替正確的噪聲值，會(huì)造成正確的觀測(cè)值誤差增大，降低了系統(tǒng)的穩(wěn)定性。針對(duì)這種情況，本文提出一種基于牛頓插值的抗差方法，利用前幾個(gè)時(shí)刻的噪聲值組成的牛頓插值多項(xiàng)式進(jìn)行外延，得到當(dāng)前時(shí)刻的觀測(cè)噪聲值，使系統(tǒng)穩(wěn)定性更高，提高了組合導(dǎo)航系統(tǒng)的抗差能力，并通過(guò)實(shí)驗(yàn)對(duì)此法進(jìn)行了驗(yàn)證，取得了較好的抗差效果。

1 卡爾曼濾波算法與系統(tǒng)建模

1.1 卡爾曼濾波算法

卡爾曼濾波是20世紀(jì)90年代提出的一種最優(yōu)估計(jì)的方法，是組合導(dǎo)航系統(tǒng)經(jīng)常用到的一種濾波方法?？柭鼮V波經(jīng)常用在SINS/GNSS中來(lái)實(shí)現(xiàn)衛(wèi)星導(dǎo)航系統(tǒng)和捷聯(lián)慣導(dǎo)系統(tǒng)的信息融合[10-13]。離散卡爾曼濾波基本公式為：

狀態(tài)一步預(yù)測(cè)：

(1)

狀態(tài)估計(jì)：

(2)

濾波增益：

(3)

一步預(yù)測(cè)均方誤差：

(4)

估計(jì)均方誤差：

(5)

式中:Qk為k時(shí)刻的系統(tǒng)噪聲方差陣;Rk為k時(shí)刻觀測(cè)噪聲方差陣。

1.2 組合導(dǎo)航系統(tǒng)建模

SINS/GNSS組合導(dǎo)航是近年來(lái)導(dǎo)航領(lǐng)域最常使用的一種方法，SINS可以輔助GNSS導(dǎo)航，而GNSS也可以對(duì)SINS不斷地進(jìn)行修正，避免了捷聯(lián)慣導(dǎo)誤差隨時(shí)間增大的問(wèn)題。在SINS/GNSS組合濾波器中，一般采用15維的誤差狀態(tài)建立狀態(tài)方程，狀態(tài)向量為：

(6)

Xk=Ak,k-1Xk-1+W

(7)

式中：Ak,k-1為狀態(tài)轉(zhuǎn)移矩陣，當(dāng)狀態(tài)向量為15維時(shí)，Ak,k-1是一個(gè)15維的方陣，因篇幅限制，且松組合導(dǎo)航系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣大同小異，本文略去狀態(tài)轉(zhuǎn)移矩陣的列寫，W為系統(tǒng)噪聲。SINS/GNSS的觀測(cè)模型取GNSS和SINS輸出的位置和速度之差作為觀測(cè)值，觀測(cè)向量為：

式中：v=[vx,vy,vz];r=[L,λ,h]。

量測(cè)方程離散化后的形式為：

Zk=HkXk+Vk

(8)

式中：Hk為量測(cè)矩陣，Hk為一個(gè)6行15列的矩陣;Vk為量測(cè)噪聲。

2 采用卡方檢驗(yàn)的故障診斷方法

(9)

引起對(duì)量測(cè)的估計(jì)誤差為：

(10)

(11)

若系統(tǒng)發(fā)生故障，量測(cè)信息存在較大偏差，則殘差均值不再為零。設(shè)統(tǒng)計(jì)量：

(12)

式中：Dk為測(cè)量殘差的協(xié)方差矩陣，即：

(13)

Sk服從自由度為d的卡方分布，d為觀測(cè)序列的維數(shù)，利用卡方分布進(jìn)行假設(shè)檢驗(yàn)，設(shè)H0為原假設(shè)，H1為備擇假設(shè)，在此問(wèn)題中，H0、H1分別定義為：

(14)

若當(dāng)前時(shí)刻觀測(cè)值發(fā)生故障，可以對(duì)當(dāng)前時(shí)刻的所有觀測(cè)值做抗差處理，但這會(huì)損壞無(wú)故障的觀測(cè)值，造成不必要的誤差。為了充分利用無(wú)故障的觀測(cè)值，提高抗差效果，本文采用單時(shí)刻單維度的檢驗(yàn)方法對(duì)故障觀測(cè)值進(jìn)行更精確的定位。取假設(shè)檢驗(yàn)統(tǒng)計(jì)量為：

(15)

式中：Sk(i)為k時(shí)刻檢驗(yàn)序列的第i個(gè)值;Jk(i)為k時(shí)刻殘差序列的第i個(gè)值。有：

(16)

圖1 閾值選取過(guò)小示例

圖2 閾值選取過(guò)大示例

表1 顯著性水平和閾值的關(guān)系

顯著性水平α閾值T(1)顯著性水平α閾值T(1)0.9750.0010.1002.7060.9500.0040.0503.8410.0900.0160.0255.0240.7500.1020.0106.6350.2501.3230.0057.879

3 基于牛頓插值的抗差方法

在處理數(shù)學(xué)問(wèn)題時(shí)，由于一些函數(shù)太復(fù)雜，或者已知函數(shù)是一張表格，就可以通過(guò)一些離散值，按照一定的要求，構(gòu)造此函數(shù)的一個(gè)近似表達(dá)式。代數(shù)插值的幾何意義是：通過(guò)平面上給定的N+1個(gè)互異點(diǎn)(xi,yi)(i=0,1,…,N)做一條代數(shù)曲線y=P(x),近似的表示y=f(x)。構(gòu)造插值多項(xiàng)式的方法不唯一，綜合考慮插值精度，構(gòu)造簡(jiǎn)便性等因素，本文將使用牛頓插值進(jìn)行插值計(jì)算。牛頓插值多項(xiàng)式[16]的表達(dá)式為：

f(x)=N(x)+R(x)

(17)

式中：N(x)為牛頓插值公式;R(x)為牛頓插值多項(xiàng)式的余項(xiàng)，本文不做研究。牛頓插值公式為：

Nn(x)=f(x0)+(x-x0)f[x1+x0]+…+

(x-x0)(x-x1)…(x-xn-1)f[xn,xn-1,…,x0]

(18)

式中：n為插值多項(xiàng)式的階數(shù)。

在經(jīng)典組合導(dǎo)航系統(tǒng)中，可以每隔一段固定的時(shí)間進(jìn)行一次導(dǎo)航濾波過(guò)程，使用等距節(jié)點(diǎn)的插值公式更加簡(jiǎn)便，其表達(dá)式為：

(19)

式中：fi=f(xi);Δnfi=Δ(Δn-1fi)=Δn-1fi+1-Δn-1fi;Δfi=fi+1-fi，設(shè)h為步長(zhǎng)，當(dāng)前時(shí)刻值為xk,起始時(shí)刻值為x0,t=(xk-x0)/h。將此牛頓插值表達(dá)式轉(zhuǎn)換為另一種表達(dá)形式[17]，令：

(20)

(21)

將δ設(shè)初值為2，步長(zhǎng)為+0.5，末值設(shè)為10，依次將δ=2,2.5,3,…代入改進(jìn)的插值多項(xiàng)式，通過(guò)大量的對(duì)比分析，δ=4時(shí)效果最佳，因此，本文將以δ=4來(lái)進(jìn)行仿真分析。

Ck=diag[c1,c2,…,ci]

(22)

(23)

相應(yīng)的觀測(cè)方程變?yōu)椋?/p>

(24)

插值多項(xiàng)式余項(xiàng)公式說(shuō)明插值節(jié)點(diǎn)越多，一般說(shuō)來(lái)誤差越小，函數(shù)逼近越好，但這也不是絕對(duì)的。適當(dāng)?shù)靥岣卟逯刀囗?xiàng)式的次數(shù)，有可能提高計(jì)算結(jié)果的準(zhǔn)確程度，但并非插值多項(xiàng)式的次數(shù)越高越好。當(dāng)插值節(jié)點(diǎn)增多時(shí)，不能保證非節(jié)點(diǎn)處的插值精度得到改善，有時(shí)反而誤差更大。龍格現(xiàn)象表明，大范圍內(nèi)使用高次插值,逼近的效果往往是不理想的。根據(jù)經(jīng)驗(yàn)，牛頓插值在四階時(shí)精度較高，出現(xiàn)龍格現(xiàn)象的概率也比較小，所以本文將以四階牛頓插值多項(xiàng)式來(lái)進(jìn)行實(shí)驗(yàn)驗(yàn)證。基于四階牛頓插值的抗差濾波方法為：

(25)

(26)

此時(shí)：

4Vk-3(i)-4Vk-4(i)]/4

(27)

觀測(cè)方程為：

(28)

4 仿真與驗(yàn)證

4.1 仿真軌跡與INS、GPS輸出數(shù)據(jù)的生成

圖3 飛行器軌跡

仿真總時(shí)長(zhǎng)為500 s，周期為1 s，位置量測(cè)噪聲分別為：經(jīng)緯度噪聲是以 0.000 15°為基數(shù)的白噪聲，高度噪聲是以5 m為基數(shù)的白噪聲；速度噪聲均為以1 km/h為基數(shù)的白噪聲；姿態(tài)角噪聲為以0.01°為基數(shù)的白噪聲。為驗(yàn)證本抗差算法的有效性，制造量測(cè)不準(zhǔn)確的外部環(huán)境，仿真在每隔10 s的時(shí)刻加入一個(gè)噪聲放大系數(shù)m,使此時(shí)刻的噪聲含有m倍粗差。

4.2 仿真對(duì)比分析

為了便于比較基于卡方檢驗(yàn)和牛頓插值抗差方法的效果，本文分別對(duì)使用本方法和不使用本方法進(jìn)行了仿真，仿真結(jié)果見(jiàn)圖4～6。

圖4 基于牛頓插值抗差濾波與一般濾波方法位置誤差

圖5 基于牛頓插值抗差濾波與一般濾波方法前段時(shí)間位置誤差

圖6 基于牛頓插值抗差濾波與一般濾波方法速度誤差

在實(shí)際應(yīng)用中，當(dāng)系統(tǒng)沒(méi)有抗差算法時(shí)，在GNSS信號(hào)受到干擾時(shí)間段內(nèi)會(huì)由INS單獨(dú)工作，為了驗(yàn)證基于牛頓插值的抗差方法的有效性，對(duì)使用本方法后和只使用INS進(jìn)行了仿真對(duì)比，仿真結(jié)果見(jiàn)圖7～9。

圖7 基于牛頓插值抗差濾波與一般濾波方法前段時(shí)間速度誤差

圖8 基于牛頓插值抗差濾波方法與只使用INS位置誤差對(duì)比

圖9 基于牛頓插值抗差濾波方法與只使用INS速度誤差對(duì)比

從圖4、圖6中可以看出，對(duì)于含有粗差的觀測(cè)量不使用抗差濾波的話，由于量測(cè)噪聲的急劇變化，量測(cè)量的值發(fā)生很大變化，從而使濾波精度大大降低，東向位置誤差粗略范圍0～-2 000 m，北向位置誤差粗略范圍0～1 000 m，天向位置誤差粗略范圍0～200 m；東向速度誤差粗略范圍0～2 m/s，北向速度誤差粗略范圍-5～0 m/s，天向速度誤差粗略范圍-2～2 m/s。在使用了基于卡方檢驗(yàn)和牛頓插值的抗差濾波方法后，此時(shí)刻的量測(cè)噪聲會(huì)根據(jù)前幾個(gè)時(shí)刻的噪聲值進(jìn)行重新設(shè)計(jì)，使量測(cè)噪聲恢復(fù)正常，使位置誤差和速度誤差減小并趨于穩(wěn)定，東向位置誤差為-13～25 m，北向位置誤差為-10～26 m，天向位置誤差為-18～19 m，東向速度誤差為-0.4～0.6 m/s，北向速度誤差為-0.8～1.5 m/s，天向位置誤差為-1～1.2 m/s。為了更直觀地展現(xiàn)出基于牛頓插值的抗差濾波的有效性，截取了前100 s的數(shù)據(jù)繪制圖像，如圖5、圖7所示，一般濾波方法的位置誤差在-20～20 m范圍內(nèi)波動(dòng)，速度東向、北向誤差在-0.5～0.5 m/s波動(dòng)，天向速度誤差在-1～1 m/s之間波動(dòng)，而使用本文所提出的抗差方法所得的誤差值均比一般濾波方法得出的誤差值要小。

從圖8、圖9中可以看到，由于慣性導(dǎo)航具有累積誤差，隨著時(shí)間的推移，只使用慣性導(dǎo)航方法無(wú)論是位置誤差還是速度誤差都會(huì)慢慢增大，與此相比，本文提出的基于牛頓插值的抗差濾波方法能較好的使誤差維持在一個(gè)較小的范圍內(nèi)，使導(dǎo)航精度得到提高。圖9中，由于在衛(wèi)星導(dǎo)航數(shù)據(jù)已加入噪聲的前提下，觀測(cè)值又加入噪聲Vk，濾波后的天向速度出現(xiàn)不穩(wěn)定的情況，對(duì)導(dǎo)航精度帶來(lái)一定影響。但天向速度誤差值仍在-2～2 m/s這樣一個(gè)較小的范圍內(nèi)，且只有天向速度誤差出現(xiàn)不穩(wěn)定的情況。綜合對(duì)比來(lái)看，基于牛頓插值的抗差卡爾曼濾波新算法仍不失為一種有效的抗差濾波方法。

5 結(jié)語(yǔ)

本文針對(duì)SINS/GNSS組合導(dǎo)航中觀測(cè)值中會(huì)出現(xiàn)較大誤差的問(wèn)題，分析了產(chǎn)生誤差的原因，根據(jù)含有粗差的殘差向量不再服從高斯分布的情況，對(duì)故障進(jìn)行檢測(cè)，通過(guò)實(shí)驗(yàn)驗(yàn)證來(lái)分析確定閾值的大小，提高了故障定位的精度。然后，提出了一種基于牛頓插值的抗差卡爾曼濾波新算法，并加入了改善因子使修正后的量測(cè)值更加穩(wěn)定。最后，通過(guò)大量的仿真分析，該算法使誤差控制在一個(gè)很小的范圍內(nèi)，具有較好的抗差效果。該算法提高了抗差濾波系統(tǒng)的穩(wěn)定性，計(jì)算簡(jiǎn)單，易于工程實(shí)現(xiàn)。