基于改進(jìn)自適應(yīng)無跡卡爾曼濾波的國產(chǎn)民機(jī)導(dǎo)航數(shù)據(jù)濾波算法

楊軍利， 王立新， 錢 宇， 劉 瑜

(中國民用航空飛行學(xué)院飛行技術(shù)學(xué)院， 廣漢 618307)

國產(chǎn)飛機(jī)的導(dǎo)航主要依靠全球定位系統(tǒng)(global positioning system，GPS)/慣性導(dǎo)航系統(tǒng)(inertial navigation system，INS)的組合導(dǎo)航系統(tǒng)[1]，將來隨北斗技術(shù)日趨完善，有望早日實(shí)現(xiàn)導(dǎo)航技術(shù)的國產(chǎn)化。組合導(dǎo)航系統(tǒng)以抗干擾性強(qiáng)、自主性完好及定位精度高等優(yōu)點(diǎn)著稱，它是將GPS和INS二者進(jìn)行了優(yōu)勢互補(bǔ)。然而即使導(dǎo)航系統(tǒng)的精度再高，信號的傳輸難免會受到傳輸路徑過長等因素的影響，使其包含一定的雜波，從而導(dǎo)致導(dǎo)航精度降低，為解決此問題，需對導(dǎo)航數(shù)據(jù)進(jìn)行濾波處理。

Kalman濾波[2-3]是組合導(dǎo)航數(shù)據(jù)處理算法之一，它對平穩(wěn)隨機(jī)過程、多維隨機(jī)過程及非平穩(wěn)的隨機(jī)過程均有很好的濾波效果，可最大限度降低噪聲等干擾信息對系統(tǒng)的影響，且計(jì)算過程較簡單，但Kalman濾波只對包含較特殊噪聲系統(tǒng)(如高斯白噪聲系統(tǒng))的線性系統(tǒng)有效，導(dǎo)致其適用性受限。為解決非線性系統(tǒng)濾波問題，Bucy等提出擴(kuò)展卡爾曼濾波(extended Kalman filter，EKF)，它采用非線性系統(tǒng)一階線性化的方法，但此方法會產(chǎn)生近似誤差，導(dǎo)致濾波發(fā)散，精度降低[4]。無跡卡爾曼濾波(unscented Kalman filter，UKF)以無跡變換(unscented transform，UT)為基礎(chǔ)，結(jié)合Kalman濾波基本框架，與EKF相比，精度更高[5-7]，但它的缺點(diǎn)在于對系統(tǒng)初始值要求較高，不準(zhǔn)確的系統(tǒng)噪聲、擾動異常等因素均會影響濾波精度。將UKF與Sage-Husa噪聲濾波器結(jié)合構(gòu)成改進(jìn)自適應(yīng)無跡卡爾曼濾波(adaptive unscented Kalman filter，AUKF)[8-9]，這樣可降低UKF對系統(tǒng)初始值的要求，準(zhǔn)確估計(jì)出狀態(tài)預(yù)測的協(xié)方差，同時(shí)濾波器可實(shí)時(shí)更新和修正系統(tǒng)噪聲，有效解決了導(dǎo)航數(shù)據(jù)濾波精度低和系統(tǒng)不穩(wěn)定的問題，但傳統(tǒng)AUKF的Sage-Husa噪聲濾波器在更新隨機(jī)變量和觀測變量系統(tǒng)白噪聲協(xié)方差時(shí)，當(dāng)二者的推導(dǎo)式中非零元素的絕對值大于某特定閾值，或自協(xié)方差元素出現(xiàn)負(fù)值時(shí)，容易導(dǎo)致隨機(jī)變量系統(tǒng)白噪聲協(xié)方差失去非負(fù)性或者觀測變量系統(tǒng)白噪聲協(xié)方差失去正定性，從而使得AUKF濾波發(fā)散，精度降低。

現(xiàn)對AUKF算法的Sage-Husa噪聲估計(jì)器進(jìn)行改進(jìn)以克服濾波發(fā)散的問題。對比改進(jìn)AUKF算法、維納濾波器及小波閾值法對國產(chǎn)ARJ21飛機(jī)實(shí)際運(yùn)行導(dǎo)航數(shù)據(jù)的濾波效果，以說明改進(jìn)AUKF算法可有效提高國產(chǎn)民機(jī)導(dǎo)航數(shù)據(jù)的精度。

1 改進(jìn)AUKF算法

1.1 改進(jìn)Sage-Husa濾波器

Sage-Husa[10]實(shí)時(shí)估計(jì)UKF的隨機(jī)變量和觀測變量系統(tǒng)白噪聲協(xié)方差的遞推過程為

(1)

(2)

(3)

(4)

(5)

(6)

(7)

(8)

1.2 UKF算法

UKF系統(tǒng)噪聲為白噪聲，并求解狀態(tài)向量和協(xié)方差矩陣，推導(dǎo)過程包括系統(tǒng)初始化、時(shí)刻更新、測量更新及濾波值更新。

1.2.1 UT變換

UT變換由三部分組成：選取采樣點(diǎn)、計(jì)算采樣點(diǎn)均值和協(xié)方差、計(jì)算采樣點(diǎn)權(quán)值。

選取2n+1個(gè)采樣點(diǎn)，n為狀態(tài)向量維數(shù)。令采樣點(diǎn)與原始狀態(tài)分布的均值和協(xié)方差相等，具體推導(dǎo)為

(9)

(10)

式(10)中：m為均值；c為協(xié)方差；β為非負(fù)權(quán)系數(shù)。

1.2.2 UKF推導(dǎo)

UKF利用包含白噪聲的隨機(jī)變量和觀測變量對非線性系統(tǒng)進(jìn)行描述，非線性系統(tǒng)可表示為

(11)

式(11)中：f為非線性狀態(tài)方程函數(shù)；h為非線性觀測方程函數(shù)；X(t)為t時(shí)刻系統(tǒng)狀態(tài)值；W(t)為隨機(jī)變量X(t+1)的白噪聲，設(shè)其協(xié)方差陣為Q；V(t)為觀測變量Z(t)的白噪聲，設(shè)其協(xié)方差陣為R。

改進(jìn)的AUKF算法步驟如下。

(1)計(jì)算采樣點(diǎn)權(quán)值，即

(12)

(2)計(jì)算采樣點(diǎn)一步預(yù)測值，即

Xi(t+1)=f[t,Xi(t)],i=1,2,…,2n+1

(13)

(3)計(jì)算狀態(tài)隨機(jī)向量一步預(yù)測和協(xié)方差，即

(14)

(15)

(4)對式(14)的預(yù)測值進(jìn)行二次UT變換，即

(16)

(5)將二次UT變換得到的預(yù)測值代入系統(tǒng)觀測方程，計(jì)算出觀測值的預(yù)測值，即

Zi(t+1)=h[Xi(t+1)]

(17)

(6)計(jì)算系統(tǒng)預(yù)測均值和協(xié)方差，即

(18)

(19)

(20)

(7)計(jì)算Kalman濾波增益，即

(21)

(8)更新系統(tǒng)狀態(tài)和協(xié)方差，即

(22)

P(t+1)=P(t+1)-K(t+1)PZkZkKT(t+1)

(23)

利用改進(jìn)AUKF[14]實(shí)現(xiàn)國產(chǎn)民用飛機(jī)導(dǎo)航數(shù)據(jù)濾波的算法流程如圖1所示。

圖1 改進(jìn)AUKF導(dǎo)航數(shù)據(jù)濾波算法流程圖Fig.1 Flow chart of improved AUKF navigation data filtering algorithm

2 維納濾波器

維納濾波[15-16]是一種最佳非遞歸估計(jì)，利用最小均方誤差法進(jìn)行估計(jì)。將信號和噪聲同時(shí)輸入，通過濾波器處理，最終在輸出端較為精準(zhǔn)的信號，而噪聲被盡可能地抑制。維納濾波器的具體推導(dǎo)過程如下。

已知希望輸出為

(24)

均方誤差為

(25)

對式(25)中的h(m)(m=0,1,…,N-1)進(jìn)行求導(dǎo)得

(26)

式中：Rxs(j)=E[s(n)x(n-j)]，Rxx(j-m)=E[x(n-m)x(n-j)]，j=0,1,…,N-1。

通過上述推導(dǎo)可得N個(gè)線性方程，即

(27)

將式(27)寫為矩陣形式可得

(28)

式中：H=[h(0)h(1)…h(huán)(N-1)]T為濾波器的系數(shù)；Rxs=[Rxs(0),…,Rxs(N-1)]T為互相關(guān)序列。

自相關(guān)矩陣為

(29)

通過上述推導(dǎo)過程可知，維納濾波器實(shí)質(zhì)就是求解維納-霍夫方程。

3 小波閾值濾波

小波分析是由傅里葉分析發(fā)展而來。小波閾值濾波認(rèn)為對于小波系數(shù)包含有信號的重要信息，其幅值較大，但數(shù)目較少，然而信號中的噪聲對于小波系數(shù)是一致分布的，個(gè)數(shù)較多，但幅值較小。

采用小波閾值法濾波的步驟[17-18]為以下3步。

(1)選擇一個(gè)小波，并確定其分解的層次M，采用離散小波變換對噪聲信號進(jìn)行M層小波分解，并得到不同的尺度系數(shù)。

(2)對這M層高頻系數(shù)進(jìn)行閾值量化處理。

(3)根據(jù)小波分解的第M層的低頻系數(shù)和經(jīng)量化處理之后的第一層到第M層的高頻系數(shù)，進(jìn)行信號的小波重構(gòu)。

常用的閾值量化處理方法有硬閾值和軟閾值，具體表達(dá)式如下。

硬閾值法：

(30)

軟閾值法：

(31)

小波閾值的選取規(guī)則如下。

(1)固定閾值T1。假設(shè)含噪信號f(t)在尺度1～m(1

(32)

(2)Stein無偏風(fēng)險(xiǎn)閾值T2。它是一種基于Stein的無偏似然估計(jì)原理的自適應(yīng)閾值選擇。設(shè)W為一向量，它所包含的元素是小波系數(shù)的平方并將值進(jìn)行由大到小的順序排列，記為T2。設(shè)風(fēng)險(xiǎn)向量為R,它的元素為

i=1,2,…,n

(33)

式(33)中：rb為R元素中的最小值，令其為風(fēng)險(xiǎn)值；由rb的下標(biāo)求出對應(yīng)的ωn。則

(34)

(3)試探法的Stein無偏風(fēng)險(xiǎn)閾值T3。試探法的Stein無偏風(fēng)險(xiǎn)閾值是以上兩種方法的綜合，是一種最優(yōu)預(yù)測變量閾值選擇。若信噪比較小，適用這種固定的閾值。設(shè)W為n個(gè)小波系數(shù)的平方和，令σ=middle(W1,k,0≤k≤2J-1-1)/0.674 5，具體閾值選擇規(guī)則為

(35)

(36)

(4)極大極小準(zhǔn)則閾值T4。這種方法采用的也是固定閾值，產(chǎn)生一個(gè)最小均方誤差的極值，而不是誤差。具體的閾值選取規(guī)則為

(37)

σ=middle(W1,k,0≤k≤2J-1-1)/0.674 5

(38)

式中：n為小波系數(shù)的個(gè)數(shù)；σ為噪聲信號的標(biāo)準(zhǔn)差；W1,k表示尺度為1的小波系數(shù)。

上述四種閾值選取規(guī)則各有優(yōu)缺點(diǎn)，為選擇更合適的閾值選取規(guī)則，分別對比4種閾值選取規(guī)則以及軟閾值和硬閾值下的濾波效果，具體如圖2和圖3所示。

圖2 緯度軟閾值與硬閾值濾波對比Fig.2 Comparison of soft threshold and hard threshold latitude filtering

圖3 不同閾值選取規(guī)則的導(dǎo)航數(shù)據(jù)濾波對比Fig.3 Comparison of navigation data filtering with different threshold selection rule

由圖2可以看出，采用軟閾值量化處理濾波效果更好。由圖3可以看出，導(dǎo)航數(shù)據(jù)在4種閾值選取規(guī)則下的濾波效果不同，經(jīng)對比驗(yàn)證發(fā)現(xiàn)采用試探法的Stein無偏風(fēng)險(xiǎn)閾值T3濾波效果更好。因此，最終確定小波閾值濾波采用軟閾值量化下試探法的Stein無偏風(fēng)險(xiǎn)閾值進(jìn)行濾波。

4 仿真算例

為了驗(yàn)證研究提出的導(dǎo)航數(shù)據(jù)濾波算法的有效性，利用國產(chǎn)ARJ21飛機(jī)的實(shí)際運(yùn)行數(shù)據(jù)進(jìn)行仿真驗(yàn)證，樣本數(shù)據(jù)分別為高度、經(jīng)度及緯度，3組樣本數(shù)據(jù)量均為200。

利用MATLAB分別編程改進(jìn)AUKF、UKF、維納濾波器及小波閾值法，并將這4種算法的濾波效果進(jìn)行對比分析。仿真是將實(shí)際運(yùn)行數(shù)據(jù)作為真實(shí)數(shù)據(jù)，給真實(shí)數(shù)據(jù)中加入信噪比為5 dB的高斯白噪聲構(gòu)造出噪聲數(shù)據(jù)，4種算法濾波效果擬合曲線如圖4所示。

由圖4可以看出，3組樣本數(shù)據(jù)通過改進(jìn)AUKF算法濾波效果明顯優(yōu)于其他3種算法，改進(jìn)AUKF算法的濾波曲線更接近真實(shí)曲線，說明該算法在提高導(dǎo)航數(shù)據(jù)濾波精度的同時(shí)能更好地保證濾波數(shù)據(jù)的可靠性。為更清晰比較不同算法的濾波效果，對不同算法濾波估計(jì)誤差進(jìn)行了對比分析，即計(jì)算不同時(shí)刻估計(jì)值與真實(shí)數(shù)據(jù)的差值，得到均方根(root mean squared，RMS)和均方根誤差(root mean squared error，RMSE)，即

(39)

式(39)中：N為樣本數(shù)據(jù)量。導(dǎo)航數(shù)據(jù)濾波誤差擬合曲線如圖5所示。

圖5 導(dǎo)航數(shù)據(jù)濾波估計(jì)誤差擬合曲線Fig.5 Estimation error fitting curve of navigation data filtering

由圖5可以看出，小波閾值法和維納濾波器的誤差明顯高于改進(jìn)AUKF和UKF算法，同時(shí)改進(jìn)AUKF算法的濾波誤差又小于UKF算法，綜合說明改進(jìn)AUKF算法較其他3種算法可靠性更好，濾波精度更高。為了衡量估計(jì)誤差的整體水平，研究利用平均相對誤差(mean relative error，MRE)和RMSE對擬合曲線進(jìn)行分析，3組樣本數(shù)據(jù)擬合曲線的評估指標(biāo)值如表1～表3所示。

表1 高度濾波效果評估Table 1 Evaluation of height filtering effect

表2 緯度濾波效果評估Table 2 Evaluation of latitude filtering effect

表3 經(jīng)度濾波效果評估Table 3 Evaluation of longitude filtering effect

由表1～表3可以看出，3組樣本數(shù)據(jù)利用改進(jìn)AUKF算法濾波得到的MRE和RMSE均小于其他3種算法，且4種算法所得誤差均在5%合理誤差范圍之內(nèi)，說明4種算法濾波降噪均有效，而改進(jìn)AUKF濾波后的數(shù)據(jù)值與真實(shí)數(shù)據(jù)偏差更小，即改進(jìn)AUKF算法的濾波精度更高，所得導(dǎo)航數(shù)據(jù)更可靠，利用其進(jìn)行濾波的數(shù)據(jù)定位精度高。

5 結(jié)論

國產(chǎn)民用飛機(jī)導(dǎo)航數(shù)據(jù)由于傳感器記錄過程中可能引入噪聲等因素導(dǎo)致數(shù)據(jù)包含雜波，為了提高導(dǎo)航定位精度需對導(dǎo)航數(shù)據(jù)進(jìn)行濾波處理，研究采用改進(jìn)Sage-Husa算法的AUKF算法對導(dǎo)航數(shù)據(jù)進(jìn)行濾波處理，所得結(jié)論如下。

(1)UKF算法不像EKF算法還需計(jì)算復(fù)雜的Jacobian矩陣，運(yùn)算較為簡單，且對非線性系統(tǒng)和線性系統(tǒng)均有很好的濾波效果，但它在處理非線性系統(tǒng)問題時(shí)需提前確定系統(tǒng)噪聲特性，在處理實(shí)際問題中，往往無法提前得知系統(tǒng)噪聲特性，致使濾波精度降低。

(2)改進(jìn)AUKF算法克服了更新過程噪聲和觀測噪聲時(shí)，導(dǎo)致過程噪聲失去非負(fù)定性或者觀測噪聲失去正定性而導(dǎo)致濾波發(fā)散的問題，同時(shí)它還克服了系統(tǒng)模型直接線性化帶來系統(tǒng)損耗的問題，以及通過對未知系統(tǒng)噪聲特性的實(shí)時(shí)估計(jì)，以修正系統(tǒng)噪聲變化對濾波的影響，較其他3種濾波算法濾波精度更高，有效提高了導(dǎo)航數(shù)據(jù)的可靠性。因此，本研究在提高國產(chǎn)民機(jī)導(dǎo)航數(shù)據(jù)精度方面具有重要意義。