一種改進的擴展卡爾曼濾波

摘 要： 針對機動目標跟蹤中目標發生狀態突變和運動模型不匹配時擴展卡爾曼濾波精度降低或發散的問題，提出一種根據新息矩陣的范數判斷濾波是否應該修正，并通過修正一步預測值來提高濾波精度的算法。該算法使用新息矩陣和量測誤差矩陣來判斷濾波是否穩定，在濾波精度降低甚至發散的情況下通過修正一步預測值來提高濾波精度。該算法計算量小，實時性強。仿真結果表明，該算法能夠根據新息實時調整，且濾波精度較高。

關鍵詞： 擴展卡爾曼濾波； 濾波發散； 新息； 目標跟蹤

中圖分類號： TN911?34 文獻標識碼： A 文章編號： 1004?373X（2016）02?0009?03

An improved extended Kalman filter

LI Zhiguo1， LI Xuming2， WANG Yunfeng1

（1. College of Computer Science， Sichuan University， Chengdu 610065，China；

2. Nanjing Changjiang Electronics Information Industry Group Co.， Ltd.， Nanjing 210000， China）

Abstract： Concerning the problem of low filtering accuracy or filtering divergence of EKF when the system model is established inaccurately and target moving state changes， a new adaptive algorithm is presented， which improves the accuracy and performance by adjusting the predicted value. The algorithm can judge whether the filtering is stable by the innovation matrix and measurement error matrix， and can improve the filtering accuracy by correcting the single?step predicted value while the filtering accuracy lowers or even divergence. The proposed algorithm has little computation burden and high real?time performance. The simulation results show that the algorithm has high filtering accuracy and is capable of quick adjustment according to the innovation.

Keyword： extended Kalman filter； filtering divergence； innovation； target tracking

0 引 言

擴展卡爾曼濾波（Extended Kalman Filter，EKF）主要解決目標跟蹤中運動模型是非線性時的問題，通常將非線性系統進行近似線性化后采用標準卡爾曼濾波。當運動模型非線性較強或者運動目標發生狀態突變時可能會造成濾波器發散[1]。主要的解決方案有如下兩種：

（1） 通過調整濾波器增益。如基于新息協方差的自適應卡爾曼濾波[2]、平方根濾波[3]、無跡卡爾曼濾波[4]和基于漸消因子的自適應卡爾曼濾波[5]等；

（2） 通過在不同的跟蹤濾波器之間切換或增大目標狀態維數來調整濾波器的結構。

但這些算法存在計算復雜和延遲等缺點。文獻[6]提出了一種根據新息和量測誤差協方差來判斷機動目標運動狀態是否發生突變和通過調整預測值從而提高濾波精度的算法，此濾波算法取得了很好的效果。在實際工程應用中，雷達的量測精度不太穩定，量測誤差不易準確實時的獲取。本文在此基礎上提出一種根據新息矩陣范數判斷濾波是否應該修正，并通過調整預測值來提高濾波精度的算法。這種方法計算簡單，能使EKF適應于機動目標跟蹤時目標運動狀態發生突變和運動模型不匹配的情況。

1 目標跟蹤建模

狀態方程為：

[X（k+1）=f（k，X（k））+V（k）] （1）

量測方程為：

[Z（k+1）=h（k，X（k））+W（k）] （2）

式中：[X（k）]是目標的狀態向量；[V（k）]是方差為[E[V（k）V′（j）]=Q（k）δkj]的過程噪聲；[Z（k）]是k時刻的量測向量；[W（k）]是方差為[E（W（k）W′（j）]=R（k）δkj]的量測噪聲；[f（k）]為狀態轉移矩陣；[h（k）]為量測矩陣。目標運動模型為Singer模型：

[f=1T（αT-1+e-αT）α201（1-e-αT）α00e-αT]

[h=[1 0 0]]

狀態方程中函數的一階展開式為：

[X（k+1）=f（k，X（kk）+fx（k）[X（k）-X（kk）]+V（k）] （3）

擴展卡爾曼濾波過程如下：

狀態的一步預測方程為：

[X（k+1k）=f[k，X（kk）]] （4）

協方差的預測方程為：

[P（k+1k）=fx（k）P（kk）fx′（k）+Q（k）] （5）

量測預測方程為：

[Z（k+1k）=h[k+1，X（k+1k）]] （6）

新息協方差為：

[S（k+1）=hx（k+1）P（k+1k）hx′（k+1）+R（k+1）] （7）

增益為：

[K（k+1）=P（k+1k）hx′（k+1）S-1（k+1）] （8）

狀態更新方程為：

[X（k+1k+1）=X（k+1k）+K（k+1）{Z（k+1）- h[k+1，X（k+1k）]}] （9）

協方差更新方程為：

[P（k+1k+1）=[I-K（k+1）hx（k+1）]P（k+1k）· [I+K（k+1）hx（k+1）]′-K（k+1）R（k+1）K′（k+1）] （10）

只需要給定狀態初始值和濾波估計狀態向量的協方差矩陣，算法就可以啟動并遞推下去[6]。

2 改進算法

系統達到穩態后，預測協方差、新息和增益均趨于極小值[6]。若此時目標進行了較強的機動性改變，預測值不再準確，新息變大，但是增益不能隨之改變，造成濾波精度下降；建立的運動模型與目標實際的運動模型不匹配是由于預測值的不準確而導致濾波值不準確。由于增益矩陣K的計算量較大，重新計算增益矩陣K會導致實時性降低，所以本文通過一步預測值的改進來提高濾波值的精度。

在濾波趨于穩態后，新息矩陣趨于極小值，可根據新息矩陣的大小判斷是否應該調整預測值：

[r（k+1）<λzmax] （11）

式中：[zmax]為量測誤差矩陣的最大值；[·]為矩陣的F?范數；[λ]取值范圍為[[0.3，0.9]]。當新息矩陣滿足式（11）時，認為濾波正常；當式（11）不成立時，認為目標發生了較強的機動，此時量測值的可靠性較高，一步預測值的可靠性較低，應修正一步預測值。

如果式（11）不滿足，表明一步預測值[X（k+1k）]不準確，修改[X（k+1k）]，通過重新計算狀態更新方程得到新的[X（k+1k+1）]；具體修正方式如下：

[x（k+1k）x（k+1k）=x（k+1k）x（k+1k）+r′（k+1）* c1c2T]

式中：[c1]是對預測位置的修正；[c2]是對預測速度的修正，[r′（k+1）]是k+1時刻新息矩陣的距離分量；[c1]和[c2]的取值范圍是[（0，1）]，在工程應用中需要根據實際情況設定。

3 仿 真

通過設置下列兩種實驗場景來比較EKF和本文提出的改進算法：

（1） 機動目標進行勻速直線運動一段時間（20 s）后進行加速運動，加速運動持續時間為20 s，然后再次進行60 s的勻速運動。

（2） 機動目標在不同的時間段進行勻速、轉彎和勻速運動，運動持續時間分別為80 s，40 s，40 s。實驗結果通過對比目標位置的RMSE來比較兩種算法的精度。

3.1 仿真1：勻速運動和加速運動

目標起始位置為：

[X（k）=2 000，200，0，1 000，150，0T]

目標運動時間為100 s，采樣間隔為2 s；系統的噪聲協方差矩陣[Q=0.010.01×0.010.01×0.010.01]；量測噪聲的協方差矩陣[R=1002001002]。

從圖1可看出，濾波算法穩定后兩種算法的位置RMSE相近；但是目標發生較強的機動變化時，EKF濾波精度大幅降低而本文算法精度較高。由于目標發生較強的機動時，量測值和一步預測值差距突然增加，但是濾波不能隨之變化，造成擴展卡爾曼濾波精度較低，而改進后的濾波算法能實時地根據新息矩陣個范數調整估計值，從而提高了濾波算法的精度。

3.2 仿真2：勻速運動和轉彎運動

目標起始位置為：

[X（k）=2 000，200，0，1 000，150，0T]

目標運動時間為160 s，采樣間隔為2 s。系統噪聲協方差[Q=0.010.01×0.010.01×0.010.01]；量測噪聲協方差[R=1002001002]。

圖1 不同算法的RMSE（位置）（一）

由圖2可以看出，勻速直線運動時，擴展卡爾曼濾波算法和本文算法精度相差不大，達到穩態所需時間幾乎相同。目標發生機動轉彎時，一步預測值和量測值相差過大造成EKF算法精度大幅降低，而改進后的算法根據新息矩陣檢測出濾波不穩定，通過修改一步預測值提高了濾波精度。以上情況表明，在機動目標發生突變或運動模型建立不準確時，改進后的算法通過修正預測值，提高了濾波算法的精度。

圖2 不同算法的RMSE（位置）（二）

4 結 語

EKF算法已經廣泛用于機動目標跟蹤中，并取得了良好的效果。但是在實際工程應用中，機動目標的運動狀態不穩定，會進行各種各樣的改變。而這種改變會導致濾波精度大幅度的降低。針對這種情況，本文對EKF算法做了深入研究，并通過新息矩陣的范數來判斷濾波是否穩定，在不穩定時通過修正預測值來提高濾波值的精度。仿真實驗通過對比改進后的算法和EKF算法在不同場景下的精度，表明改進后的算法提高了算法的精度，能夠較好地適應于機動目標跟蹤過程。

參考文獻

[1] 何友，修建娟，關欣，等.雷達數據處理及應用[M].北京：電子工業出版社，2013：63?66.

[2] 徐定杰，賀瑞，沈鋒，等.基于新息協方差的自適應漸消卡爾曼濾波器[J].系統工程與電子技術，2011，33（12）：2696?2698.

[3] 解春明，趙剡，鄧俊云.一種改進的自適應平方根傳遞對準濾波算法[J].系統工程與電子技術，2011，33（3）：622?626.

[4] 林瑞陽，楊東升，邱鋒.Unscented卡爾曼濾波對目標位置預測[J].現代電子技術，2014，37（1）：35?37.

[5] 徐景碩，秦永元，彭蓉.自適應卡爾曼濾波器漸消因子選取方法研究[J].系統工程與電子技術，2004，26（11）：1552?1554.

[6] 楊永建，樊曉光，王晟達，等.基于修正卡爾曼濾波的目標跟蹤[J].系統工程與電子技術，2014，36（5）：846?848.

[7] 巴宏欣，趙宗貴，楊飛，等.機動目標自適應算法[J].系統仿真學報，2002，16（6）：1181?1183.

[8] 張池平，劉宗堯.一種改進的自適應模糊卡爾曼濾波算法[J].計算機工程與應用，2007，43（28）：25?28.

[9] 王向華，覃征，楊新宇，等.基于兩次Kalman濾波的觀測噪聲自適應調整算法[J].系統工程與電子技術，2010，32（2）：232?233.