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

蔡保杰,邵 雷,李正杰

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

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

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

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

1.1 卡爾曼濾波算法

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

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

(1)

狀態(tài)估計：

(2)

濾波增益：

(3)

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

(4)

估計均方誤差：

(5)

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

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

SINS/GNSS組合導(dǎo)航是近年來導(dǎo)航領(lǐng)域最常使用的一種方法，SINS可以輔助GNSS導(dǎo)航，而GNSS也可以對SINS不斷地進行修正，避免了捷聯(lián)慣導(dǎo)誤差隨時間增大的問題。在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維時，Ak,k-1是一個15維的方陣，因篇幅限制，且松組合導(dǎo)航系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣大同小異，本文略去狀態(tài)轉(zhuǎn)移矩陣的列寫，W為系統(tǒng)噪聲。SINS/GNSS的觀測模型取GNSS和SINS輸出的位置和速度之差作為觀測值，觀測向量為：

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

量測方程離散化后的形式為：

Zk=HkXk+Vk

(8)

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

2 采用卡方檢驗的故障診斷方法

(9)

引起對量測的估計誤差為：

(10)

(11)

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

(12)

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

(13)

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

(14)

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

(15)

式中：Sk(i)為k時刻檢驗序列的第i個值;Jk(i)為k時刻殘差序列的第i個值。有：

(16)

圖1 閾值選取過小示例

圖2 閾值選取過大示例

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

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

(17)

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

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

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

(18)

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

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

(19)

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

(20)

(21)

將δ設(shè)初值為2，步長為+0.5，末值設(shè)為10，依次將δ=2,2.5,3,…代入改進的插值多項式，通過大量的對比分析，δ=4時效果最佳，因此，本文將以δ=4來進行仿真分析。

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

(22)

(23)

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

(24)

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

(25)

(26)

此時：

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

(27)

觀測方程為：

(28)

4 仿真與驗證

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

圖3 飛行器軌跡

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

4.2 仿真對比分析

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

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

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

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

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

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

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

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

從圖4、圖6中可以看出，對于含有粗差的觀測量不使用抗差濾波的話，由于量測噪聲的急劇變化，量測量的值發(fā)生很大變化，從而使濾波精度大大降低，東向位置誤差粗略范圍0～-2 000 m，北向位置誤差粗略范圍0～1 000 m，天向位置誤差粗略范圍0～200 m；東向速度誤差粗略范圍0～2 m/s，北向速度誤差粗略范圍-5～0 m/s，天向速度誤差粗略范圍-2～2 m/s。在使用了基于卡方檢驗和牛頓插值的抗差濾波方法后，此時刻的量測噪聲會根據(jù)前幾個時刻的噪聲值進行重新設(shè)計，使量測噪聲恢復(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)波動，速度東向、北向誤差在-0.5～0.5 m/s波動，天向速度誤差在-1～1 m/s之間波動，而使用本文所提出的抗差方法所得的誤差值均比一般濾波方法得出的誤差值要小。

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

5 結(jié)語

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