朱文超

?

一種限定記憶的自適應擴展Kalman濾波器

朱文超

(中國電子科技集團第三十八研究所，安徽，合肥 230041)

為解決擴展卡爾曼濾波器（EKF）魯棒性差，且無法實時精確跟蹤系統(tǒng)突變狀態(tài)的問題，研究一種基于限定記憶濾波的自適應EKF算法。算法將EKF與限定記憶濾波器相融合，減小舊量測數(shù)據(jù)對濾波效果的影響，提高估計精度；引入自適應因子與漸消因子，通過實時調(diào)節(jié)新舊濾波增益陣以及預測狀態(tài)值，精確地跟蹤系統(tǒng)突變狀態(tài)。仿真實例表明，強跟蹤算法與經(jīng)典EKF算法相比，自適應EKF算法魯棒性好，濾波精度高，能夠有效地跟蹤系統(tǒng)突變狀態(tài)。

擴展卡爾曼濾波；限定記憶濾波；舊量測數(shù)據(jù)；自適應算法；系統(tǒng)突變狀態(tài)

Kalman濾波器是一種基于最小均方誤差原則的貝葉斯估計算法，可以實時有效地處理線性高斯濾波問題，廣泛地應用于傳感器測量噪聲消除、航空航海系統(tǒng)狀態(tài)跟蹤等領域[1]。其核心思想是當系統(tǒng)觀測值更新時，利用濾波增益調(diào)整先驗估計與測量殘差之間的權重比，獲取當前信號的最優(yōu)估計值[2]。Kalman濾波器以線性狀態(tài)空間模型為基石，但實際應用過程中，線性高斯假設條件較難滿足，若將Kalman濾波器用于處理非線性系統(tǒng)問題，其跟蹤性能會嚴重下降。為此，國內(nèi)外研究人員引入擴展Kalman濾波（EKF）技術[3-5]，處理非線性系統(tǒng)估計問題。然而，EKF采用的是高階狀態(tài)方程線性化的策略，其魯棒性較差，當狀態(tài)模型不精確或系統(tǒng)狀態(tài)發(fā)生突變時，容易出現(xiàn)發(fā)散現(xiàn)象。

為了提高EKF的魯棒性，學者們從不同的角度出發(fā)，設計了大量改進EKF算法。其中用途較為廣泛的是強跟蹤EKF濾波器。文獻[6-7]借鑒強跟蹤Kalman濾波思想，通過引入單重時變漸消因子，調(diào)整增益矩陣，實現(xiàn)對突變系統(tǒng)狀態(tài)的實時跟蹤。文獻[8-9]設計了改進的強跟蹤濾波器，通過多重次優(yōu)漸消因子矩陣實現(xiàn)對老數(shù)據(jù)的漸消，增強算法跟蹤突變狀態(tài)的能力。文獻[10]重新定義了強跟蹤濾波器中測量殘差序列的取值規(guī)則，利用不同的加權系數(shù)突出新進信息的作用。文獻[11]將強跟蹤濾波器與經(jīng)典Kalman濾波器相結合，給出濾波器發(fā)散或收斂的判據(jù)，保證估計精度與濾波速率。上述文獻中呈現(xiàn)的強跟蹤濾波器從不同程度上提高了EKF的跟蹤性能，但它們均忽視了舊測量數(shù)據(jù)對狀態(tài)估計的影響，僅僅用漸消因子調(diào)整預測方差陣，這顯然是不夠的。系統(tǒng)狀態(tài)突變后，實時更新的濾波增益雖然能夠調(diào)整新測量數(shù)據(jù)的權重，但舊測量數(shù)據(jù)仍隱含在先驗估計中。先驗估計的不精確將導致系統(tǒng)趨于穩(wěn)態(tài)時間過長，估計精度降低，故消除突變前舊測量數(shù)據(jù)的影響顯得尤為重要。

針對以上問題，本文將限定記憶思想融入強跟蹤濾波器中，研究一種具有限定記憶功能的自適應EKF濾波算法。一方面利用限定記憶濾波器減小舊測量數(shù)據(jù)對系統(tǒng)狀態(tài)估計的影響；另一方面通過激活自適應因子與漸消因子，采用調(diào)整先驗狀態(tài)在最優(yōu)估計中權重的策略，實時有效地跟蹤系統(tǒng)突變狀態(tài)。

1 限定記憶EKF算法

限定記憶濾波只利用離當前時刻最近的前-1個觀測值計算最優(yōu)估計值，完全截斷第-1個觀測值之前的舊數(shù)據(jù)對濾波值的影響。根據(jù)先驗知識確定記憶長度，保證測量數(shù)據(jù)的實時性。將限定記憶濾波和EKF算法相融合，消除舊觀測數(shù)據(jù)產(chǎn)生的累積誤差，進而推導出限定記憶擴展Kalman濾波公式：

系統(tǒng)狀態(tài)一步預測：

一步預測誤差方差陣：

濾波增益矩陣：

時刻綜合誤差：

殘差序列：

系統(tǒng)最優(yōu)估計：

后驗誤差方差陣：

2 限定記憶自適應EKF濾波算法

2.1 強跟蹤濾波算法

傳統(tǒng)的EKF是一種開環(huán)濾波器，當系統(tǒng)到達穩(wěn)態(tài)時，增益K趨向于穩(wěn)定。若此時系統(tǒng)發(fā)生突變，限定記憶EKF將失去對系統(tǒng)的跟蹤能力。為了避免這種現(xiàn)象出現(xiàn)，試利用強跟蹤算法（STF）[12]，引入漸消因子，實時調(diào)整一步預測誤差方差陣和濾波增益。修改式（2）為：

根據(jù)STF在線實時調(diào)整增益矩陣的正交性原理[13]，構建殘差序列正交方程：

3.2 限定記憶自適應EKF算法

根據(jù)正交方程（10）、（11），限定記憶EKF算法并存兩個濾波增益矩陣和漸消因子，當誤差協(xié)方差矩陣發(fā)生變化時，濾波增益陣，變化趨勢相同。但不精確的反轉(zhuǎn)移矩陣可能導致舊量測值不能完全轉(zhuǎn)化為新息量測值，最終影響濾波精度。

準確跟蹤系統(tǒng)突變狀態(tài)，關鍵在于實時地調(diào)整新舊殘差序列的權重，。為了避免出現(xiàn)濾波發(fā)散現(xiàn)象，提高濾波精度，應突出新息序列增益陣，以為主漸消因子，并用多重自適應調(diào)節(jié)因子調(diào)節(jié)舊殘差序列增益陣：

其中：待定因子

Step 4：由（14）式和（16）~（19）式求出自適應調(diào)節(jié)因子，由（3）式分別求出新舊濾波增益陣，。

Step 5：結合上述結果，利用（6）式求出系統(tǒng)最優(yōu)估計，轉(zhuǎn)向第二步，繼續(xù)循環(huán)。

3 限定記憶自適應EKF濾波實例

3.1 連續(xù)階躍信號

本節(jié)以普通二階系統(tǒng)為研究對象，分析AEKF的濾波效果，設系統(tǒng)狀態(tài)方程為：

將（21）式代入（20）式，將控制項歸為噪聲項，得到非線性函數(shù)一階線性化后的系統(tǒng)狀態(tài)方程：

上述方程顯示的狀態(tài)時域曲線可以近似為階躍信號曲線，如圖1所示。當系統(tǒng)控制項發(fā)生突變時（方程中表現(xiàn)為噪聲項突變），系統(tǒng)狀態(tài)將產(chǎn)生跳變，則突變狀態(tài)曲線將化為具有個單位躍變量的連續(xù)階躍信號，具體函數(shù)形式如圖2所示。本節(jié)實例中，假設信號分別在初始時刻和第47時刻發(fā)生躍變，第一躍變量2與第二躍變量8象征著兩個不同的系統(tǒng)真實狀態(tài)，繼而將噪聲信號加載到連續(xù)階躍信號中（圖3）。

3.2 連續(xù)階躍信號AEKF濾波實例

同上節(jié)假設的情況一樣，假設連續(xù)階躍信號從第47時刻發(fā)生突變，從追蹤突變狀態(tài)的角度來驗證強跟蹤算法（Strong Tracking Filter，STF）、經(jīng)典Kalman濾波算法（EKF）以及限定記憶自適應EKF算法（AEKF）的性能，根據(jù)先驗信息，設置限定記憶長度為15位，遺忘因子=0.95，三種算法的跟蹤效果及性能評價如圖4~圖6以及表1所示。

在系統(tǒng)狀態(tài)未突變前，STF算法的濾波效果與EKF算法相同，由于記憶長度的限制，當濾波過程迭代到第16時刻時，AEKF算法的精度高于其余兩種算法。系統(tǒng)狀態(tài)在第47時刻發(fā)生突變，EKF算法始終采用舊濾波增益進行濾噪，故其逐漸偏離真實狀態(tài)，最終導致濾波發(fā)散，后驗誤差無限增大（見圖7）。由于漸消因子的持續(xù)激活，AEKF和STF可以成功地跟蹤系統(tǒng)突變狀態(tài)，圖6顯示了突變后兩者的跟蹤效果。可以看出，在濾波后期，由于大量新息測量值的涌入，導致AEKF算法的精度逐漸高于STF，并且能更快地趨于系統(tǒng)穩(wěn)態(tài)。

三種算法相關系數(shù)的變化趨勢如圖7~圖10所示。EKF算法的濾波增益從第9時刻開始就不曾改變，STF算法的濾波增益和漸消因子均在47時刻發(fā)生突變，并在濾波后期逐漸趨于穩(wěn)定。漸消因子和自適應因子實時在線調(diào)節(jié)先驗誤差和舊濾波增益，使AEKF能在系統(tǒng)突變以及獲取足夠的測量值后具有較高的跟蹤精度。從表1跟蹤精度數(shù)據(jù)中，不難得知，與STF與EKF相比，AEKF在狀態(tài)突變前后均具有較好的跟蹤效果，尤其在突變后期，AEKF的精度較STF提升了20.32%。

圖1 單位階躍信號

圖2 連續(xù)階躍信號

圖3 加噪連續(xù)階躍信號

圖4 未突變前三種算法跟蹤效果

圖5 三種算法全區(qū)域跟蹤效果

圖6 突變后算法跟蹤效果

圖7 三種算法后驗誤差評價

圖8 三種算法相關濾波增益

圖9 AEKF漸消因子

圖10 AEKF自適應因子

表1 三種算法性能評價表

5 結論

一階近似擴展卡爾曼濾波通過對非線性狀態(tài)的泰勒展開式進行一階線性化截斷，將非線性濾波問題轉(zhuǎn)化為線性Kalman濾波，但其魯棒性較差，當系統(tǒng)模型不準確時，容易出現(xiàn)發(fā)散現(xiàn)象。針對這一問題，設計了一種限定記憶自適應EKF濾波器，借鑒限定記憶思想減小舊測量值的影響，通過自適應調(diào)節(jié)因子與漸消因子激活突變狀態(tài)下的先驗誤差，從預測狀態(tài)，濾波增益及殘差序列三方面出發(fā)，實時在線調(diào)整先驗估計的權重，獲取系統(tǒng)最優(yōu)估計。仿真實例表明：自適應EKF濾波算法可以有效地跟蹤系統(tǒng)突變狀態(tài)，當量測次數(shù)大于記憶長度時，其濾波精度高于強跟蹤濾波，在系統(tǒng)突變前后，可以更快地趨向于穩(wěn)態(tài)。

[1] Rigatos G G. Extended Kalman and Particle Filtering for sensor fusion in motion control of mobile robots[J]. Mathematics and computers in simulation, 2010, 81(3): 590-607.

[2] Bijker J, Steyn W. Kalman filter configurations for a low-cost loosely integrated inertial navigation system on an airship[J]. Control Engineering Practice, 2008, 16(12): 1509-1518.

[3] Rigatos G G. Nonlinear Kalman Filters and Particle Filters for integrated navigation of unmanned aerial vehicles[J]. Robotics and Autonomous Systems, 2012, 60(7): 978-995.

[4] 陸可. 基于強跟蹤濾波算法的異步電機參數(shù)自適應無速度傳感器控制[J]. 電機與控制應用, 2011,38(5): 16-21.

[5] Rigatos G G. Extended Kalman and Particle Filtering for sensor fusion in motion control of mobile robots[J]. Mathematics and computers in simulation, 2010, 81(3): 590-607.

[6] Wenzel T A, Burnham K J, Blundell M V. Dual extended Kalman filter for vehicle state and parameter estimation[J]. Vehicle System Dynamics,2006, 44(2): 153-171.

[7] 宗長富,胡丹. 基于擴展Kalman濾波的汽車行駛狀態(tài)估計[J]. 吉林大學學報:工學版,2009,39(1):7-9.

[8] 童春霞，張?zhí)鞓? 強跟蹤濾波理論在非線性飛控中的應用[J].計算機仿真,2004,21(8):17-19.

[9] 周聰,肖建. 改進強跟蹤濾波算法及其在汽車狀態(tài)估計中的應用[J].自動化學報,2012,38(9):1521-1527.

[10] 趙學梅,陳懇,李冬. 強跟蹤卡爾曼濾波在視頻目標跟蹤中的應用[J].計算機工程與應用,2011，47(11): 128-132.

[11] 劉華普,孔金生,董文麗. 改進的強跟蹤器自適應濾波信息融合方法[J].傳感器與儀器儀表,2007,23(7): 141-143.

[12] 張劍鋒，曾國強. 擴展卡爾曼粒子濾波在編隊衛(wèi)星相對軌道確定中的應用[J]. 航天控制,2010,28(4):40-43.

[13] 文成林,陳志國,周東華. 基于強跟蹤濾波器的多傳感器非線性動態(tài)系統(tǒng)狀態(tài)與參數(shù)聯(lián)合估計[J].電子學報，2002,30(11):1715-1717.

[14] 付夢印,鄧志紅,張繼偉.Kalman 濾波理論及其在導航系統(tǒng)中的應用[M].北京:科學出版社,2003.

ADAPTIVE EXTENDED KALMAN FILTERING BASED ON LIMIT MEMORY ALGORITHM

ZHU Wen-chao

(The 38thresearch institute of China electronics technology group corporation, Hefei, Anhui 230041, China)

In order to solve the problem that Extended Kalman filtering (EKF) cannot tracking mutant system state accuracy and improve the robustness of the filtering. A new adaptive Kalman filtering (AEKF) based on limited memory has been proposed. This algorithm combines EKF with Limit memory filtering that minimizes the influence of the old measurements. Forgetting factor and weakening factor can track mutant state accuracy by the technology of dynamically adjusting the weight of state prediction and Kalman gains in the filter estimation. The results of simulation experiments demonstrate that in comparison with Strong Tracking Filtering and Extended Kalman Filtering, AEKF provides higher estimated accuracy and better .robustness to track mutant state.

extended Kalman filtering; limit memory filtering; old measurements; adaptive filtering; mutant system state

1674-8085(2015)04-0049-06

TP391

A

10.3969/j.issn.1674-8085.2015.04.009

2015-03-01；修改日期：2015-04-06

朱文超(1989-)，男，安徽合肥人，助理工程師，碩士，主要從事雷達伺服控制、雷達信號處理研究(E-mail: zhuwenchao5102951@126.com).