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

張 凱

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

0 引 言

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

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

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

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

1 濾波算法

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

1.1 擴展卡爾曼濾波

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

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

步驟1系統狀態方程和測量方程：

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

(1)

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

步驟2初始化濾波如下：

(2)

步驟3循環k=1，2，…，完成步驟4至步驟7。

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

(3)

步驟5狀態估計和估計誤差協方差的時間更新：

(4)

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

(5)

步驟7狀態估計的測量更新和估計誤差協方差的更新如下：

Pk+=(I-KkHk)Pk-

(6)

1.2 無跡卡爾曼濾波

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

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

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

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

(7)

步驟1初始化：

(8)

(9)

步驟3時間更新：

(10)

步驟4量測更新：

(11)

2 系統狀態方程和量測方程

2.1 系統狀態方程

假設系統做勻加速運動，目標的初始位置為(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為采樣時間)：

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

(12)

則目標系統的運動狀態方程為：

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

(13)

式中：Φ為系統狀態轉移矩陣，w(k)為系統狀態噪聲。

2.2 系統量測方程

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

圖1 量測模型

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

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

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

(14)

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

(15)

2.3 改進的UKF(IUKF)

(16)

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

(17)

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

(18)

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

(19)

測量噪聲的協方差矩陣為：

(20)

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

(21)

(22)

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

(23)

3 仿真結果及分析

為了對比驗證IUKF與EKF、UKF兩種算法的性能，進行100次Monte Carlo數值仿真實驗，比較其濾波結果的均方根誤差(RMSE)。仿真基于以下的假設：聲吶所在的位置為坐標原點，假設系統目標在水下的深度恒為35 m，在這個平面內做勻加速運動，目標開始位置為(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，狀態噪聲wk和測量噪聲vk二者互不相關，其協方差Qk和Rk分別如下：

(24)

(25)

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

圖2 目標運動軌跡

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

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

表1 三種方法狀態估計的RMSE比較

4 結 語

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