基于觀測距離的EKF和UKF目標跟蹤方法對比*

趙思琦，胡曉桐，郭 琴，曾萃霞，鄧小飛

(吉首大學信息科學與工程學院，湖南 吉首 416000)

根據(jù)運動觀測器獲得測量距離并對目標進行跟蹤，是目標運動分析領域中的一個經典問題，在很多應用場合都可以獲得較精確的距離信息.因此，研究僅利用距離信息進行目標跟蹤具有十分重要的意義[1].對于非線性系統(tǒng)，Bucy等[2]在1971年提出了擴展卡爾曼濾波(Extended Kalman Filter, EKF).其基本思想是采用 Taylor 變化對非線性方程進行展開，截斷高階項對非線性系統(tǒng)進行一階線性化，以一階線性化方程作為系統(tǒng)狀態(tài)方程和量測方程，然后在標準卡爾曼濾波框架下進行濾波處理.EKF算法具備計算量小、易實現(xiàn)的優(yōu)點，但在處理強非線性系統(tǒng)和非高斯白噪聲環(huán)境時效果較差，且需要計算狀態(tài)方程和量測方程的雅可比(Jacobian)矩陣.此外， EKF 線性化過程中只截取了展開式的一階項，故算法線性化過程中存在較大的近似誤差[3].基于此，Julier等[4]在2000年提出了無跡卡爾曼濾波(Unscented Kalman Filter,UKF).UKF 采用無跡變換(Unscented Transform, UT)對非線性系統(tǒng)狀態(tài)空間變量進行高斯密度(概率密度)近似，通過確定性采樣獲取一定數(shù)量的采樣點來處理非線性系統(tǒng)狀態(tài)方程和量測方程的均值，以及協(xié)方差在卡爾曼濾波框架下的非線性傳遞.UKF不需要計算雅可比矩陣或海森(Hessians)矩陣，拓寬了算法的使用范圍，并通過保留高階項，提高了算法的計算精度[5].相比于EKF，UKF沒有忽略高階項，因此對于非線性分布的統(tǒng)計量有較高的計算精度，有效地克服了EKF估計精度低和穩(wěn)定性差的缺點[6].筆者擬基于觀測距離，分別采用EKF和UKF進行目標跟蹤，并通過Matlab仿真工具對2種方法進行比較.

1 擴展卡爾曼濾波原理

1.1 局部線性化

離散非線性系統(tǒng)動態(tài)方程可表示為

X(k+1)=f(k,X(k))+G(k)W(k)，

(1)

Z(k)=h(k,X(k))+V(k).

(2)

當過程噪聲W(k)和觀測噪聲V(k)恒為0時，非線性系統(tǒng)(1)和(2)的真實解稱為“真軌跡”或“真狀態(tài)”.

為了使數(shù)學計算更加方便，假設輸入的控制量為0，過程噪聲是高斯白噪聲且均值為0，噪聲驅動矩陣G(k)是已知的，觀測噪聲V(k)是加性均值為0的高斯白噪聲，再假設過程噪聲和觀測噪聲序列二者之間相互獨立.

(3)

令

則由(3)式可得狀態(tài)方程

X(k+1)=Φ(k+1|k)X(k)+G(k)W(k)+Φ(k)，

(4)

初始值X(0)=E(X(0)).

(5)

令

則由(5)式可得觀測方程

Z(k)=H(k)X(k)+y(k)+V(k).

(6)

1.2 線性卡爾曼濾波

在得到線性化的狀態(tài)方程(4)和觀測方程(6)之后，可得擴展Kalman濾波遞推關系：

P(k|k+1)=Φ(k|k+1)P(k|k)ΦT(k|k+1)+Q(k+1)，

(7)

P(k+1)=(I-K(k+1)H(k+1))P(k+1|k).

(8)

其中濾波的初值和濾波的誤差方差矩陣的初值分別為

X(0)=E(X(0)),P(0)=var(X(0)).

(9)

(10)

1.3 EKF設計步驟

現(xiàn)選用一個非線性系統(tǒng)來詳細說明EKF設計步驟.設系統(tǒng)狀態(tài)方程為

(11)

觀測方程為

(12)

狀態(tài)方程和觀測方程都為非線性系統(tǒng).EKF算法實現(xiàn)步驟如下：

(ⅰ)初始化初始狀態(tài)X(0)，Z(0)和協(xié)方差矩陣P0；

(ⅵ)協(xié)方差矩陣P(k|k-1)=Φ(k)P(k-1|k-1)ΦT(k)+Q，

(ⅶ)由(7)式可得EKF濾波增益K(k)=P(k|k-1)HT(k)(H(k)P(k|k-1)HT(k)+R)；

(ⅷ)目標狀態(tài)更新為X(k)=X(k|k-1)+K(Z(k)-Z(k|k-1))；

(ⅸ)由(8)式可得協(xié)方差更新為P(k)=(In-K(k)H(k))P(k|k-1).

以上步驟為EKF的一個計算周期，如此循環(huán)，就是各個時刻對非線性系統(tǒng)的處理過程.

2 無跡卡爾曼濾波原理

2.1 UT算法

UT算法是UKF中的核心步驟，是一種對隨機變量在經過非線性變換之后進行概率統(tǒng)計的方法.通過UT算法得到2n+1個Sigma采樣點X和權值ω，再來計算y的統(tǒng)計特征：

(1)計算2n+1個Sigma采樣點：

(13)

(2)計算Sigma采樣點相應的權值：

(14)

其中：m為均值；c為協(xié)方差；i為采樣點的位數(shù)；λ為縮放比例參數(shù)，可用其降低預測誤差，λ=α2(n+k)-n；α為控制采樣點分布狀態(tài)的值.

在UT算法中，最重要的是確定Sigma采樣點的方法，也就是如何確定使用Sigma采樣點的個數(shù)、位置和相應權值.

2.2 UKF設計步驟

對于不同時刻k，X為一個隨機變量，Z為一個觀測變量，分別添加高斯白噪聲W(k)和V(k)，構成一個非線性系統(tǒng)：

(15)

其中f為非線性函數(shù)的狀態(tài)方程，h為非線性函數(shù)的觀測方程.設W(k)的協(xié)方差矩陣為Q，V(k)的協(xié)方差矩陣為R，則UKF算法實現(xiàn)步驟如下：

(ⅰ)根據(jù)UT預測方程(13)和(14)，獲得一組Sigma采樣點及其權值，得到如下的X(i)(k|k)：

(ⅱ)對2n+1個Sigma采樣點進行預測：

X(i)(k+1|k)=f(k,X(i)(k|k))i=1,2,…,2n+1.

(16)

(ⅲ)將(16)式中預測到的Sigma采樣點代入到觀測方程中，得到預測觀測量

Z(i)(k+1|k)=h(X(i)(k+1|k))i=1,2,…,2n+1.

(17)

(ⅳ)由(17)式得到的預測觀測量，通過加權求和得到預測系統(tǒng)結果的均值及協(xié)方差：

(18)

(19)

(ⅵ)根據(jù)卡爾曼增益值獲得準確的狀態(tài)向量和協(xié)方差，對系統(tǒng)結果的狀態(tài)向量和協(xié)方差進行更新：

P(k+1|k+1)=P(k+1|k)-K(k+1)PZkZkKT(k+1).

由上述步驟可以看出，UKF在處理非線性系統(tǒng)問題時，可以直接在預測點附近進行UT，得到Sigma采樣點，將采樣點的均值和協(xié)方差與原統(tǒng)計特性作比較，之后直接進行非線性映射，可得到近似的概率密度函數(shù).

3 基于觀測距離的EKF和UKF目標跟蹤理論及仿真結果

3.1目標跟蹤算法數(shù)學建模

假定目標做勻速直線運動，運動速度為v，在k時刻，設目標的位置為s(k)，經過采樣時間T之后，目標的位置為s(k+1)=s(k)+vT.顯然，在直角坐標系中目標在x和y方向都有相應的分量，運動系統(tǒng)的狀態(tài)量包括x方向的位置、y方向的位置、x方向的速度分量和y方向的速度分量.在目標運動過程中，可表示為

X(k+1)=ΦX(k)+G(k)W(k)，

(20)

其中

通過觀測站或者傳感器對目標進行探測(如雷達探測).設雷達站的位置為(x0,y0)，目標k時刻的位置為(x(k),y(k))，又設觀測量Z(k)為目標與雷達之間的距離，則觀測方程為

(21)

其中V(k)為雷達自身的測量誤差.由此得到勻速運動的系統(tǒng)模型，其中狀態(tài)方程(20)為線性的，觀測方程(21)為非線性的.

3.2 EKF目標跟蹤

根據(jù)局部線性化方法將非線性方程(21)線性化，并由(10)式得到相應的雅可比矩陣

(22)

3.3 UKF目標跟蹤

根據(jù)3.1節(jié)的數(shù)學模型，設采樣時間間隔T=1 s，運行總時間N=60 s，則過程驅動矩陣Φ和噪聲驅動矩陣G(k)為常數(shù)矩陣：

W(k)的協(xié)方差矩陣Q=σw*diag(1,1)，其中σw為可調參數(shù)，σw?1.V(k)的協(xié)方差矩陣R為已知量.設置UT中的相關系數(shù)，α=0.01，K=0，β=2，維數(shù)n=9.

(23)

3.4 仿真結果

根據(jù)(20)—(23)式求解，運行各時刻點，分別得到EKF和UKF的跟蹤軌跡和誤差曲線，結果如圖1和圖2所示.

EKF和UKF對運動目標的跟蹤軌跡如圖1所示.從圖1可以看出，EKF對真實軌跡的仿真效果沒有UKF好，當橫坐標增大到一定程度時，EKF與真實軌跡有較大誤差.這是由泰勒展開式保留一階近似項、省略二階(及以上)項導致的線性化誤差所引起的，因為UKF使用UT，所以在此情況下UKF的仿真結果優(yōu)于EKF.

EKF和UKF的跟蹤誤差如圖2所示.從圖2可以看出，EKF的跟蹤誤差與UKF的跟蹤誤差的差距很明顯，EKF的變化劇烈，而UKF的較好，且UKF均方差變化小，收斂性能優(yōu)于EKF.這說明相較于EKF，UKF具有良好的跟蹤效果.

圖1 EKF和UKF對運動目標的跟蹤軌跡Fig.1 Track of Moving Targets by EKF and UKF

圖2 EKF和UKF的跟蹤誤差Fig.2 EKF and UKF Tracking Errors

4 結語

由EKF和UKF對目標跟蹤的原理可知，EKF是將非線性函數(shù)方程通過求其偏導數(shù)高階導數(shù)和雅可比矩陣作線性化處理，而得到一個近似的線性化函數(shù)方程，再進行目標跟蹤；UKF是直接對非線性函數(shù)方程通過高斯分布而得到概率密度的近似值，再利用UT得到Sigma采樣點，與之前估計的狀態(tài)方程進行非線性映射，從而得到準確的目標跟蹤方程.所以，UKF的準確度和收斂性比EKF的好.UKF算法雖然能夠有效提高非線性系統(tǒng)目標跟蹤的精度，但目前只是在高斯噪聲的環(huán)境下進行處理，若系統(tǒng)是在非高斯的環(huán)境下，則可能出現(xiàn)較大誤差，這就需要對算法進行不斷的改進.