張 凱

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

0 引 言

水下目標的測量噪聲很高，需要使用濾波器消除這種噪聲，常見的線性系統(tǒng)濾波器有維納濾波器和卡爾曼濾波器?？柭鼮V波器是一種應用非常廣泛的線性濾波器，能對過程噪聲和測量噪聲進行最優(yōu)濾波。但是其前提條件是已知噪聲的協(xié)方差，過程噪聲和測量噪聲滿足白噪聲或者近似為白噪聲，且要求兩者獨立互不相關(guān)。但現(xiàn)實世界中系統(tǒng)基本都是非線性的，擴展卡爾曼濾波(Extended Kalman Filter，EKF)和無跡卡爾曼濾波(Unscented Kalman Filter，UKF)是常見的兩種非線性濾波系統(tǒng)。EKF是針對非線性系統(tǒng)應用非常普遍的狀態(tài)估計方法，但是其依賴對非線性系統(tǒng)的線性化來傳播狀態(tài)的均值和協(xié)方差，依賴于局部非線性強度，當系統(tǒng)的線性化比較嚴重時，其估計并不準確。而且在線性化時需要推導復雜的雅可比矩陣，需要計算并保留非線性函數(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)的目標跟蹤。

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

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

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

1 濾波算法

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

1.1 擴展卡爾曼濾波

在實際工程應用中，非線性系統(tǒng)的情況較多，所以常常不能滿足應用卡爾曼濾波的前提條件，如果應用卡爾曼濾波，容易使濾波發(fā)散和精度下降。擴展卡爾曼濾波是比其他非線性濾波較為簡便且應用廣泛的一種方法，用簡單的算法即可獲得較好的結(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é)方差的線性化變換近似等同于真實的非線性變換，但僅僅是在較簡單的二維情況下近似成立。當系統(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)做勻加速運動，目標的初始位置為(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)

則目標系統(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)所在的位置為坐標原點，對目標M進行跟蹤，可以得到目標M和聲吶之間的距離及角度。模型如圖1所示。

圖1 量測模型

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

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

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

(14)

由式(14)可以知道該模型的狀態(tài)方程是線性的，量測方程是非線性的。在對系統(tǒ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è)：聲吶所在的位置為坐標原點，假設(shè)系統(tǒng)目標在水下的深度恒為35 m，在這個平面內(nèi)做勻加速運動，目標開始位置為(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)

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

圖2 目標運動軌跡

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

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

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

4 結(jié) 語

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