基于改進無跡卡爾曼濾波算法的水下目標(biāo)跟蹤

張 凱

(鄭州旅游職業(yè)學(xué)院機電工程系 河南 鄭州 450009)

0 引 言

水下目標(biāo)的測量噪聲很高，需要使用濾波器消除這種噪聲，常見的線性系統(tǒng)濾波器有維納濾波器和卡爾曼濾波器?？柭鼮V波器是一種應(yīng)用非常廣泛的線性濾波器，能對過程噪聲和測量噪聲進行最優(yōu)濾波。但是其前提條件是已知噪聲的協(xié)方差，過程噪聲和測量噪聲滿足白噪聲或者近似為白噪聲，且要求兩者獨立互不相關(guān)。但現(xiàn)實世界中系統(tǒng)基本都是非線性的，擴展卡爾曼濾波(Extended Kalman Filter，EKF)和無跡卡爾曼濾波(Unscented Kalman Filter，UKF)是常見的兩種非線性濾波系統(tǒng)。EKF是針對非線性系統(tǒng)應(yīng)用非常普遍的狀態(tài)估計方法，但是其依賴對非線性系統(tǒng)的線性化來傳播狀態(tài)的均值和協(xié)方差，依賴于局部非線性強度，當(dāng)系統(tǒng)的線性化比較嚴(yán)重時，其估計并不準(zhǔn)確。而且在線性化時需要推導(dǎo)復(fù)雜的雅可比矩陣，需要計算并保留非線性函數(shù)泰勒展開式的一階近似項，不適用于更高階的非線性系統(tǒng)。Julier等[1-2]基于近似概率分布比近似非線性函數(shù)或非線性變換要相對簡單的思想，提出了采用無跡變換(Unscented transformation，UT)結(jié)合卡爾曼濾波的無跡卡爾曼濾波算法。該算法不像EKF采用近似系統(tǒng)的非線性函數(shù)，而是近似量測方程的概率密度函數(shù)，降低了EKF線性化帶來的誤差，而且相比于EKF具有更小的估計誤差。在UKF系統(tǒng)中的，無須計算雅可比矩陣，其狀態(tài)分布是利用σ點通過非線性函數(shù)進行近似變換得到，比EKF更適合用于非線性系統(tǒng)的目標(biāo)跟蹤。

文獻[3]提出一種集成無跡卡爾曼，使用常規(guī)的最小二乘法先對多個UKF的估計值積分，再進行濾波。該方法沒有考慮量測信息的統(tǒng)計特性的權(quán)重，因而跟蹤的精度有限。馬艷等[4]提出自適應(yīng)調(diào)整目標(biāo)狀態(tài)噪聲的概率分布的無跡卡爾曼濾波，改善了非機動目標(biāo)的估計性能。但該算法在維數(shù)較高的非線性系統(tǒng)中，對概率分布的調(diào)整易引起系統(tǒng)發(fā)散。高文娟等[5]提出了基于EKF和UKF的交互式多模型的水下機動目標(biāo)跟蹤算法。宋緒棟等[6]提出了基于純方位角測量的水下目標(biāo)被動跟蹤技術(shù)。吳盤龍等[7]提出了基于平方根UKF的水下純方位目標(biāo)跟蹤，使用協(xié)方差平方根代替協(xié)方差，解決了無跡卡爾曼濾波算法中誤差協(xié)方差矩陣負定導(dǎo)致濾波發(fā)散的問題。高劍等[8]提出了采用三階精度獲得非線性變換的均值和協(xié)方差的估計的無跡卡爾曼濾波算法。鄧兵等[9]提出了基于估計偏差修正的擴展卡爾曼濾波算法，減小了局部線性化截斷誤差對于觀測矩陣的影響，但是其增加了系統(tǒng)的計算量。蔚婧等[10]提出了通過曲線擬合前幾個時刻的方位角測量序列獲得當(dāng)前時刻目標(biāo)的估計值對目標(biāo)參數(shù)進行估計的純方位目標(biāo)跟蹤算法，但該算法的適用范圍較窄。吳林煌等[11]采用神經(jīng)網(wǎng)絡(luò)對跟蹤系統(tǒng)建模，提出基于自適應(yīng)EKF與神經(jīng)網(wǎng)絡(luò)的預(yù)失真算法，該算法中的測量誤差矩陣的自適應(yīng)迭代公式復(fù)雜度高且計算量大。

因為每個測量值都攜帶有噪聲，而不同設(shè)備得到的測量值的精度也各不相同。基于該思想，本文應(yīng)用多個UKF對目標(biāo)進行跟蹤，設(shè)定每個UKF不同的權(quán)重，將根據(jù)得到的測量值的精度大小而賦予相應(yīng)的權(quán)重，精度高的權(quán)重取值較大，反之取值小，再利用加權(quán)最小二乘法(Weighted Least Squares,WLS)對測量值進行積分得到改進的無跡卡爾曼濾波(Improved UKF，IUKF)算法。

針對目標(biāo)的位置和方向，本文將擴展卡爾曼濾波(EKF)、無跡卡爾曼濾波(UKF)和改進的無跡卡爾曼濾波(IUKF)應(yīng)用到水下的目標(biāo)跟蹤中，并對算法的可靠性和準(zhǔn)確性進行了仿真分析。仿真結(jié)果表明，改進的無跡卡爾曼濾波能更有效地對水下目標(biāo)進行跟蹤，比另外兩種方法具有較高的位置和方向估計精度。

1 濾波算法

卡爾曼濾波是最優(yōu)線性濾波，在假定系統(tǒng)模型和噪聲統(tǒng)計特性已知的前提下效果很好。但在一般的應(yīng)用中，大多數(shù)都是非線性系統(tǒng)，甚至歐姆定律也是在一定范圍內(nèi)的近似表達式，而且多數(shù)系統(tǒng)不容易獲得精確的系統(tǒng)模型和測量噪聲的統(tǒng)計特性。因為自然界的噪聲往往是非高斯或者近似高斯的，而且存在重尾效應(yīng)，需要對其進行修正以適應(yīng)這種可能性。

1.1 擴展卡爾曼濾波

在實際工程應(yīng)用中，非線性系統(tǒng)的情況較多，所以常常不能滿足應(yīng)用卡爾曼濾波的前提條件，如果應(yīng)用卡爾曼濾波，容易使濾波發(fā)散和精度下降。擴展卡爾曼濾波是比其他非線性濾波較為簡便且應(yīng)用廣泛的一種方法，用簡單的算法即可獲得較好的結(jié)果。

離散時間非線性系統(tǒng)及其EKF遞推步驟如下[12]：

步驟1系統(tǒng)狀態(tài)方程和測量方程：

xk=fk-1(xk-1,uk-1,wk-1)yk=hk(xk,vk)

(1)

式中：f(·)和h(·)分別表示系統(tǒng)狀態(tài)函數(shù)和測量函數(shù)；xk和yk分別為k時刻的狀態(tài)值和測量值；wk和vk為狀態(tài)噪聲和測量噪聲，其均值均為0，協(xié)方差分別為Qk和Rk，且二者互不相關(guān)。

步驟2初始化濾波如下：

(2)

步驟3循環(huán)k=1，2，…，完成步驟4至步驟7。

步驟4計算偏微分(雅可比矩陣)矩陣(如果狀態(tài)方程是線性的，可跳過此步驟)：

(3)

步驟5狀態(tài)估計和估計誤差協(xié)方差的時間更新：

(4)

步驟6計算如下偏微分矩陣：

(5)

步驟7狀態(tài)估計的測量更新和估計誤差協(xié)方差的更新如下：

Pk+=(I-KkHk)Pk-

(6)

1.2 無跡卡爾曼濾波

非線性系統(tǒng)的概率密度函數(shù)很難通過一般的非線性函數(shù)求解。雖然EKF和迭代EKF求解均值和協(xié)方差的線性化變換近似等同于真實的非線性變換，但僅僅是在較簡單的二維情況下近似成立。當(dāng)系統(tǒng)的維數(shù)大于二維的時候，EKF和迭代EKF的估計誤差會急劇增大。而無跡卡爾曼濾波能較好地解決這個問題，得到滿意的近似結(jié)果。無跡卡爾曼濾波的思想是：因為相對于整個概率分布函數(shù)來說，對單一點執(zhí)行非線性變換是比較容易的，所以可以在狀態(tài)方程中找一組點(稱為sigma點)，再將sigma點經(jīng)過已知的非線性函數(shù)y=h(x)變換，得到變換后的均值和協(xié)方差，再估計出y的真實均值和方差，從而來近似狀態(tài)向量的真實概率分布函數(shù)。

無跡卡爾曼濾波簡要步驟如下[12]：

設(shè)滿足N維離散非線性系統(tǒng)為(假設(shè)狀態(tài)方程和測量方程相對噪聲是線性的)

xk+1=f(xk,uk,tk)+wkyk=h(xk,tk)+vk

(7)

步驟1初始化：

(8)

(9)

步驟3時間更新：

(10)

步驟4量測更新：

(11)

2 系統(tǒng)狀態(tài)方程和量測方程

2.1 系統(tǒng)狀態(tài)方程

假設(shè)系統(tǒng)做勻加速運動，目標(biāo)的初始位置為(x0,y0)，k時刻的位置為(xk,yk)，x軸和y軸方向的速度分別為vx,k、vy,k，初始速度為(vx,0,vy,0)，k時刻的速度為(vx,k,vy,k)，x軸和y軸方向的加速度分別為ax,k、ay,k。即X(k)=[xk,yk,vx,k,vy,k,ax,k,ay,k]T，其狀態(tài)變量中的關(guān)系分別為(其中T為采樣時間)：

xk=x0+vx,kT+0.5ax,kT2yk=y0+vy,kT+0.5ay,kT2vx,k=vx,0+ax,kTvy,k=vy,0+ay,kT

(12)

則目標(biāo)系統(tǒng)的運動狀態(tài)方程為：

X(k+1)=ΦX(k)+w(k)=

(13)

式中：Φ為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，w(k)為系統(tǒng)狀態(tài)噪聲。

2.2 系統(tǒng)量測方程

假設(shè)聲吶系統(tǒng)所在的位置為坐標(biāo)原點，對目標(biāo)M進行跟蹤，可以得到目標(biāo)M和聲吶之間的距離及角度。模型如圖1所示。

圖1 量測模型

由該聲吶系統(tǒng)獲得目標(biāo)的離散距離rk和方位角θk，則在該直角坐標(biāo)系中，系統(tǒng)的量測方程為：

Y(k+1)=h(X(k))+v(k)=

h(r(k),θ(k))+v(k)=

(14)

由式(14)可以知道該模型的狀態(tài)方程是線性的，量測方程是非線性的。在對系統(tǒng)應(yīng)用EKF時，需要計算量測方程的雅可比矩陣為：

(15)

2.3 改進的UKF(IUKF)

(16)

Pi(k/k)=diag[Pi1Pi2…Pin]

(17)

Pij是第i個UKF給定的狀態(tài)向量X的第j個元素的估計方差。則：

(18)

式中：Ri(k)是高斯噪聲的p×1矩陣，是第p個UKF給出的估計的第i個元素，其均值為零的協(xié)方差矩陣。設(shè)每個測量值的噪聲都是零均值且相互獨立，設(shè)測量噪聲為vi，則：

(19)

測量噪聲的協(xié)方差矩陣為：

(20)

為了得到測量值，使式(21)最小。取回歸函數(shù)為：

(21)

(22)

進而得到式(22)的解的形式：

(23)

3 仿真結(jié)果及分析

為了對比驗證IUKF與EKF、UKF兩種算法的性能，進行100次Monte Carlo數(shù)值仿真實驗，比較其濾波結(jié)果的均方根誤差(RMSE)。仿真基于以下的假設(shè)：聲吶所在的位置為坐標(biāo)原點，假設(shè)系統(tǒng)目標(biāo)在水下的深度恒為35 m，在這個平面內(nèi)做勻加速運動，目標(biāo)開始位置為(1 000,1 000)，x方向的初速度為20 m/s，y方向的初速度為40 m/s，x方向的加速度為3 m/s2，y方向的加速度為-3.6 m/s2，仿真總時間t=50 s，采樣時間間隔T=0.5 s，狀態(tài)噪聲wk和測量噪聲vk二者互不相關(guān)，其協(xié)方差Qk和Rk分別如下：

(24)

(25)

應(yīng)用EKF、UKF和IUKF三種方法濾波的結(jié)果如圖2至圖4所示。圖2是對目標(biāo)運動軌跡的仿真對比，可以看出IUKF比EKF、UKF更接近于真實的運動軌跡。圖3是對目標(biāo)位置跟蹤誤差的仿真。圖4是對目標(biāo)運動速度跟蹤誤差的仿真。表1給出了三種方法濾波后位置、速度的均方誤差對比。可以看出，IUKF的跟蹤誤差小，濾波誤差曲線收斂較快。這是因為EKF、UKF將目標(biāo)的非線性測量方程近似為一個線性濾波問題，然后套用線性濾波求解原非線性濾波問題，其濾波的穩(wěn)定性及狀態(tài)的估計精度不但與過程噪聲和量測噪聲的統(tǒng)計特性關(guān)系很大(如這里假設(shè)Qk和Rk在濾波過程中一直不變，如果設(shè)定的值不太準(zhǔn)確，易產(chǎn)生誤差積累，導(dǎo)致濾波發(fā)散)，而且與狀態(tài)方程及量測方程中的非線性因子有關(guān)，是一種次優(yōu)的濾波算法。而IUKF是使用UT變換處理非線性的，不用求解其量測方程的雅可比矩陣來對其進行近似的線性化，而是對狀態(tài)向量的概率密度函數(shù)進行的，而近似后的概率密度函數(shù)還是滿足高斯分布的，因而其濾波效果較好，具有更好的濾波精度，但其計算量卻與EKF、UKF相近。

圖2 目標(biāo)運動軌跡

圖3 濾波算法的位置跟蹤誤差

圖4 濾波算法的速度跟蹤誤差

表1 三種方法狀態(tài)估計的RMSE比較

4 結(jié) 語

本文以水下機動目標(biāo)跟蹤為應(yīng)用背景，提出了一種改進的無跡卡爾曼濾波算法(適用于非線性應(yīng)用)跟蹤估計水下目標(biāo)運動參數(shù)的方法，在確定目標(biāo)的初始距離和方位的情況下對目標(biāo)運動進行了仿真，并和EKF、UKF進行了跟蹤結(jié)果對比，驗證了IUKF算法在有效性、測量誤差及機動性方面優(yōu)于EKF、UKF，表明該方法可方便地應(yīng)用于水下目標(biāo)的跟蹤，比EKF、UKF更加適合對水下機動目標(biāo)的跟蹤。另外，盡管IUKF的估計精度比EKF、UKF高，但IUKF不能應(yīng)用于非高斯分布系統(tǒng)，而粒子濾波可以解決這一問題。