一種改進(jìn)的擴(kuò)展卡爾曼濾波

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

關(guān)鍵詞： 擴(kuò)展卡爾曼濾波； 濾波發(fā)散； 新息； 目標(biāo)跟蹤

中圖分類號： TN911?34 文獻(xiàn)標(biāo)識碼： 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 引 言

擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）主要解決目標(biāo)跟蹤中運(yùn)動模型是非線性時的問題，通常將非線性系統(tǒng)進(jìn)行近似線性化后采用標(biāo)準(zhǔn)卡爾曼濾波。當(dāng)運(yùn)動模型非線性較強(qiáng)或者運(yùn)動目標(biāo)發(fā)生狀態(tài)突變時可能會造成濾波器發(fā)散[1]。主要的解決方案有如下兩種：

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

（2） 通過在不同的跟蹤濾波器之間切換或增大目標(biāo)狀態(tài)維數(shù)來調(diào)整濾波器的結(jié)構(gòu)。

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

1 目標(biāo)跟蹤建模

狀態(tài)方程為：

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

量測方程為：

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

式中：[X（k）]是目標(biāo)的狀態(tài)向量；[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）]為狀態(tài)轉(zhuǎn)移矩陣；[h（k）]為量測矩陣。目標(biāo)運(yùn)動模型為Singer模型：

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

[h=[1 0 0]]

狀態(tài)方程中函數(shù)的一階展開式為：

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

擴(kuò)展卡爾曼濾波過程如下：

狀態(tài)的一步預(yù)測方程為：

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

協(xié)方差的預(yù)測方程為：

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

量測預(yù)測方程為：

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

新息協(xié)方差為：

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

狀態(tài)更新方程為：

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

協(xié)方差更新方程為：

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

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

2 改進(jìn)算法

系統(tǒng)達(dá)到穩(wěn)態(tài)后，預(yù)測協(xié)方差、新息和增益均趨于極小值[6]。若此時目標(biāo)進(jìn)行了較強(qiáng)的機(jī)動性改變，預(yù)測值不再準(zhǔn)確，新息變大，但是增益不能隨之改變，造成濾波精度下降；建立的運(yùn)動模型與目標(biāo)實際的運(yùn)動模型不匹配是由于預(yù)測值的不準(zhǔn)確而導(dǎo)致濾波值不準(zhǔn)確。由于增益矩陣K的計算量較大，重新計算增益矩陣K會導(dǎo)致實時性降低，所以本文通過一步預(yù)測值的改進(jìn)來提高濾波值的精度。

在濾波趨于穩(wěn)態(tài)后，新息矩陣趨于極小值，可根據(jù)新息矩陣的大小判斷是否應(yīng)該調(diào)整預(yù)測值：

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

式中：[zmax]為量測誤差矩陣的最大值；[·]為矩陣的F?范數(shù)；[λ]取值范圍為[[0.3，0.9]]。當(dāng)新息矩陣滿足式（11）時，認(rèn)為濾波正常；當(dāng)式（11）不成立時，認(rèn)為目標(biāo)發(fā)生了較強(qiáng)的機(jī)動，此時量測值的可靠性較高，一步預(yù)測值的可靠性較低，應(yīng)修正一步預(yù)測值。

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

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

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

3 仿 真

通過設(shè)置下列兩種實驗場景來比較EKF和本文提出的改進(jìn)算法：

（1） 機(jī)動目標(biāo)進(jìn)行勻速直線運(yùn)動一段時間（20 s）后進(jìn)行加速運(yùn)動，加速運(yùn)動持續(xù)時間為20 s，然后再次進(jìn)行60 s的勻速運(yùn)動。

（2） 機(jī)動目標(biāo)在不同的時間段進(jìn)行勻速、轉(zhuǎn)彎和勻速運(yùn)動，運(yùn)動持續(xù)時間分別為80 s，40 s，40 s。實驗結(jié)果通過對比目標(biāo)位置的RMSE來比較兩種算法的精度。

3.1 仿真1：勻速運(yùn)動和加速運(yùn)動

目標(biāo)起始位置為：

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

目標(biāo)運(yùn)動時間為100 s，采樣間隔為2 s；系統(tǒng)的噪聲協(xié)方差矩陣[Q=0.010.01×0.010.01×0.010.01]；量測噪聲的協(xié)方差矩陣[R=1002001002]。

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

3.2 仿真2：勻速運(yùn)動和轉(zhuǎn)彎運(yùn)動

目標(biāo)起始位置為：

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

目標(biāo)運(yùn)動時間為160 s，采樣間隔為2 s。系統(tǒng)噪聲協(xié)方差[Q=0.010.01×0.010.01×0.010.01]；量測噪聲協(xié)方差[R=1002001002]。

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

由圖2可以看出，勻速直線運(yùn)動時，擴(kuò)展卡爾曼濾波算法和本文算法精度相差不大，達(dá)到穩(wěn)態(tài)所需時間幾乎相同。目標(biāo)發(fā)生機(jī)動轉(zhuǎn)彎時，一步預(yù)測值和量測值相差過大造成EKF算法精度大幅降低，而改進(jìn)后的算法根據(jù)新息矩陣檢測出濾波不穩(wěn)定，通過修改一步預(yù)測值提高了濾波精度。以上情況表明，在機(jī)動目標(biāo)發(fā)生突變或運(yùn)動模型建立不準(zhǔn)確時，改進(jìn)后的算法通過修正預(yù)測值，提高了濾波算法的精度。

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

4 結(jié) 語

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

參考文獻(xiàn)

[1] 何友，修建娟，關(guān)欣，等.雷達(dá)數(shù)據(jù)處理及應(yīng)用[M].北京：電子工業(yè)出版社，2013：63?66.

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

[3] 解春明，趙剡，鄧俊云.一種改進(jìn)的自適應(yīng)平方根傳遞對準(zhǔn)濾波算法[J].系統(tǒng)工程與電子技術(shù)，2011，33（3）：622?626.

[4] 林瑞陽，楊東升，邱鋒.Unscented卡爾曼濾波對目標(biāo)位置預(yù)測[J].現(xiàn)代電子技術(shù)，2014，37（1）：35?37.

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

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

[7] 巴宏欣，趙宗貴，楊飛，等.機(jī)動目標(biāo)自適應(yīng)算法[J].系統(tǒng)仿真學(xué)報，2002，16（6）：1181?1183.

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

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