Sage-Husa隨機加權無跡卡爾曼濾波及其在導航中的應用

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

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

1 引言

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

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

2 隨機加權法的基本思想

設X1,X2,…,Xn是來自分布函數F(x)的獨立同分布樣本，相應地經驗分布函數

(1)

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

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

3 非線性Sage-Husa隨機加權極大驗后估值器

考慮如下離散非線性系統

(2)

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

假設系統過程噪聲Wk和觀測噪聲Vk為相互獨立，均值和協方差陣為時變的正態白噪聲序列，統計特性滿足

(3)

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

則Sage-Husa的噪聲統計估值器為

(4)

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

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

(5)

相應地，q，r，Q和R的隨機加權估計為

(6)

(7)

式(7)所表示的非線性Sage-Husa隨機加權極大驗后估計器是次優的。

4 Sage-Husa隨機加權UKF算法

工程實際中，動力學系統的過程噪聲和觀測噪聲的方差陣Q和R對UKF的濾波精度有嚴重影響，這是由于Q和R決定了當前信息和前一時刻觀測信息之間的權重。而當前信息和前一時刻的觀測信息又將直接影響UKF的遞推精度。由此可見，動力學系統的過程噪聲和觀測噪聲中的任何偏差，都將導致濾波變為次優，甚至可能出現發散。為此，本文提出一種非線性Sage-Husa隨機加權UKF算法，該算法的步驟為

(1)初始化

初始化后的濾波值為

(8)

和

(9)

(2)計算Sigma點

(10)

非線性變換產生的樣本點為

(11)

(3)時間更新

狀態的一步預測為

(12)

一步預測均方誤差為

(13)

λ=α2(n+κ)-n

(14)

式中，α是很小的正數，一般取10-1≤α≤1；κ=3-n；β取值與x的分布有關，對于正態分布β=2最優。

(15)

(4)量測更新

(16)

(17)

濾波增益為

(18)

估計均方誤差為

(19)

狀態估計值為

(20)

5 算例與分析

5.1 SINS/SAR組合導航系統數學模型

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

系統的狀態方程為

(21)

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

(22)

噪聲系數矩陣為

(23)

系統噪聲向量W(t)為

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

(24)

將氣壓高度計得到的高度信息加入到SINS/SAR組合導航系統中，得到組合導航系統的狀態量為

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)為姿態誤差，(εx,εy,εz)為陀螺常值漂移，(▽X,▽Y,▽Z)為加計零偏，δhb高度計誤差。

SINS/SAR組合導航系統的量測誤差為

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

(26)

因為SINS和SAR都可以輸出經、緯度和航向角信息，所以，組合導航系統的量測量選為

(27)

其中

(28)

(29)

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

5.2 計算與分析

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

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

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

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

計算結果如圖1～圖3所示。從計算結果可以看出，UKF、AUKF和SRWUKF三種算法在位置誤差解算精度上濾波性能有明顯差別。圖1中，由UKF濾波得到的經度誤差范圍在(-23 m， 20 m)之間，誤差均值為5.425 0 m，由AUKF濾波得到的經度誤差范圍在(-16 m， 15 m)之間，誤差均值為4.089 6 m，而SRWUKF濾波得到的經度誤差范圍在(-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。從計算結果可以看出，AUKF算法得到的位置誤差精度高于UKF算法解得的誤差精度，提出的SRWUKF算法的精度明顯高于UKF和AUKF這兩種算法。

6 結束語

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

[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.