Sage-Husa隨機(jī)加權(quán)無跡卡爾曼濾波及其在導(dǎo)航中的應(yīng)用

侯 俊，朱長青，2，閻海峰，3，高社生

(1.西北工業(yè)大學(xué) 自動化學(xué)院，西安 710072；2.空軍裝備部 科研訂貨部機(jī)載處，北京 100843；3.中航工業(yè)集團(tuán) 深圳南航電子工業(yè)有限公司，廣東 深圳 518052)

1 引言

無跡卡爾曼濾波(unscented Kalman filter， UKF)是以無跡變換為基礎(chǔ)，以標(biāo)準(zhǔn)卡爾曼濾波算法為框架，采用確定性采樣的一種近似線性最小方差估計方法。該算法具有實(shí)現(xiàn)簡單、濾波精度高、收斂性好等優(yōu)點(diǎn)。然而UKF要求精確已知噪聲的先驗統(tǒng)計特性，在實(shí)際應(yīng)用中，受到試驗樣本的限制或內(nèi)外部不確定因素的影響，噪聲的先驗統(tǒng)計是未知或不準(zhǔn)確的，使得傳統(tǒng)UKF的應(yīng)用受到限制。Sage濾波不需要給出動力學(xué)模型噪聲先驗協(xié)方差矩陣，就可以估計出狀態(tài)預(yù)報向量的協(xié)方差矩陣。但是，所給定的模型噪聲先驗協(xié)方差矩陣是特定時間窗口內(nèi)模型誤差的平滑值。當(dāng)動力學(xué)模型存在較大的異常擾動時，用開窗平滑法求出的狀態(tài)預(yù)測信息的協(xié)方差矩陣不能反映模型誤差的實(shí)際大小[1-4]。文獻(xiàn)[1]等提出了一種改進(jìn)的Sage自適應(yīng)濾波算法，即利用Sage濾波的開窗平滑法求得狀態(tài)預(yù)測信息的協(xié)方差矩陣，然后用自適應(yīng)因子進(jìn)行調(diào)節(jié)。但文獻(xiàn)[1]中在求觀測殘差向量和新息向量(預(yù)測殘差向量)的協(xié)方差陣時，對觀測殘差向量和新息向量均采用算術(shù)平均值法，沒有區(qū)分各個歷元觀測殘差向量值和新息向量值的大小，因而估計精度不高。隨機(jī)加權(quán)法是一種非常實(shí)用的統(tǒng)計計算方法，該方法具有計算量小、估計精度高、具有良好的大樣本性質(zhì)，不需要知道統(tǒng)計量的概率分布等優(yōu)點(diǎn)，被廣泛應(yīng)用于電子信息，自動控制和航空航天等領(lǐng)域?qū)Ш絽?shù)的估計。與文獻(xiàn)[1]中所采用的算術(shù)平均值法比較，隨機(jī)加權(quán)法能根據(jù)觀測信息和新息向量的有用程度，選取不同權(quán)值，增強(qiáng)對有用信息的利用，從而提高濾波解算精度。

本文在吸收Sage-Husa濾波和UKF優(yōu)點(diǎn)的基礎(chǔ)上，提出一種非線性Sage-Husa隨機(jī)加權(quán)無跡卡爾曼濾波(Sage-Husa random weighting unscented Kalman filter， SRWUKF)算法，該方法利用Sage濾波的開創(chuàng)平滑方法求得觀測殘差向量和新息(預(yù)測殘差)向量的協(xié)方差陣。然后用隨機(jī)加權(quán)自適應(yīng)因子對觀測殘差和預(yù)測殘差進(jìn)行調(diào)節(jié)，對狀態(tài)預(yù)報向量的協(xié)方差矩陣進(jìn)行自適應(yīng)隨機(jī)加權(quán)估計，以控制觀測殘差和預(yù)測殘差對導(dǎo)航精度的影響。最后對UKF、自適應(yīng)無跡卡爾曼濾波(adaptive unscented Kalman filter， AUKF)和本文所提出的SRWUKF三種算法進(jìn)行了計算和比較分析。

2 隨機(jī)加權(quán)法的基本思想

設(shè)X1,X2,…,Xn是來自分布函數(shù)F(x)的獨(dú)立同分布樣本，相應(yīng)地經(jīng)驗分布函數(shù)

(1)

f(v1,v2,…,vn-1)=Γ(n)，

(v1,v2,…,vn-1)∈sn-1

3 非線性Sage-Husa隨機(jī)加權(quán)極大驗后估值器

考慮如下離散非線性系統(tǒng)

(2)

式(2)中，Xk∈Rn為k時刻系統(tǒng)的狀態(tài)向量，f(·)是狀態(tài)向量的非線性函數(shù)，Wk∈Rn為系統(tǒng)過程噪聲，Zk∈Rm為k時刻系統(tǒng)的量測輸出，Vk∈Rn為觀測噪聲，Bk為系統(tǒng)觀測矩陣。

假設(shè)系統(tǒng)過程噪聲Wk和觀測噪聲Vk為相互獨(dú)立，均值和協(xié)方差陣為時變的正態(tài)白噪聲序列，統(tǒng)計特性滿足

(3)

式(3)中，q和r分別為Wk和Vk的均值，Q和R分別為Wk和Vk方差陣，q、r、Q和R均是未知的，δk,j為示性函數(shù)。

則Sage-Husa的噪聲統(tǒng)計估值器為

(4)

式(4)中，dk-1=(1-b)/(1-bk)，0

對于式(2)所描述的非線性系統(tǒng)，根據(jù)極大驗后噪聲統(tǒng)計估值的定義，可得q，r，Q和R的算術(shù)平均值估計為

(5)

相應(yīng)地，q，r，Q和R的隨機(jī)加權(quán)估計為

(6)

(7)

式(7)所表示的非線性Sage-Husa隨機(jī)加權(quán)極大驗后估計器是次優(yōu)的。

4 Sage-Husa隨機(jī)加權(quán)UKF算法

工程實(shí)際中，動力學(xué)系統(tǒng)的過程噪聲和觀測噪聲的方差陣Q和R對UKF的濾波精度有嚴(yán)重影響，這是由于Q和R決定了當(dāng)前信息和前一時刻觀測信息之間的權(quán)重。而當(dāng)前信息和前一時刻的觀測信息又將直接影響UKF的遞推精度。由此可見，動力學(xué)系統(tǒng)的過程噪聲和觀測噪聲中的任何偏差，都將導(dǎo)致濾波變?yōu)榇蝺?yōu)，甚至可能出現(xiàn)發(fā)散。為此，本文提出一種非線性Sage-Husa隨機(jī)加權(quán)UKF算法，該算法的步驟為

(1)初始化

初始化后的濾波值為

(8)

和

(9)

(2)計算Sigma點(diǎn)

(10)

非線性變換產(chǎn)生的樣本點(diǎn)為

(11)

(3)時間更新

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

(12)

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

(13)

λ=α2(n+κ)-n

(14)

式中，α是很小的正數(shù)，一般取10-1≤α≤1；κ=3-n；β取值與x的分布有關(guān)，對于正態(tài)分布β=2最優(yōu)。

(15)

(4)量測更新

(16)

(17)

濾波增益為

(18)

估計均方誤差為

(19)

狀態(tài)估計值為

(20)

5 算例與分析

5.1 SINS/SAR組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型

無人機(jī)(型號U-SN05)采用捷聯(lián)慣導(dǎo)(strap-down inertial navigation system， SINS)和合成孔徑雷達(dá)(synthetic aperture radar， SAR)組合系統(tǒng)進(jìn)行導(dǎo)航，系統(tǒng)的狀態(tài)方程和觀測方程如下所示[8-9]。

系統(tǒng)的狀態(tài)方程為

(21)

式中，f(·)為非線性函數(shù)，其表達(dá)式由式(22)給出，X(t)為系統(tǒng)狀態(tài)向量，W(t)為系統(tǒng)噪聲。

(22)

噪聲系數(shù)矩陣為

(23)

系統(tǒng)噪聲向量W(t)為

W(t)=[WεxWεyWεzWεmxWεmyWεmz
W▽XW▽YW▽ZWΨxWΨyWΨzWhb]T

(24)

將氣壓高度計得到的高度信息加入到SINS/SAR組合導(dǎo)航系統(tǒng)中，得到組合導(dǎo)航系統(tǒng)的狀態(tài)量為

X(t)=[δvN,δvW,δvU,δφ,δλ,δh,φN,
φW,φU,εx,εy,εz,▽X,▽Y,▽Z,δhb]T

(25)

式中,(δvN,δvW,δvU)為速度誤差，(δφ,δλ,δh)表示位置誤差，(φN,φW,φU)為姿態(tài)誤差，(εx,εy,εz)為陀螺常值漂移，(▽X,▽Y,▽Z)為加計零偏，δhb高度計誤差。

SINS/SAR組合導(dǎo)航系統(tǒng)的量測誤差為

Z(t)=A(t)X(t)+e(t)

(26)

因為SINS和SAR都可以輸出經(jīng)、緯度和航向角信息，所以，組合導(dǎo)航系統(tǒng)的量測量選為

(27)

其中

(28)

(29)

式中，δpψ為SAR圖像匹配輸出的航向角誤差，δpw和δpn分別為東向和北向位置誤差，其大小依賴于所采用的圖像匹配算法的精度。RM,RN分別為參考橢球子午圈和卯酉圈上各點(diǎn)的曲率半徑。

5.2 計算與分析

為了測試試驗型無人機(jī)(型號U-SN05)的性能，于2012-09-21在內(nèi)蒙古某地進(jìn)行了測試飛行。無人機(jī)采用5.1節(jié)所描述的組合系統(tǒng)進(jìn)行導(dǎo)航，經(jīng)過約5 min初始化后起飛，分別進(jìn)行了轉(zhuǎn)彎、爬升、俯沖等機(jī)動飛行動作，持續(xù)飛行約56 min后完成整個飛行過程。在無人機(jī)起飛地點(diǎn)約500 m處設(shè)地面觀測站，觀測記錄無人機(jī)的實(shí)時導(dǎo)航信息，作為導(dǎo)航系統(tǒng)輸出的參考信息。

圖1 采用三種濾波算法得到的經(jīng)度誤差曲線

圖2 采用三種濾波算法得到的緯度誤差曲線

圖3 采用三種濾波算法得到的高度誤差曲線

計算結(jié)果如圖1～圖3所示。從計算結(jié)果可以看出，UKF、AUKF和SRWUKF三種算法在位置誤差解算精度上濾波性能有明顯差別。圖1中，由UKF濾波得到的經(jīng)度誤差范圍在(-23 m， 20 m)之間，誤差均值為5.425 0 m，由AUKF濾波得到的經(jīng)度誤差范圍在(-16 m， 15 m)之間，誤差均值為4.089 6 m，而SRWUKF濾波得到的經(jīng)度誤差范圍在(-11 m， 8 m)之間，誤差均值為2.614 6 m。圖2中，由UKF濾波得到的緯度誤差范圍在(-22 m， 16 m)之間，誤差均值為4.937 1 m，由AUKF濾波得到的緯度誤差范圍在(-25 m， 25 m)之間，誤差均值為4.066 8 m，而SRWUKF濾波得到的緯度誤差范圍在(-9 m， 11 m)之間，誤差均值為2.629 3 m。圖3中，由UKF濾波得到的高度誤差范圍在(-32 m， 25 m)之間，誤差均值為10.458 9 m，由AUKF濾波得到的高度誤差范圍在(-30 m， 25 m)之間，誤差均值為8.766 1 m，而SRWUKF濾波得到的高度誤差范圍在(-25 m， 25 m)之間，誤差均值為5.091 2 m。從計算結(jié)果可以看出，AUKF算法得到的位置誤差精度高于UKF算法解得的誤差精度，提出的SRWUKF算法的精度明顯高于UKF和AUKF這兩種算法。

6 結(jié)束語

本文在吸收Sage-Husa濾波和無跡卡爾曼濾波優(yōu)點(diǎn)的基礎(chǔ)上，利用隨機(jī)加權(quán)估計算法將傳統(tǒng)的定義在線性系統(tǒng)上的Sage-Husa噪聲估計方法推廣到非線性系統(tǒng)中，提出一種非線性Sage-Husa隨機(jī)加權(quán)無跡卡爾曼濾波算法。該方法利用Sage濾波的開窗平滑方法求得觀測殘差向量和新息向量的協(xié)方差陣，并用隨機(jī)加權(quán)自適應(yīng)因子對觀測殘差和預(yù)測殘差進(jìn)行調(diào)節(jié)，以控制預(yù)測殘差和預(yù)測殘差對導(dǎo)航精度的影響。并將提出的非線性Sage-Husa隨機(jī)加權(quán)UKF算法應(yīng)用到SINS/SAR組合導(dǎo)航系統(tǒng)中進(jìn)行計算，結(jié)果表明提出算法的濾波精度明顯優(yōu)于UKF和自適應(yīng)UKF算法，提高了導(dǎo)航系統(tǒng)的精度。

[1] YANG Yuan-xi， XU Tian-he.An Adaptive Kalman Filter Based on Sage Windowing Weights and Variance Components[J].The Journal of Navigation， 2003， 56(2)：231-240.

[2] GAO Wei-guang， YANG Yuan-xi， CUI Xian-qiang，et al.Application of Adaptive Kalman Filtering Algorithm in IMU/GPS Integrated Navigation System[J].Geo-spatial Information Science， 2007， 10(1)：22-26.

[3] YANG Yuan-xi， GAO Wei-guang.An Optimal Adaptive Kalman Filter[J].Journal of Geodesy， 2006， 80(4)：177-183.

[4] GAO She-sheng， ZHONG Yong-min， SHIRINZDEH Bijan.Random Weighting Estimation for Fusion of Multi-dimensional Position Data[J].Information Sciences， 2010， 180(4)：4999-5007.

[5] GAO She-sheng， GAO Yi， ZHONG Yong-min.Random Weighting Estimation Method for Dynamic Navigation Positioning[J].Chinese Journal of Aeronautic， 2011， 24(3)：318-323.

[6] GAO She-sheng， ZHONG Yong-min.Random Weighting Estimation of Kernel Density[J].Journal of Statistical Planning and Inference， 2010， 140(9)：2403-2407.

[7] GAO She-sheng， FENG Zhi-hua， ZHONG Yong-min， et al.Random Weighting Estimation of Parameters in Generalized Gaussian Distribution[J].Information Science， 2008， 178(9)：2275-2281.

[8] ZHONG Yong-min， GAO She-sheng， LI Wei.A Quaternion-based Method for SINS/SAR Integrated Navigation System[J].IEEE Transactions on Aerospace and Electronic Systems， 2012， 48(1)：514-524.

[9] GAO She-sheng， ZHONG Yong-min， ZHANG Xue-yuan，et al.Multi-sensor Optimal Data Fusion for INS/GPS/SAR Integrated Navigation System[J].Aerospace Science and Technology， 2009， 13(4-5)：232-237.