自適應(yīng)衰減記憶UKF 算法在三維水下目標(biāo)跟蹤中的應(yīng)用

王滿林

(海裝重慶局，重慶 400023)

聲納實(shí)時(shí)量測(cè)跟蹤水下目標(biāo)是非線性濾波應(yīng)用領(lǐng)域中一個(gè)高度復(fù)雜的問(wèn)題，這主要是因?yàn)闇y(cè)量方程為非線性的。純方位被動(dòng)跟蹤技術(shù)是目標(biāo)跟蹤的重要研究領(lǐng)域之一，其所要解決的是如何利用被動(dòng)聲納觀測(cè)到的目標(biāo)方位信息來(lái)估計(jì)目標(biāo)的運(yùn)動(dòng)參數(shù)，如距離、航速、航向等參數(shù)。

解決非線性濾波問(wèn)題最為普遍的方法是擴(kuò)展卡爾曼濾波器。但是該方法只適用于弱非線性的系統(tǒng)，對(duì)于強(qiáng)非線性系統(tǒng)，很容易導(dǎo)致發(fā)散［1-2］。EKF 算法則是KF 算法的非線性擴(kuò)展，是一種遞推算法，由于在每個(gè)采樣時(shí)刻對(duì)估計(jì)狀態(tài)矢量的線性化導(dǎo)致協(xié)方差矩陣產(chǎn)生畸變，容易使EKF 發(fā)散?？柭鼮V波是增長(zhǎng)記憶濾波，一般情況下它給出了一個(gè)隨著觀察資料的積累精度不斷提高的估計(jì)。估計(jì)誤差協(xié)方差的范數(shù)隨著時(shí)間的增長(zhǎng)不斷減小，因此新的觀察資料對(duì)修正狀態(tài)估計(jì)的作用逐漸減弱。但是，實(shí)際上系統(tǒng)動(dòng)態(tài)模型的變化規(guī)律是很難在事前完全知道的。這樣，一方面這些系統(tǒng)的參數(shù)在變化，另一方面，在遞推過(guò)程中還是把這些已經(jīng)變化了的參數(shù)當(dāng)作精確值來(lái)進(jìn)行運(yùn)算。于是，計(jì)算的誤差協(xié)方差陣的范數(shù)隨著時(shí)間的增長(zhǎng)不斷減小，而實(shí)際誤差協(xié)方差卻不斷增大，最后導(dǎo)致濾波器發(fā)散。由于新近的觀察資料含有變化了的系統(tǒng)動(dòng)態(tài)模型的較多信息，在濾波器中須增強(qiáng)這些新觀察資料對(duì)修正狀態(tài)估計(jì)誤差的作用，降低老的觀察資料的地位。有限記憶和衰減記憶濾波器就是基于這一考慮而提出的防止濾波器發(fā)散的方法［3-4］。

最近，研究人員提出一種新的用于解決非線性濾波問(wèn)題的濾波器，他們將這種濾波器稱為Unscented 卡爾曼濾波器(UKF)［5-6］。其實(shí)現(xiàn)原理為:在原先狀態(tài)分布中按某一規(guī)則取一些點(diǎn)，使這些點(diǎn)的均值和協(xié)方差等于原狀態(tài)分布的均值和協(xié)方差;將這些點(diǎn)代入非線性函數(shù)中，相應(yīng)得到非線性函數(shù)值點(diǎn)集，通過(guò)這些點(diǎn)求取變換后的均值和協(xié)方差。由于這樣得到的函數(shù)值沒(méi)有經(jīng)過(guò)線性化、沒(méi)有忽略其高階項(xiàng)，因而其均值和協(xié)方差的估計(jì)比EKF 方法要精確。

UKF 雖然能夠克服EKF 存在的一系列問(wèn)題，改善了系統(tǒng)線性化誤差，但并沒(méi)有明顯改善卡爾曼濾波器容易發(fā)散的問(wèn)題，在實(shí)際應(yīng)用時(shí)，UKF 對(duì)濾波初始值也比較敏感，系統(tǒng)噪聲相關(guān)信息的不確定性以及狀態(tài)模型擾動(dòng)等都會(huì)影響UKF的濾波精度。針對(duì)UKF 存在的問(wèn)題，本文提出了一種自適應(yīng)衰減記憶UKF 算法(adaptive fading memory UKF，AFMUKF)。

1 問(wèn)題描述

本文考慮迪卡爾坐標(biāo)系下三維雙基陣純方位問(wèn)題。假定目標(biāo)是作勻速直線運(yùn)動(dòng)的，用X(t)=［x(t)，y(t)，z(t)，vx，vy，vz］T表示目標(biāo)的狀態(tài)，X0i(t)=［x0i(t)，y0i(t)，z0i(t)，v0x，v0y，v0z］T(i=1，2)表示基陣1 與基陣2 的運(yùn)動(dòng)狀態(tài)。則系統(tǒng)狀態(tài)方程為

其中

測(cè)量方程為

其中βmi(k)為雙基陣的2 個(gè)聲納在第k 個(gè)采樣時(shí)刻(采樣周期為T)所提供的觀測(cè)方位，

其中βi表示真值方位，

αm1(k)為基陣1 對(duì)目標(biāo)觀測(cè)的俯仰角，

其中:

雙基陣純方位問(wèn)題就是，根據(jù)測(cè)量得到的方位序列來(lái)估計(jì)目標(biāo)的運(yùn)動(dòng)要素。

2 UKF 濾波算法

2.1 UT 變換

Unscented 卡爾曼濾波器(UKF)也是一種遞歸式貝葉斯估計(jì)方法，它利用Unscented 變換(unscented transformation，UT)方法，用一組確定的取樣點(diǎn)來(lái)近似后驗(yàn)概率。但是UKF不必線性化非線性狀態(tài)方程和觀測(cè)方程，它直接利用非線性狀態(tài)方程來(lái)估算狀態(tài)向量的概率密度函數(shù)。

Unscented 變換是計(jì)算進(jìn)行非線性傳遞的隨機(jī)向量概率的一種方法，它是基于這樣一種考慮:近似一種概率分布比近似一種任意的非線性方程或者非線性變換要容易的多［5，6］。設(shè)x 是nx維的隨機(jī)向量，g:Rnx→Rnz是一非線性函數(shù)z=g(x)，考慮將x 通過(guò)非線性函數(shù)g 傳遞，假定x 的均值和協(xié)方差分別為和Px。為了計(jì)算關(guān)于z 的統(tǒng)計(jì)量，首先選擇2nx+1 個(gè)帶有權(quán)值的樣本點(diǎn)(也稱SIGMA 點(diǎn))Si={Wi，χi}，，使其能夠完全獲取隨機(jī)變量x 的真實(shí)的均值和協(xié)方差。SIGMA 點(diǎn)的選擇以及權(quán)值的確定是根據(jù)以下方程:

其中λ=α2(nx+κ)-nx是一個(gè)尺度調(diào)節(jié)因子，α 決定了選擇的SIGMA 點(diǎn)在其均值ˉx 附近的分布情況，通常將α 設(shè)置為一個(gè)很小的正值(如0.001)。κ 是次級(jí)尺度調(diào)節(jié)因子，通常設(shè)置為0，β 是用來(lái)結(jié)合關(guān)于x 的分布的先驗(yàn)知識(shí)(對(duì)于高斯分布，β 的最佳取值為2是矩陣(nx+λ)Px平方根的第i 行，Wi表示第i 個(gè)SIGMA 點(diǎn)的權(quán)值，且滿足∑Wi=1。是用來(lái)計(jì)算均值的權(quán)值是用來(lái)計(jì)算協(xié)方差的權(quán)值。

將每個(gè)SIGMA 點(diǎn)通過(guò)非線性函數(shù)向前傳遞，

通過(guò)計(jì)算可以得到z 的均值和協(xié)方差的估計(jì)，

2.2 UKF 算法原理及步驟

根據(jù)以上的U 變換，可得純方位條件下的UKF 算法，假設(shè)k-1 時(shí)刻的狀態(tài)及相應(yīng)協(xié)方差的估計(jì)分別為和其計(jì)算步驟:

1)利用初始狀態(tài)估計(jì)，設(shè)定最初的2nx+1 個(gè)SIGMA 點(diǎn)

2)利用過(guò)程模型變換這些SIGMA 點(diǎn)

3)計(jì)算預(yù)測(cè)估計(jì)值

4)計(jì)算預(yù)測(cè)協(xié)方差

5)通過(guò)測(cè)量方程計(jì)算測(cè)量值

6)計(jì)算預(yù)測(cè)測(cè)量值

7)計(jì)算新息方差

8)計(jì)算協(xié)方差P~xk~zk

9)計(jì)算Kalman 增益

10)更新誤差協(xié)方差

11)更新狀態(tài)

3 AFMUKF 算法

采用衰減記憶濾波器，在UKF 的基礎(chǔ)上引入衰減因子，構(gòu)成衰減記憶UKF 濾波器(FMUKF)，采用Tarn 和Zaborsky提出的一種簡(jiǎn)單實(shí)用的引入衰減因子的方法［7］。FMUKF 的遞推關(guān)系和UKF 的不同之處在于，在式(19)中加入了衰減因子S，使式(19)變?yōu)?/p>

將UKF 計(jì)算步驟中的式(19)替換為式(27)即可得到衰減記憶UKF 算法［8］。但在仿真中我們發(fā)現(xiàn)，衰減因子的選取會(huì)對(duì)濾波結(jié)果產(chǎn)生很大影響，引入衰減因子后的濾波將使估值的方差增大，且衰減記憶濾波的精度隨S 的增加而降低，在一些情況下可能會(huì)發(fā)生異常，甚至導(dǎo)致濾波發(fā)散。為此，在加入衰減因子的基礎(chǔ)上，再引入一個(gè)自適應(yīng)因子，將式(22)、(23)和(25)變?yōu)?/p>

式中αk為自適應(yīng)因子，0 ＜αk≤1。

合理的自適應(yīng)因子能夠自適應(yīng)地平衡狀態(tài)方程與觀測(cè)信息的權(quán)比，還能夠控制狀態(tài)模型擾動(dòng)對(duì)濾波解的影響。αk構(gòu)造為［9-10］:

其中Vk為預(yù)測(cè)殘差，

由上述分析可知，當(dāng)初值選取不合理或模型存在異常擾動(dòng)時(shí)，αk將小于1，即預(yù)測(cè)信息對(duì)濾波解的貢獻(xiàn)將盡量小;當(dāng)預(yù)測(cè)信息明顯異常時(shí)，tr(VkVTk)將非常大，αk則將接近于0，即預(yù)測(cè)信息被完全棄用，從而有效控制了模型誤差對(duì)濾波解的影響。

4 仿真實(shí)驗(yàn)

傳統(tǒng)的跟蹤濾波算法擴(kuò)展卡爾曼濾波器解算三維空間運(yùn)動(dòng)目標(biāo)存在一個(gè)明顯的缺陷是Jacobian 矩陣推導(dǎo)的復(fù)雜性。與EKF 中采用Jacobian 矩陣進(jìn)行線性化不同，UKF 通過(guò)一組仔細(xì)選擇確定的采樣點(diǎn)來(lái)捕獲系統(tǒng)的相關(guān)統(tǒng)計(jì)量，將非線性映射直接作用于各采樣點(diǎn)。因此，本文對(duì)三維水下運(yùn)動(dòng)目標(biāo)采用UKF 和AFMUKF 兩種算法進(jìn)行數(shù)學(xué)仿真。

假設(shè)一水下運(yùn)動(dòng)目標(biāo)開始在水下-200 m 處作勻速直線運(yùn)動(dòng)，100 s 后持續(xù)向上爬升，200s 后停止爬升，繼續(xù)作勻速直線運(yùn)動(dòng)。仿真初始條件為:采樣間隔T=1 s，方位角、俯仰角觀測(cè)誤差方差均為1°，兩基陣位置分別為(-1000，0)、(1000，0)，目標(biāo)初始狀態(tài)為［- 2 000，4 000，- 200，20，-20，0］T，濾波器初始化為

x(0 | 0)= ［-4 000，7 000，-250，10，10，10］T

P(0 | 0)= diag(10 000，10 000，10 000，10，10，10)

對(duì)于AFMUKF 算法，衰減因子S=1.02。對(duì)2 種算法分別進(jìn)行30 次Monte-Carlo 仿真實(shí)驗(yàn)，仿真結(jié)果如圖1 ～6所示。

從仿真結(jié)果可以看出，在仿真的初始時(shí)刻，濾波器都有較大的初始偏差，這是由于初始估值造成的。隨著濾波的進(jìn)行，AFMUKF 算法能很快地收斂到真值位置附近，并保持較高的濾波精度。AFMUKF 算法較UKF 算法提高了解算精度及收斂速度，且增強(qiáng)了數(shù)值穩(wěn)定性。究其原因，主要在于AFMUKF 算法加大了對(duì)新測(cè)量值的利用權(quán)重，減少了對(duì)陳舊測(cè)量值和初始估值的依賴，并且具有一定的自適應(yīng)能力，能夠?qū)崟r(shí)檢測(cè)出濾波中出現(xiàn)的異常，有效控制了初始值偏差及模型異常擾動(dòng)誤差對(duì)濾波解的影響。

5 結(jié)束語(yǔ)

由于UKF 能有效處理系統(tǒng)方程非線性程度大的情況，所以在處理被動(dòng)目標(biāo)跟蹤等典型的非線性問(wèn)題時(shí)性能較好，然而，UKF 雖然改善了系統(tǒng)線性化誤差，但并沒(méi)有明顯改善卡爾曼濾波器容易發(fā)散的問(wèn)題，為此，本文在UKF 中引入衰減因子及自適應(yīng)因子，對(duì)純方位目標(biāo)跟蹤采用自適應(yīng)衰減記憶UKF 算法。理論分析和仿真結(jié)果均表明，AFMUKF 算法的濾波精度、穩(wěn)定性、收斂時(shí)間等都明顯優(yōu)于UKF 算法。

［1］Aidala V J.Kalman filter behavior in bearings-Only tracking applications［J］. IEEE Transaction on AES，1979，15(1):29-39.

［2］杜選民，姚藍(lán).多基陣聯(lián)合的無(wú)源純方位目標(biāo)運(yùn)動(dòng)分析研究［J］.聲學(xué)學(xué)報(bào)，1999，24(6):604-610.

［3］LU P，ZHAO L，CHEN Z. Memory-attenuated least squre filtering and its application［C］//Proceedings of the 6thWorld Congress on Intelligent Control and Automation.Dalian:［s.n.］，2006:1483-1486.

［4］LEE T S.Theory and application of adaptive fading memory Kalman filters［J］.IEEE Transactions on Circuits and Systems，1988，35(4):474-477.

［5］Julier S J，Uhlmann J K.Unscented filtering and nonlinear estimation［J］.Proceedings of the IEEE，2004，92(3):401-422.

［6］Wan E A，Merwe R.The unscented kalman filter for nonlinear estimation［C］//Proceedings of International Symposium on Adaptive Systems for Signal Processing，Communications and Control.Alberta:［s.n.］，2000，153-158.

［7］TARN T，ZARBOSKY J.A practical non-driving filter［J］.AIAA Journal，1970，8(6):1127-1133.

［8］羅涌，鐘洪聲.一種改進(jìn)的UKF 算法在目標(biāo)跟蹤系統(tǒng)建模中的應(yīng)用［J］.遙測(cè)遙控，2008，29 (1):55-58.

［9］YANG Y X，GAO W G.An optimal adaptive Kalman filter［J］.Journal of Geodesy，2006，80(4):177-183.

［10］GAO W G，YANG Y X，CUI X Q.Application of adaptive Kalman filtering algorithm in IMU/GPS integrated navigation system［J］. Geo-Spatial Information Science，2007，10(1):22-26.

［11］張林琳，楊日杰，熊華.非線性濾波方法在水下目標(biāo)跟蹤中的應(yīng)用［J］.火力與指揮控制，2010(8):13-17.