薛文婷,張 波,李署堅

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

0 引 言

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

1 IAE爾曼濾波算法

1.1 卡爾曼濾波新息的概念

離散卡爾曼系統(tǒng)有以下兩個方程

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

(1)

Zk=HkXk+Vk，

(2)

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

cov[WkWj]=Qxδkj，

(3)

cov[VkVj]=Rkδkj.

(4)

假定系統(tǒng)為一致完全能觀和一致完全能控,則濾波是穩(wěn)定的,激蕩濾波遞推充分多步以后,濾波誤差協(xié)方差陣Pk,k將趨于常數(shù)矩陣P+,增益矩陣Kk將趨于常數(shù)矩陣K,預測誤差協(xié)方差陣Pk,k-1將趨于常數(shù)矩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卡爾曼濾波算法

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

(9)

狀態(tài)估計

(10)

濾波增益矩陣

(11)

一步預測誤差方差陣

(12)

誤差估計方差陣

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

(13)

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

(14)

(15)

(16)

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

(17)

(18)

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

(19)

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

2 GPS/DR組合導航模型

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

系統(tǒng)的狀態(tài)量選為

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

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

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.為觀測系統(tǒng)對外部量測干擾的效果,對GPS觀測數(shù)據(jù)進行加噪處理,做如下設定:測量誤差選擇三種不同強度的誤差,1～300 s取1倍的觀測誤差,301～600 s取4倍的觀測誤差,601～900 s取9倍的觀測誤差,之后量測誤差恢復為1倍。常規(guī)的卡爾曼濾波算法模型始終采取預先設定的量測誤差。改進的自適應卡爾曼濾波采取有限記憶的滑動新息方差,數(shù)據(jù)窗口大小N=20,以改進卡爾曼增益K的計算。濾波效果如圖2～圖4和表1所示。

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

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

圖4 聯(lián)邦卡爾曼濾波與改進的IAE濾波

表1新息自適應卡爾曼濾波與聯(lián)邦卡爾曼濾波算法位置誤差統(tǒng)計特性

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

4 結束語

在系統(tǒng)量測噪聲較大時,改進的新息卡爾曼濾波算法仍能保持較好的濾波效果,而傳統(tǒng)卡爾曼濾波算法則隨量測噪聲的變化出現(xiàn)了較大的發(fā)散,證明改進的新息自適應卡爾曼濾波算法能根據(jù)外界噪聲變化自適應的調整濾波,在系統(tǒng)噪聲增強時候可以保持優(yōu)于聯(lián)邦卡爾曼濾波的定位精度及穩(wěn)定性。

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

[2]卞鴻巍，李 安，覃方君，等.現(xiàn)代信息融合技術在組合導航中的應用[M].北京:國防工業(yè)出版社.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姿態(tài)量測系統(tǒng)的綜合導航系統(tǒng)研究[M].彈箭與制導學報,2003,23(5):8-11.