帶非線性約束的自適應(yīng)高斯和卡爾曼濾波目標(biāo)跟蹤算法

(江南大學(xué) 物聯(lián)網(wǎng)工程學(xué)院, 江蘇 無(wú)錫 214122)

0 引言

無(wú)線傳感網(wǎng)絡(luò)中運(yùn)動(dòng)目標(biāo)狀態(tài)估計(jì)與跟蹤已廣泛運(yùn)用到諸多領(lǐng)域,例如無(wú)人飛行器的邊界監(jiān)視[1]、多移動(dòng)機(jī)器人目標(biāo)跟蹤[2]以及太空監(jiān)測(cè)[3]等等。貝葉斯框架是研究這些動(dòng)態(tài)系統(tǒng)常用的方法,從貝葉斯理論得到的狀態(tài)后驗(yàn)密度中可以得到目標(biāo)當(dāng)前時(shí)刻狀態(tài)的完整統(tǒng)計(jì)[4]。

卡爾曼濾波常用于基于狀態(tài)過(guò)程和測(cè)量模型來(lái)估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)。如果運(yùn)動(dòng)目標(biāo)的動(dòng)態(tài)狀態(tài)空間模型(dynamic state-space model, DSSM)與加性高斯噪聲是線性的并且噪聲統(tǒng)計(jì)特性是服從高斯分布的,那么對(duì)于這種特殊情況,卡爾曼濾波器能夠在最小均方誤差(minimum mean square error, MMSE),最大似然(maximum likelihood, ML)和最大后驗(yàn)概率(maximum a posteriori, MAP)意義下產(chǎn)生最優(yōu)解[4-5]。

然而,隨著實(shí)際情況中越來(lái)越多的復(fù)雜要求,經(jīng)典卡爾曼濾波估計(jì)會(huì)遇到以下兩個(gè)難點(diǎn):首先,動(dòng)態(tài)系統(tǒng)的實(shí)際過(guò)程和測(cè)量模型通常是非線性的,其次,過(guò)程噪聲和測(cè)量噪聲經(jīng)常是是非高斯的[6]。因此卡爾曼濾波器可能會(huì)無(wú)法得到有效的狀態(tài)估計(jì)。

高斯和濾波器(gaussian sum filter, GSF)以及粒子濾波器(Particle Filter ,PF)常被用于解決非線性、非高斯系統(tǒng)的狀態(tài)估計(jì)問(wèn)題。GSF使用高斯分布的加權(quán)和來(lái)逼近狀態(tài)概率密度函數(shù),同時(shí)可以近似任意分布[6]。但是傳統(tǒng)高斯和濾波器存在嚴(yán)重的局限性,即每次迭代時(shí)用于近似密度函數(shù)的高斯項(xiàng)的數(shù)量都會(huì)增加。經(jīng)過(guò)許多學(xué)者的研究擴(kuò)展,提出了一種改進(jìn)的估計(jì)算法。其本質(zhì)是將唯一描述每個(gè)高斯項(xiàng)的參數(shù)向量加上某種限制規(guī)則。一種方案是使高斯項(xiàng)的每次迭代都從有限組已知值中隨機(jī)選擇[6-8];另一種則是對(duì)高斯子項(xiàng)的數(shù)量上限設(shè)置一個(gè)閾值。因此濾波器每次迭代的高斯項(xiàng)的數(shù)量將被固定,從而避免了項(xiàng)數(shù)增長(zhǎng)的累積問(wèn)題[9-12]。

當(dāng)一個(gè)運(yùn)動(dòng)系統(tǒng)的約束無(wú)法被引入濾波器的結(jié)構(gòu)之中時(shí),該約束信息常常會(huì)被忽視或做近似處理[13]。譬如在WSN中要實(shí)現(xiàn)地面目標(biāo)跟蹤,目標(biāo)的運(yùn)行軌道大多數(shù)情況下是已知的。但雷達(dá)的觀測(cè)軌跡由于測(cè)量中的噪聲和雜波而顯示出失真情況。如果不使用這些關(guān)于狀態(tài)約束的附加信息,那么用卡爾曼濾波器得到的估計(jì),也僅僅是因?yàn)槭褂昧艘唤M不完全的信息而得到的偽最優(yōu)估計(jì)。

文獻(xiàn)[13]對(duì)目前的約束卡爾曼濾波做了很好的歸納總結(jié)。文獻(xiàn)[14]提出了一個(gè)嚴(yán)格的分析方法,將等式約束納入卡爾曼濾波過(guò)程。首先是獲得無(wú)約束卡爾曼濾波解,然后將無(wú)約束狀態(tài)估計(jì)投影到約束曲面上。Teixeira和Torres等人提出的算法是將非線性等式約束并入無(wú)跡卡爾曼濾波(unscented kalman filter, UKF)過(guò)程,來(lái)解決非線性系統(tǒng)的狀態(tài)估計(jì)問(wèn)題。其中包括PUKF、ECUKF和MAUKF3種算法[15-16]。

為了解決傳統(tǒng)約束卡爾曼濾波方法對(duì)非線性、非高斯系統(tǒng)狀態(tài)估計(jì)精度低等問(wèn)題,本文提出一種帶非線性等式約束的自適應(yīng)高斯和卡爾

曼濾波算法,可用于WSN中非高斯噪聲背景下的運(yùn)動(dòng)目標(biāo)跟蹤。通過(guò)引入自適應(yīng)高斯和濾波策略,解決了傳統(tǒng)高斯和濾波器中的項(xiàng)數(shù)增長(zhǎng)問(wèn)題,提高了跟蹤精度與穩(wěn)定性。

1 相關(guān)工作

1.1 系統(tǒng)模型

假設(shè)無(wú)線傳感網(wǎng)絡(luò)中的某個(gè)運(yùn)動(dòng)目標(biāo)的系統(tǒng)狀態(tài)和觀測(cè)模型滿足下式:

xk+1=f(xk,wk)

(1)

zk=h(xk,vk)

(2)

其中,xk∈x為系統(tǒng)在k時(shí)刻的狀態(tài)矢量,zk∈z為k時(shí)刻的觀測(cè)矢量,wk和vk分別為非高斯的過(guò)程噪聲和測(cè)量噪聲,f(·)和h(·)分別為已知的狀態(tài)函數(shù)和觀測(cè)函數(shù)。

濾波的本質(zhì)即基于每個(gè)k時(shí)刻的量測(cè)Zk={z1,...zk}來(lái)得到狀態(tài)矢量xk的后驗(yàn)概率密度p(xk|Zk),則基于遞歸貝葉斯理論的狀態(tài)估計(jì)步驟如下:

a)時(shí)間更新:

(3)

b)量測(cè)更新:

(4)

1.2 系統(tǒng)非線性約束

假設(shè)對(duì)于某個(gè)運(yùn)動(dòng)目標(biāo)在k≥1的任意時(shí)刻,其狀態(tài)矢量xk均滿足如下非線性約束:

g(xk,k-1)=Bk

(5)

其中：g(·)與Bk∈x均為已知。那么在考慮系統(tǒng)約束的條件下將式(4)作為整個(gè)狀態(tài)估計(jì)的目標(biāo)函數(shù):

J(xk)=p(xk|Zk),s.t.g(xk,k-1)=Bk

(6)

1.3 非高斯噪聲模擬

假設(shè)ζ是具有非高斯密度函數(shù)的隨機(jī)噪聲過(guò)程,其可以被建模為兩個(gè)統(tǒng)計(jì)獨(dú)立的隨機(jī)噪聲過(guò)程之和:

ζ=α+β

(7)

(8)

‖po-p*‖k=

(9)

2 帶非線性約束的自適應(yīng)高斯和卡爾曼濾波算法

2.1 自適應(yīng)高斯和濾波策略

考慮如下離散系統(tǒng):

xk+1=F(xk)+wk

(10)

zk=H(xk)+vk

(11)

其中：xk是狀態(tài)向量,wk是具有非高斯統(tǒng)計(jì)特性的過(guò)程噪聲,zk是測(cè)量向量,vk是具有非高斯統(tǒng)計(jì)特性的測(cè)量噪聲,Φ和H是各自轉(zhuǎn)移函數(shù)。由1.3節(jié)可知,假設(shè)wk=θk+αk,vk=λk+βk,那么wk和vk可分別表示為高斯和的形式:

(12)

(13)

本文提出的帶非線性約束的自適應(yīng)高斯和卡爾曼濾波目標(biāo)跟蹤算法分為自適應(yīng)高斯和濾波策略及非線性約束投影策略。而高斯和自適應(yīng)濾波策略又主要分為兩個(gè)部分：第一部分是分布式高斯和濾波;第二部分是權(quán)值自適應(yīng)策略。約束投影策略將在2.2節(jié)介紹。

1)分布式高斯和濾波:

(14)

其中,Zk是當(dāng)前時(shí)刻測(cè)量序列{z1,...,zk}。將兩個(gè)偏差矢量θk和λk代入p(xk|Zk),引入貝葉斯規(guī)則,其可改寫(xiě)為:

(15)

代入式(14)可得:

(16)

(1) 狀態(tài)預(yù)測(cè):

(17)

(18)

(19)

(2) 狀態(tài)估計(jì):

(20)

(21)

(22)

其中:

因此,式(16)改寫(xiě)為:

(23)

式(23)的協(xié)方差Pk滿足公式：

(24)

2)權(quán)值自適應(yīng)策略:

對(duì)于式(23)的加權(quán)項(xiàng)p(θk,λk|Zk)。引入Zk={z1,…,zk}序列及貝葉斯規(guī)則:

p(θi,λi|Zk)=

(25)

式(25)的分子第一項(xiàng)可以近似為高斯密度,表示為:

(26)

式(25)的分子第二項(xiàng)是給定過(guò)去測(cè)量序列Zk中θk=i,λk=j的預(yù)測(cè)概率值。可由下式得到:

(27)

(28)

(29)

(30)

(31)

自適應(yīng)高斯和濾波的本質(zhì)是每個(gè)單獨(dú)高斯項(xiàng)的估計(jì)乘以相應(yīng)的權(quán)值。

2.2 狀態(tài)約束投影策略

考慮引入目標(biāo)約束先驗(yàn)知識(shí)。文獻(xiàn)[14]通過(guò)引入拉格朗日乘子λ來(lái)求約束卡爾曼濾波的最優(yōu)解。但此方法要求迭代的初始值λ0靠近λ的最優(yōu)解。但實(shí)際條件下,好的初始值并不容易獲得。這里考慮將非線性約束引入自適應(yīng)高斯和濾波,進(jìn)行投影量測(cè)更新,來(lái)獲得更精確的估計(jì)?？紤]2.1的自適應(yīng)高斯和濾波,那么經(jīng)過(guò)約束投影后的估計(jì)式表達(dá)為:

(32)

subjecttog(xk)-Bk=0

(33)

首先,重新選擇k時(shí)刻的Sigma點(diǎn),并進(jìn)行非線性傳播:

(34)

(35)

k時(shí)刻約束估計(jì):

(36)

k時(shí)刻約束誤差協(xié)方差及估計(jì)約束互協(xié)方差:

(37)

(38)

非線性約束卡爾曼增益:

(39)

計(jì)算k時(shí)刻的協(xié)方差及對(duì)權(quán)值自適應(yīng)濾波器的輸出進(jìn)行約束投影修正:

(40)

(41)

3 系仿真實(shí)驗(yàn)

xk=Fxk-1+wk

(42)

yk=Hxk+vk

(43)

Rv=diag[25 25],P0=diag[49 1 49 1]

在目標(biāo)初始位置處于真實(shí)位置條件下,圖1給出了某一次蒙特卡洛仿真結(jié)果。圖中分別顯示了兩個(gè)濾波器的輸出,模擬的路段,以及小車真實(shí)位置和測(cè)量。真實(shí)位置用一系列繪制在路段上的(°)點(diǎn)表示。因?yàn)榧僭O(shè)的小車具有恒定角速度,因此真實(shí)位置是等距分布在路段上的。將小車的運(yùn)動(dòng)軌跡看作約束并引入濾波器:

(44)

同時(shí)將文獻(xiàn)[14]提出的一種非線性約束濾波器納入比較。從圖中濾波器的輸出可以看出,由于約束信息的引入,文獻(xiàn)[14]提出的算法能夠跟蹤小車的轉(zhuǎn)彎運(yùn)動(dòng)。但是因?yàn)橘ゑR噪聲的影響,其跟蹤精度大大降低,無(wú)法滿足目前跟蹤需求。因此傳統(tǒng)約束卡爾曼濾波在非高斯噪聲下難以達(dá)到令人滿意的效果。而本文提出的自適應(yīng)高斯和濾波與卡爾曼濾波結(jié)合后的濾波器輸出基本都落在真實(shí)位置附近,性能明顯優(yōu)于文獻(xiàn)[14]提出的濾波算法。

圖1 情景1下各算法的目標(biāo)跟蹤結(jié)果

(45)

圖2 θk的權(quán)重值

同時(shí)式(44)應(yīng)滿足以下兩個(gè)條件:

2)當(dāng)某一個(gè)權(quán)重Λi≥1,令其等于1,同時(shí)令其它權(quán)重等于0。

接下來(lái)在實(shí)驗(yàn)中考慮一種更加復(fù)雜的情景2,同時(shí)將PUKF和GSUKF兩種算法納入比較。采用500次蒙特卡洛仿真,以各算法的均方根誤差作為目標(biāo)跟蹤精度的指標(biāo)。計(jì)算方法如下:

(46)

為了驗(yàn)證算法在系統(tǒng)非線性條件下的性能,考慮加入更多傳感器來(lái)實(shí)時(shí)跟蹤小車的位置。假設(shè)有3個(gè)雷達(dá)站每個(gè)時(shí)刻k發(fā)送信號(hào)來(lái)獲得3個(gè)距離,假設(shè)雷達(dá)位置分別為R1=(0 m,0 m),R2=(0 m,650 m),R3=(320 m,0 m)。由于R1處于原點(diǎn),考慮在k時(shí)刻獲得兩個(gè)距離差,即每個(gè)時(shí)刻目標(biāo)距R2與R3距R1的距離差si→1,i=2,3,則測(cè)量方程改寫(xiě)為:

yk=sk+vk

(47)

其中:sk=[si→1]i=2,3其它條件與情景1相同。3種算法的均方根誤差如圖所示。

由圖3可以看出, 傳統(tǒng)的高斯和卡爾曼濾波算法由于沒(méi)有約束先驗(yàn)信息的導(dǎo)入,其收斂速度及精度都低于估計(jì)量投影法以及本文提出的算法。 因此,相對(duì)于傳統(tǒng)的GSUKF,本文算法采用了高斯子項(xiàng)的權(quán)值自適應(yīng)策略,并將系統(tǒng)非線性約束引入濾波器,從而能夠得到更加準(zhǔn)確的全局優(yōu)化結(jié)果。 估計(jì)量投影法與本文算法類似,均能夠在迭代初期快速收斂,但是由于無(wú)法消除伽馬噪聲的影響,其跟蹤精度隨著時(shí)間累積會(huì)大大降低。因此與估計(jì)量投影法相比,自適應(yīng)高斯和卡爾曼濾波能加適合實(shí)際工程的使用。

情景3中將考慮更加實(shí)際的情況,即在初始時(shí)刻,目標(biāo)的真實(shí)位置未知。量測(cè)噪聲vk=βk(令λk=0)的協(xié)方差矩陣為R=diag([25 25])。算法首先使用2.1節(jié)的自適應(yīng)高斯和濾波策略目標(biāo)初始估計(jì),來(lái)代入濾波器輸出初值。初始協(xié)方差P0=diag([1027 1027])。其它情況與情景2實(shí)驗(yàn)條件相同,采用500次蒙特卡洛仿真實(shí)驗(yàn),各算法的均方根誤差如圖4所示。

圖4 情景3下各算法跟蹤精度比較

由圖4可以看出,由于各算法均引入約束信息先驗(yàn),所以即使在初始狀態(tài)非目標(biāo)真值的情況下, 投影無(wú)跡卡爾曼濾波算法、估計(jì)量投影法以及本文提出的自適應(yīng)高斯和卡爾曼濾波算法算法均能夠快速收斂。但是與另外兩種算法相比,本文算法的跟蹤精度更高,說(shuō)明非高斯噪聲對(duì)另外兩種濾波器產(chǎn)生了極大干擾。另外,對(duì)比圖4和圖3,當(dāng)在量測(cè)方程引入非線性時(shí),3種算法的誤差曲線均會(huì)產(chǎn)生波動(dòng),使得所有誤差曲線更加曲折。其中投影無(wú)跡卡爾曼濾波算法受到的影響最嚴(yán)重,表明其處理非線性問(wèn)題能力較估計(jì)量投影法和本文算法更差。估計(jì)量投影法算法受非線性影響較小,但其累計(jì)跟蹤誤差會(huì)逐漸增大,對(duì)實(shí)際跟蹤方案會(huì)產(chǎn)生較大影響。而本文算法在非線性條件下依然擁有較高的跟蹤精度。因此,本文提出的算法在初始條件失真以及非線性非高斯條件下相對(duì)于目前約束卡爾曼濾波算法均具有更高的跟蹤精度。

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

在本文中,針對(duì)無(wú)線傳感網(wǎng)絡(luò)中非高斯噪聲下運(yùn)動(dòng)目標(biāo)的跟蹤精度問(wèn)題,我們提出一種基于自適應(yīng)高斯和的約束卡爾曼濾波目標(biāo)跟蹤算法。它首先計(jì)算目標(biāo)狀態(tài)的各初始高斯子項(xiàng),然后對(duì)每個(gè)高斯項(xiàng)進(jìn)行UKF濾波得到無(wú)約束條件下的狀態(tài)估計(jì),同時(shí)加入一種高斯子項(xiàng)權(quán)值自適應(yīng)策略,解決了傳統(tǒng)高斯和濾波算法的子項(xiàng)累積問(wèn)題，提高了算法實(shí)時(shí)性。引入運(yùn)動(dòng)目標(biāo)的非線性狀態(tài)約束,將其看作一種無(wú)約束投影問(wèn)題以獲得全局最優(yōu)解,提高了算法收斂速度與精度。最后通過(guò)仿真實(shí)驗(yàn)測(cè)試算法性能。仿真結(jié)果表明,相對(duì)于傳統(tǒng)的約束卡爾曼濾波以及GSUKF等算法,本文提出的跟蹤算法具有更高的跟蹤精度。