EKF與UKF的目標(biāo)跟蹤算法應(yīng)用與對(duì)比

唐 哲，王庭軍，陳志豪

（中國(guó)航天科技集團(tuán)公司九院16所，陜西 西安 710100）

目標(biāo)跟蹤是計(jì)算機(jī)視覺(jué)領(lǐng)域的一個(gè)研究熱點(diǎn)和重要研究方向[1]。目標(biāo)跟蹤的基本問(wèn)題是在某個(gè)特定的區(qū)域內(nèi)找到目標(biāo)的位置，以確定目標(biāo)的運(yùn)動(dòng)軌跡、具體形態(tài)，在精確制導(dǎo)、智能視頻監(jiān)控、軍事偵察、人機(jī)交互、機(jī)器人導(dǎo)航等領(lǐng)域具有廣泛的應(yīng)用，有著重要的實(shí)用價(jià)值[2]。針對(duì)目標(biāo)外觀變化建立外觀模型，進(jìn)行在線學(xué)習(xí)并且利用輔助目標(biāo)來(lái)改善目標(biāo)跟蹤的效果成為近年來(lái)的研究熱點(diǎn)[3]。目標(biāo)跟蹤算法是目標(biāo)跟蹤技術(shù)的核心，運(yùn)動(dòng)目標(biāo)跟蹤算法具有廣泛的研究?jī)r(jià)值和挑戰(zhàn)性，當(dāng)前主流的運(yùn)動(dòng)目標(biāo)跟蹤算法有Kalman濾波算法、Mean Shift算法、粒子濾波算法等。其中Kalman濾波目標(biāo)跟蹤算法成本較低，實(shí)時(shí)性強(qiáng)、計(jì)算量小，效率較高，預(yù)測(cè)結(jié)果穩(wěn)定，廣泛應(yīng)用于導(dǎo)彈追蹤、軍事雷達(dá)系統(tǒng)和機(jī)器人導(dǎo)航等領(lǐng)域。

自20世紀(jì)60年代卡爾曼濾波理論一經(jīng)提出，就不斷發(fā)展并得到越來(lái)越廣泛的應(yīng)用[4]。卡爾曼濾波算法在滿足白噪聲、隨機(jī)變量服從高斯分布的線性系統(tǒng)中廣泛應(yīng)用。隨著對(duì)卡爾曼濾波原理研究的不斷深入，該濾波理論在慣性器件姿態(tài)測(cè)量[5-6]、車載組合導(dǎo)航、雷達(dá)目標(biāo)跟蹤和天氣預(yù)測(cè)等重要領(lǐng)域應(yīng)用越來(lái)越廣泛。以雷達(dá)的目標(biāo)跟蹤系統(tǒng)為例，目標(biāo)的狀態(tài)方程和觀測(cè)方程在同一坐標(biāo)系下不可能都是線性的[7]。擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）與無(wú)跡卡爾曼濾波（Unscented Kalman Filter，UKF）就是兩種典型的處理非線性濾波問(wèn)題的方法。

其中EKF的核心思想是，圍繞濾波值X(k)將非線性狀態(tài)函數(shù)f(·)和非線性觀測(cè)函數(shù)h(·)展開(kāi)成Taylor級(jí)數(shù)并略去二次項(xiàng)及以上項(xiàng)，得到一個(gè)近似化的線性化模型，然后應(yīng)用經(jīng)典卡爾曼濾波完成對(duì)目標(biāo)的濾波估計(jì)。所以EKF的濾波精度和系統(tǒng)的非線性程度有關(guān)，隨著系統(tǒng)的非線性程度的增強(qiáng)，濾波精度會(huì)變差，甚至出現(xiàn)發(fā)散現(xiàn)象。與此相類似的，如果在非線性函數(shù)的Taylor級(jí)數(shù)展開(kāi)中不僅保留一次項(xiàng)，還保留二次項(xiàng)，則相應(yīng)的濾波算法即為二階廣義卡爾曼濾波，簡(jiǎn)稱二階EKF。從理論和實(shí)踐上來(lái)講，二階EKF總是優(yōu)于一階EKF的，代價(jià)是需要計(jì)算更復(fù)雜的二階雅克比矩陣。而UKF摒棄了對(duì)非線性函數(shù)進(jìn)行線性化處理的傳統(tǒng)做法，它是利用UT變換在估計(jì)點(diǎn)附近確定采樣點(diǎn)，用這些樣本點(diǎn)表示的高斯密度近似狀態(tài)的概率密度函數(shù)。UKF算法是對(duì)非線性函數(shù)的概率密度分布進(jìn)行近似，并不是對(duì)非線性函數(shù)進(jìn)行近似，并沒(méi)有把高階項(xiàng)忽略掉，因此對(duì)于非線性分布的統(tǒng)計(jì)量有較好的計(jì)算精度[8]。

文章首先從理論角度給出了EKF與UKF濾波的原理，建立了單目標(biāo)跟蹤模型，并進(jìn)行了仿真，仿真數(shù)據(jù)結(jié)果表明，二階EKF濾波精度較一階EKF有所提高，UKF明顯優(yōu)于一階EKF濾波，進(jìn)而可以在目標(biāo)跟蹤或者其他一般的非線性系統(tǒng)中廣泛應(yīng)用，具有重要的理論和工程實(shí)用價(jià)值。

1 非線性濾波技術(shù)

1.1 EKF算法

離散非線性系統(tǒng)的動(dòng)態(tài)方程可表示如下：

式中X(k)是n維系統(tǒng)狀態(tài)向量；Z(k)為m維觀測(cè)向量；f(·)和h(·)為非線性函數(shù)；G(k)為系統(tǒng)噪聲驅(qū)動(dòng)陣；W(k)零均值系統(tǒng)白噪聲；V(k)為零均值觀測(cè)白噪聲；{W(k)}和{V(k)}互不相關(guān)，方差分別為Q和R。

將系統(tǒng)方程（1）圍繞濾波值做一階泰勒展開(kāi)，得：

其中，

將觀測(cè)方程（2）圍繞濾波值X?k做一階泰勒展開(kāi)，得：

其中，

因此，非線性動(dòng)態(tài)方程可線性化為：

與線性卡爾曼濾波基本方程相比，在線性化后的方程中，狀態(tài)轉(zhuǎn)移矩陣F(k+1/k)和觀測(cè)矩陣H(k)由f和h的雅克比矩陣代替，再利用線性卡爾曼濾波遞推方程對(duì)系統(tǒng)進(jìn)行濾波即可。

1.2 UKF算法

1.2.1 UT變換

下面以對(duì)稱分布采樣的UT變換為例，簡(jiǎn)單介紹UT變換的基本原理。假設(shè)一個(gè)非線性變換Y=f(X)，可以把n維隨機(jī)向量X變換為m維隨機(jī)向量Y，并且已知X的均值和方差陣PX，則Y的均值和方差PY可以通過(guò)UT變換求取，步驟如下：

根據(jù)和PX計(jì)算2n+1個(gè)σ樣本點(diǎn)：

計(jì)算非線性變換產(chǎn)生的樣本點(diǎn)：

確定權(quán)值：

式中，α是很小的正數(shù)；κ=3－n；β為非負(fù)的權(quán)系數(shù)，與X的分布形式有關(guān)。

確定映射的均值和方差陣：

1.2.2 UKF濾波算法實(shí)現(xiàn)

在選定濾波初值：

的前提下，UKF濾波遞推步驟為：

計(jì)算k－1時(shí)刻2n+1個(gè)σ樣本點(diǎn)；

計(jì)算k時(shí)刻的一步預(yù)測(cè)模型值：

計(jì)算一步預(yù)測(cè)值的σ樣本點(diǎn)；

將預(yù)測(cè)的σ樣本點(diǎn)帶入觀測(cè)方程：

計(jì)算增益矩陣：

計(jì)算濾波值：

2 單目標(biāo)跟蹤模型

2.1 系統(tǒng)建模

考慮一個(gè)在二維平面x－y內(nèi)運(yùn)動(dòng)的質(zhì)點(diǎn)M，其在某一時(shí)刻k的位置、速度和加速度可用矢量

表示。假設(shè)M在水平方向（x軸方向）作近似勻加速運(yùn)動(dòng)，垂直方向上（y軸方向）亦作勻加速直線運(yùn)動(dòng)。兩方向上運(yùn)動(dòng)都具有加性系統(tǒng)噪聲W(k），則在笛卡爾坐標(biāo)系下該質(zhì)點(diǎn)運(yùn)動(dòng)的狀態(tài)方程為式中F如下所示，其中，T為采樣時(shí)間。

假設(shè)位于坐標(biāo)原點(diǎn)(0,0)的雷達(dá)對(duì)質(zhì)點(diǎn)M進(jìn)行跟蹤，可以得到雷達(dá)與M之間的距離和質(zhì)點(diǎn)M相對(duì)于雷達(dá)的。

角度，實(shí)際測(cè)量中雷達(dá)具有測(cè)量噪聲v(k)，觀測(cè)方程為：

系統(tǒng)噪聲W(k)具有協(xié)方差陣Qk、V(k)具有協(xié)方差陣Rk，兩者均已知。初始濾波值取X0，初始濾波均方誤差陣P0已知。

2.2 EKF跟蹤算法

對(duì)于上述的單目標(biāo)跟蹤模型，可見(jiàn)其狀態(tài)方程式線性的，觀測(cè)方程是非線性的。由（4）式方法可得其線性化觀測(cè)方程為：

其中，

下面列出關(guān)于狀態(tài)方程（21）和觀測(cè)方程（22）的EKF濾波遞推方程。

狀態(tài)一步預(yù)測(cè)：

一步預(yù)測(cè)均方誤差陣：

濾波增益：

濾波均方誤差陣：

狀態(tài)濾波值：

2.3 二階EKF跟蹤算法

在二階EKF跟蹤算法中，對(duì)非線性函數(shù)h(·)進(jìn)行二階Taylor級(jí)數(shù)展開(kāi)，應(yīng)用二階EKF遞推方程并結(jié)合該模型的特殊性，可以得到二階EKF與一階EKF的不同之處僅在于狀態(tài)濾波值增加了一個(gè)修正項(xiàng)πk，使得估計(jì)為無(wú)偏估計(jì)[9-10]，即：

式中，

其中Tr為對(duì)矩陣求跡，二階雅克比矩陣如下，

3 仿真與分析

指定濾波初值X0，W(k)和V(k)的協(xié)方差矩陣Qk和Rk及初始濾波均方誤差陣P0，如下：

選擇采樣時(shí)間為0.5 s，采樣次數(shù)為100次，在Matlab平臺(tái)中對(duì)模型仿真。

首先，比較相同條件下的一階EKF濾波與UKF濾波結(jié)果，一階EKF濾波與UKF濾波的位置誤差、速度誤差和加速度誤差對(duì)比如圖1—3所示。

由圖1可以看出，在相同仿真條件下，大多數(shù)仿真時(shí)間內(nèi)，EKF濾波位置誤差明顯大于UKF濾波位置誤差，而且在第2 s、第14 s、第19 s、第44 s附近均出現(xiàn)較大的誤差值。僅在第8～10 s內(nèi)，UKF濾波誤差值略大于EKF濾波誤差值。圖1所示100個(gè)時(shí)間步長(zhǎng)UKF位置誤差均值約為6.53 m，EKF位置誤差均值約為10.93 m。

圖1 EKF與UKF濾波位置誤差對(duì)比

從圖2和圖3所示的濾波速度誤差和濾波加速度誤差，也可以看出在大多數(shù)濾波時(shí)間內(nèi)，UKF濾波是優(yōu)于EKF濾波的。這表明從整體上來(lái)說(shuō)，UKF的狀態(tài)估計(jì)優(yōu)于EKF的狀態(tài)估計(jì)。

圖2 EKF與UKF濾波速度誤差對(duì)比

圖3 EKF與UKF濾波加速度誤差對(duì)比

為了衡量誤差的整體水平，對(duì)100個(gè)時(shí)間序列的仿真位置誤差求平均值，并做多次仿真實(shí)驗(yàn)，每次仿真的濾波位置誤差平均值對(duì)比情況如表1所示，可以看出，UKF濾波誤差均值大多數(shù)情況下是小于EKF濾波誤差的，也就是說(shuō)，UKF從概率統(tǒng)計(jì)意義上優(yōu)于EKF。

表1 試驗(yàn)位置誤差對(duì)比（單位：m）

在相同條件下，一階EKF與二階EKF仿真位置誤差對(duì)比情況如表2所示，由表2可知，二階EFK濾波精度比一階EFK濾波精度有所提高，但提高并不明顯，這主要是因?yàn)槭剑?9）中的修正項(xiàng)πk相比Xk的量級(jí)較低，即二階雅克比矩陣Dk,1較小。

參考文獻(xiàn)[9]中，二階EKF濾波比一階EKF濾波表現(xiàn)出較大的改進(jìn)，原因之一是其非線性項(xiàng)包含求導(dǎo)不變性的指數(shù)項(xiàng)。而文中單目標(biāo)跟蹤模型非線性項(xiàng)，二階雅克比矩陣較小，所以二階EKF和一階EKF濾波精度相近。對(duì)于非線性系統(tǒng)，非線性濾波方法都優(yōu)于嚴(yán)格的線性濾波方法，但沒(méi)有一種確定的非線性濾波方法在任何系統(tǒng)、任何模型中總能優(yōu)于其他非線性濾波。不同算法之間的比較是主觀的，和系統(tǒng)非線性的強(qiáng)弱、系統(tǒng)動(dòng)力學(xué)或隨機(jī)噪聲的產(chǎn)生有很大的關(guān)系[11-12]。

4 結(jié)語(yǔ)

EKF是通過(guò)將非線性方程局部線性化的策略，并結(jié)合線性卡爾曼濾波遞推方程來(lái)實(shí)現(xiàn)非線性系統(tǒng)濾的。在線性化過(guò)程中需要計(jì)算非線性函數(shù)的雅克比矩陣，具有一定的計(jì)算量；并且需要忽略高階項(xiàng)，引入系統(tǒng)誤差，甚至可能導(dǎo)致濾波發(fā)散。UKF是通過(guò)一種非線性變換來(lái)解決均值和協(xié)方差的傳遞問(wèn)題的，無(wú)需忽略高階項(xiàng)，提高了濾波精度，原理上適合任何非線性系統(tǒng)的濾波問(wèn)題。本文簡(jiǎn)單介紹了一階EKF、二階EKF和UKF的濾波原理，建立了單目標(biāo)跟蹤系統(tǒng)模型并在相同條件下進(jìn)行了仿真。對(duì)仿真數(shù)據(jù)分析表明，UKF濾波精度明顯高于一階EFK濾波精度。在非線性不強(qiáng)的條件下，二階EKF相對(duì)于一階EKF的優(yōu)越性體現(xiàn)不明顯。說(shuō)明UKF可以實(shí)際應(yīng)用于目標(biāo)跟蹤系統(tǒng)，進(jìn)而可以在一般的非線性系統(tǒng)中廣泛應(yīng)用，具有很高的理論和工程實(shí)用價(jià)值。

[參考文獻(xiàn)]

[1]YILMAZ A，JAVED O，SHAH M.Object tracking[J].ACM Computer Survey，2006（4）：13.

[2]羅海波，許凌云，惠斌，等.基于深度學(xué)習(xí)的目標(biāo)跟蹤方法研究現(xiàn)狀與展望[J].紅外與激光工程，2017（5）：406-413.

[3]LIU Q，ZHAO X，HOU Z.Survey of single-target visual tracking methods based on online learning[J].Computer Vision Letters，2014（5）：419-428.

[4]KALMAN R E.A new approach to linear fi ltering and prediction problems[J].Journal of Basic Engineering，1960（1）：35-45.

[5]周樹(shù)道，金永奇，衛(wèi)克晶，等.采用卡爾曼濾波算法的MEMS器件姿態(tài)測(cè)量[J].實(shí)驗(yàn)室研究與探索，2015（2）：38-42.

[6]XU H，ZENG C，HUANG Q H.Drift compensati-on of mems gyro with kalman fi ltering algorithm[J].Chinese Journal of Sensors and Actuators，2016（7）：962-965.

[7]萬(wàn)莉，劉焰春，皮亦鳴.EKF、UKF、PF目標(biāo)跟蹤性能的比較[J].雷達(dá)科學(xué)與技術(shù)，2007（1）：13-16.

[8]秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].3版.西安：西北工業(yè)大學(xué)出版社，2015.

[9]西蒙.最優(yōu)狀態(tài)估計(jì)：卡爾曼，H∞及非線性濾波[M].張勇剛，李寧，奔粵陽(yáng)，譯，北京：國(guó)防工業(yè)出版社，2013.

[10]HENRIKSEN R.The truncated second-order nonlinear fi lter revi-sited[J].IEEE Transactions on Automatic Control，1982（1）：247-251.

[11]SCHWARTZ L，STEAR E.A computational comparison of several nonlinear fi lters[J].IEEE Transactions on Automatic Control，1968（1）：83-86.

[12]WISHER R，TABACZYNSKI J，ATHANS M.A comparison of three non-linear fi lters[J].Automatica，1969（4）：487-496.