組合導航中一種新息自適應卡爾曼濾波算法

薛文婷,張 波,李署堅

(北京航空航天大學電子信息工程學院,北京 100191)

0 引 言

卡爾曼濾波是在組合導航中常用的信息融合算法,使用標準卡爾曼濾波的前提條件是系統的動力學特性和噪聲的統計特性已知,但在實際情況中由于全球定位系統接收機與航位推算(DR)器件及外部環境變化導致的不確定噪聲使得這個前提不能滿足,常導致濾波發散[1]。要控制發散首先需要判斷系統所處的不穩定狀況,然后選擇不同的發散控制方式,新息可以用來作為判斷和調整濾波器增益的依據[2]。目前,國內外對于自適應卡爾曼濾波器的研究逐漸集中在兩個方面:基于新息自適應估計(IAE)[3]的卡爾曼濾波器和多模型卡爾曼濾波器(MMAE)[4]。本文提出了一種基于IAE的卡爾曼濾波算法,可以通過實際新息的量測計算直接實現修正卡爾曼濾波增益,提高了在GPS測量發生較大變化時卡爾曼濾波器的精度和魯棒性[5]。通過仿真實驗對比分析了這種方法與傳統卡爾曼濾波方法的定位誤差,研究結果表明基于新息的卡爾曼濾波具有優于傳統卡爾曼濾波的定位結果且不易發散。

1 IAE爾曼濾波算法

1.1 卡爾曼濾波新息的概念

離散卡爾曼系統有以下兩個方程

Xk=φk,k-1Xk-1+Γk,k-1Wk-1，

(1)

Zk=HkXk+Vk，

(2)

式中:Xk為n維狀態向量;φk,k-1為n×n階狀態轉移矩陣;Γk,k-1為n×p階干擾矩陣;Wk-1為p維系統噪聲向量;Zk為m維觀測向量;Hk為m×n觀測矩陣;Vk為m維觀測噪聲向量。假定Wk與Vk為相互獨立的零均值白噪聲序列,分別有協方差陣

cov[WkWj]=Qxδkj，

(3)

cov[VkVj]=Rkδkj.

(4)

假定系統為一致完全能觀和一致完全能控,則濾波是穩定的,激蕩濾波遞推充分多步以后,濾波誤差協方差陣Pk,k將趨于常數矩陣P+,增益矩陣Kk將趨于常數矩陣K,預測誤差協方差陣Pk,k-1將趨于常數矩P-,它們之間關系為

P-=φP+φT+ΓQΓT，

(5)

K=P-HT(HP-HT+R)-1，

(6)

P+=(I-KH)P-，

(7)

(8)

式中:rk為預報殘差,通常稱為新息,描述了濾波器的觀測量的估計值與實際觀測量之間的差值。

1.2 改進的IAE卡爾曼濾波算法

以新息方式列寫卡爾曼濾波算法,狀態的一步預測為

(9)

狀態估計

(10)

濾波增益矩陣

(11)

一步預測誤差方差陣

(12)

誤差估計方差陣

Pk=(I-KkHk)Pk,k-1.

(13)

濾波器的新息序列rk狀態和其新息方差Crk的表達式為

(14)

(15)

(16)

基于零均值白色濾波新息更新序列,濾波的統計新息矩陣的更新方程為

(17)

(18)

式中:Δxk=Kkrk,將式(15)帶入式(11),即得改進的IAE自適應卡爾曼濾波增益的計算公式為

(19)

圖1 IAE卡爾曼濾波算法流程框圖

2 GPS/DR組合導航模型

本算法將GPS/DR信息進行集中處理,運動模型為當前統計模型。即當前機動目標在以某一加速度機動時,它在下一瞬間的加速度取值是有限的,并且在當前加速度的領域內呈現某種統計分布。狀態量一般選取位置信息或位置差信息、速度或速度差信息、加速度或加速度差信息,便于直觀觀測運動狀態的變化。DR系統一般選取陀螺儀和里程計作為航位推算部分,考慮在狀態量中加入DR設備量,且知道陀螺和里程輸出均為單位時間內變化量,所以考慮加入陀螺和里程 誤差量作為狀態量的一部分。

系統的狀態量選為

將GPS接收機輸出的定位信息er、en以及陀螺儀的輸出θ和里程計的輸出s作為外部觀測量。

當前時刻狀態量與前一時刻狀態量及當前時刻觀測量之間的關系表示為

X(k)=Φ(k,k-1)X(k-1)+μk,

(20)

Z(k)=H(k)X(k)+εk.

(21)

H[X(k,k-1)]=

3 仿真結果

對改進的新息自適應卡爾曼濾波算法進行仿真驗證,設定仿真條件為:接收機三軸定位誤差均為20 m, 接收機量測誤差方差為10 m, 接收機采樣周期為1 s,陀螺零漂為0.05(°)/h,里程計零漂0.05,東向和北向機動加速度誤差方差1.5 m/s2,誤差相關時間0.1,東向和北向位置誤差相關時間設為0.01,微機械陀螺誤差和里程計誤差相關時間均為1 000 s,仿真時間設定為2 000 s,陀螺誤差均方差0.1°/h,里程計誤差均方差2 m.車輛沿著東北方向變加速行駛,初始速度為10 m/s,仿真時間1 600 s,采樣周期1 s.為觀測系統對外部量測干擾的效果,對GPS觀測數據進行加噪處理,做如下設定:測量誤差選擇三種不同強度的誤差,1～300 s取1倍的觀測誤差,301～600 s取4倍的觀測誤差,601～900 s取9倍的觀測誤差,之后量測誤差恢復為1倍。常規的卡爾曼濾波算法模型始終采取預先設定的量測誤差。改進的自適應卡爾曼濾波采取有限記憶的滑動新息方差,數據窗口大小N=20,以改進卡爾曼增益K的計算。濾波效果如圖2～圖4和表1所示。

圖2 一般卡爾曼濾波位置誤差與觀測誤差

圖3 改進IAE卡爾曼濾波位置誤差與觀測誤差

圖4 聯邦卡爾曼濾波與改進的IAE濾波

表1新息自適應卡爾曼濾波與聯邦卡爾曼濾波算法位置誤差統計特性

與參考軌跡的誤差范圍/m新息自適應卡爾曼濾波聯邦卡爾曼濾波 小于1050.12%17.26% 小于2081.55%32.96% 小于3094.06%56.54% 小于40100%62.32% 小于50100%76.89%

4 結束語

在系統量測噪聲較大時,改進的新息卡爾曼濾波算法仍能保持較好的濾波效果,而傳統卡爾曼濾波算法則隨量測噪聲的變化出現了較大的發散,證明改進的新息自適應卡爾曼濾波算法能根據外界噪聲變化自適應的調整濾波,在系統噪聲增強時候可以保持優于聯邦卡爾曼濾波的定位精度及穩定性。

[1]李 旦.組合導航自適應卡爾曼濾波改進算法研究[J].測控技術,2011,3:114.

[2]卞鴻巍，李 安，覃方君，等.現代信息融合技術在組合導航中的應用[M].北京:國防工業出版社.2010.

[3]MOHAMED A H, SCHWARZ K P. Adaptive Kalman filtering for INS/GPS[J]. Journal of Geodesy, 1999, 73(4):193-203.

[4]LOEBIS D, SUTTON R, CHUDLEY J,etal. Adaptive tuning of a Kalman filter via fuzzy logic for an intelligent AUV navigation system[J]. Control Engineering Practice, 2004, 12(12):1531-1539.

[5]卞鴻巍,楊艷娟，金志華，等.基于GPS姿態量測系統的綜合導航系統研究[M].彈箭與制導學報,2003,23(5):8-11.