基于擴(kuò)散卡爾曼濾波算法的目標(biāo)跟蹤估計

程 華 張雪婷 房一泉

(華東理工大學(xué)信息科學(xué)與工程學(xué)院 上海 200237)

0 引 言

以往關(guān)于目標(biāo)跟蹤的研究大多專注于采用集中式算法，這些集中式算法常常運(yùn)行在靜態(tài)多傳感器平臺上[1-3]，而集中式卡爾曼(Kalman)濾波在傳統(tǒng)的目標(biāo)跟蹤算法中又起到了至關(guān)重要的作用。對目標(biāo)狀態(tài)估計的性能在很大程度上依賴于所采用的協(xié)作跟蹤算法。在集中式解決方案中，全部傳感器節(jié)點將其測量值轉(zhuǎn)發(fā)到一個數(shù)據(jù)融合中心，采用傳統(tǒng)的Kalman濾波來得到最佳狀態(tài)估計，然后又將全局估計值發(fā)送回每個傳感器節(jié)點。顯然，這種策略需要消耗大量的能量和通信資源[4]，而且依賴中心節(jié)點及與之通信的可靠性；為此，人們又提出了分布式協(xié)作跟蹤算法，這種策略之所以成為一種很有吸引力的替代方案，是因為它通常需要較少的能量和通信資源，并且還允許并行處理，所以更具魯棒性。

適用于這種分布式控制問題的算法主要是分散式Kalman濾波算法。文獻(xiàn)[5]基于Gossip的分布式Kalman濾波算法，分別結(jié)合成對Gossip算法和廣播Gossip算法提出了2種具體的基于Gossip的分布式Kalman濾波(Gossip based Distributed Kalman Filter,GDKF)實現(xiàn)算法，即基于成對Gossip的分布式Kalman濾波算法(Pairwise Gossip based Distributed Kalman Filter,PG-DKF)和基于廣播Gossip的分布式Kalman濾波算法(Broadcast Gossip based Distributed KalmanFilter,BG-DKF)，同時給出了采用2種分布式Kalman濾波算法進(jìn)行網(wǎng)絡(luò)目標(biāo)跟蹤的具體實現(xiàn)步驟。文獻(xiàn)[6]采用容積Kalman濾波器來處理分布式攝像機(jī)網(wǎng)絡(luò)中的目標(biāo)跟蹤問題。首先將平方根容積信息濾波器進(jìn)行擴(kuò)展，提出了分布式平方根容積信息濾波器，使其能適應(yīng)大規(guī)模網(wǎng)絡(luò)。另外，針對攝像機(jī)網(wǎng)絡(luò)中由于攝像機(jī)裝置在一個較大的區(qū)域內(nèi)，攝像機(jī)觀測區(qū)域有限，目標(biāo)可能會出現(xiàn)在觀察的盲區(qū)，造成某些攝像機(jī)的測量數(shù)據(jù)無效的問題，提出了平方根容積信息加權(quán)一致性濾波器(Square-Root Cubature Information Weighted Consensus Filter,SCIWCF)對狀態(tài)信息和信息矩陣加權(quán)，減小無效信息在一致性算法的作用，從而提高整體的濾波性能。仿真實驗結(jié)果表明，算法能夠在攝像機(jī)網(wǎng)絡(luò)中對目標(biāo)進(jìn)行有效跟蹤，在估計精度和濾波器穩(wěn)定性等方面要優(yōu)于傳統(tǒng)的信息濾波。文獻(xiàn)[7]基于兩階段Kalman濾波器對多傳感器信息融合問題進(jìn)行了研究。首先在并列集中式融合框架下，利用觀測值擴(kuò)維融合的思想，將各傳感器的觀測信息擴(kuò)維為系統(tǒng)狀態(tài)的增廣觀測矩陣，并構(gòu)建出對應(yīng)的增廣觀測方程。然后基于單傳感器的兩階段Kalman濾波對系統(tǒng)狀態(tài)和系統(tǒng)偏差進(jìn)行聯(lián)合估計。最后依據(jù)并列分布式融合框架，在分析各傳感器局部兩階段Kalman濾波結(jié)果的基礎(chǔ)上，利用系統(tǒng)狀態(tài)估計誤差協(xié)方差陣構(gòu)建對應(yīng)的加權(quán)融合系數(shù)，進(jìn)而實現(xiàn)系統(tǒng)狀態(tài)的全局融合估計。仿真驗證了并列分布式兩階段Kalman融合濾波方法性能的優(yōu)越性。文獻(xiàn)[4]也對此問題進(jìn)行了研究，但假設(shè)網(wǎng)絡(luò)是完全連通的。文獻(xiàn)[8-9]的研究中采用了同樣的假設(shè)，但考慮了嚴(yán)格的量化通信情況。文獻(xiàn)[10]提出了Kalman濾波迭代并行運(yùn)行在一組傳感器上。然而，該算法主要針對多處理器系統(tǒng)，而且仍然需要一個數(shù)據(jù)融合中心來合并估計值。文獻(xiàn)[11]的研究采用了簡化的狀態(tài)空間模型，并提出了一種基于平均協(xié)商一致的聚合過程。

文獻(xiàn)[12]針對Kalman濾波和RLS濾波之間的聯(lián)系進(jìn)行了研究。文獻(xiàn)[13]把關(guān)于自適應(yīng)傳感器網(wǎng)絡(luò)上的RLS研究擴(kuò)展到了Kalman濾波領(lǐng)域。文獻(xiàn)[14]針對在一般情況下的分布式目標(biāo)跟蹤問題，提出了一種基于協(xié)商一致性的Kalman濾波算法，算法需要若干平均迭代來獲得估計值，但仍然要求網(wǎng)絡(luò)是全連通的。

本文研究了一個節(jié)點網(wǎng)絡(luò)(如傳感器網(wǎng)絡(luò))上的分布式目標(biāo)跟蹤問題，提出了擴(kuò)散Kalman濾波算法，目標(biāo)是為每個節(jié)點估計系統(tǒng)的狀態(tài)。首先對每個測量值和每個節(jié)點，采用來自于鄰近區(qū)域的數(shù)據(jù)計算出一個局部狀態(tài)估計值；然后，采用擴(kuò)散步驟得到每個節(jié)點鄰近區(qū)域估計值的一個局部平均。算法中節(jié)點僅與它們的鄰居通信，不需要數(shù)據(jù)融合中心。仿真實驗結(jié)果表明，本文算法在狀態(tài)估計均方偏差性能和平均能耗方面都有很大的改進(jìn)。

1 分布式Kalman濾波算法原理

1.1 Kalman濾波算法

考慮一個狀態(tài)空間模型的形式為：

(1)

式中：xi∈CM和yi∈CP分別表示系統(tǒng)在時刻i的狀態(tài)向量和測量向量；Fi、Gi、Hi為大小合適的系數(shù)矩陣；信號ni和vi分別表示狀態(tài)和測量噪聲，而且假設(shè)為零均值和白色的，具有協(xié)方差矩陣：

(2)

式中：E{·}表示求數(shù)學(xué)期望；*表示共軛轉(zhuǎn)置；Qi和Ri分別表示時刻i的狀態(tài)和測量矩陣；δij為克羅內(nèi)克符號，即當(dāng)i=j時，δij=1，當(dāng)i≠j時，δij=0。假設(shè)初始狀態(tài)x0有零均值、協(xié)方差矩陣Π0，且對于全部i，與ni和vi不相關(guān)。

測量值更新：

(3)

時間更新：

(4)

1.2 分布式Kalman濾波算法

考慮一組有N個節(jié)點在某個區(qū)域上空間分布的情形，設(shè)Nk表示節(jié)點k的閉鄰域(即連接節(jié)點k且包括k自身在內(nèi)的節(jié)點集合)。假設(shè)在時刻i，每個節(jié)點k根據(jù)式(1)的模型收集一個測量值yk,i∈CP如下：

yk,i=Hk,ixi+vk,ik=1,2，…,N

(5)

這一過程如圖1所示。假設(shè)式(1)的模型對應(yīng)于從式(5)收集全部N個測量值如下：

圖1 每個時刻i節(jié)點k收集一個測量值yk,i

(6)

進(jìn)一步假設(shè)測量噪聲vk,i是空間不相關(guān)的，即：

(7)

2 基于擴(kuò)散Kalman濾波算法的分布式目標(biāo)跟蹤

2.1 局部Kalman濾波算法

為了實現(xiàn)擴(kuò)散Kalman濾波的分布式目標(biāo)跟蹤，首先假設(shè)每個節(jié)點都能與它的鄰居共享數(shù)據(jù)，并且僅使用來自于鄰域給定的數(shù)據(jù)來獲得目標(biāo)狀態(tài)的最佳估計，稱這種估計為在節(jié)點k的鄰域的局部Kalman濾波估計，它可以通過運(yùn)行對于每個鄰居的若干個測量值用式(3)更新來計算，其迭代偽代碼如下：

Pk,i←Pk,i|i-1

forl∈Nkrepeat:

(8)

end

Pk,i|i←Pk,i

算法中箭頭“←”表示按序或非并行任務(wù)。本文將式(8)稱為增量步[15]，因為一個最優(yōu)局部估計值是通過逐步將來自鄰域的估計值和數(shù)據(jù)按順序合并而得到的。局部Kalman濾波迭代算法僅計算每個鄰域的最優(yōu)估計值，而不考慮鄰域的相互連接。而本文把一個擴(kuò)散步驟增加到這個過程中，使得信息通過整個網(wǎng)絡(luò)傳播來顯著地提高算法性能。

2.2 擴(kuò)散Kalman濾波

在擴(kuò)散算法實現(xiàn)中，節(jié)點以各向同性的方式與鄰居進(jìn)行通信和合作以獲得比不合作情況下更好的估計值。對于自適應(yīng)濾波的擴(kuò)散算法如LMS和RLS已得到廣泛研究[13,16]。本文遵循同樣的思想來得到一種擴(kuò)散Kalman濾波(Diffusion Kalman Filtering,Diff-KF)算法。

擴(kuò)散KF算法及其變體需要定義一個擴(kuò)散矩陣C∈RN×N，它具有以下性質(zhì)：

(9)

式中：1是一個具有單位元素的N(1的列向量；cl,k是矩陣C的第(l,k)個元素。稱C為擴(kuò)散矩陣，是因為它控制擴(kuò)散過程，并在網(wǎng)絡(luò)的穩(wěn)態(tài)性能中起著重要的作用。C中的元素表示權(quán)值，是擴(kuò)散算法用于組合附近估計值的。此外，定義一個連接矩陣L如下：

(10)

根據(jù)擴(kuò)散RLS算法[11-12]的并行機(jī)制，并結(jié)合局部Kalman濾波算法原理，通過在Kalman濾波更新后增加一個擴(kuò)散步驟，就可以得到擴(kuò)散Kalman濾波算法。這個擴(kuò)散步驟就是采用鄰域估計值的一個凸組合，即：

(11)

式中：ψl,i表示鄰域估計矩陣。擴(kuò)散步驟是通過局部節(jié)點交互通信來嘗試獲得全局KF性能的?？梢宰C明，式(11)的組合是最小二乘最優(yōu)的[11]。算法1為Diff-KF算法原理實現(xiàn)的偽代碼。

算法1擴(kuò)散Kalman濾波(時間和測量值更新)

Step1增量更新：

Pk,i←Pk,i|i-1

對于每個相鄰節(jié)點l∈Nk，重復(fù)：

end

Step2擴(kuò)散更新：

Pk,i|i←Pk,i

算法1要求在每個時刻i節(jié)點與它們的鄰居節(jié)點的測量矩陣Hk,i、協(xié)方差矩陣Rk,i、增量更新測量值yk,i，以及它們的擴(kuò)散更新先前估計值ψk,i通信。對于每個節(jié)點和每個測量值而言，總的通信需求是PM+M+P2/2+3P/2個標(biāo)量，而且對于每個增量更新來說，它需要對一個矩陣求逆。

圖2 擴(kuò)散Kalman濾波算法的更新

盡管記號Pk,i|i和Pk,i|i-1已被保留，但這兩個矩陣不再代表估計誤差的協(xié)方差，因為擴(kuò)散更新在這兩個矩陣的遞歸式中不被考慮。

算法2擴(kuò)散Kalman濾波的信息形式

Step1增量更新：

Step2擴(kuò)散更新：

算法2中每個節(jié)點的每個測量值所需的總通信為M2/2+3M/2+P個標(biāo)量，而且對于每個增量更新，它需要求2個矩陣的逆。算法1和算法2在數(shù)學(xué)上是等價的，會得到相同的估計結(jié)果。

算法2的增量更新類似于文獻(xiàn)[11]中提出的更新方式，主要區(qū)別是擴(kuò)散步驟。文獻(xiàn)[11]采用基于協(xié)商一致性的方法求平均：

(12)

式中：ε為加權(quán)因子。與此相反，本文采用如式(11)的鄰居的估計值的凸組合。這種差異使得本文算法的性能得到了顯著提高。在文獻(xiàn)[11]的基于平均協(xié)商一致性方法中，網(wǎng)絡(luò)必須在達(dá)到協(xié)商一致之前執(zhí)行重復(fù)的平均步驟。當(dāng)這樣的方法用于分布式估計時，節(jié)點更新它們的局部估計值(采用Kalman濾波或最小二乘法更新)，然后用若干迭代運(yùn)行一個一致性步驟來融合估計值。在數(shù)據(jù)被融合后，采用一個新的測量值，估計值再次被更新，等等。因此，在協(xié)商一致性的估計算法中，有2個時間指標(biāo)：一個是收集測量值；另一個是運(yùn)行平均協(xié)商一致性。而在本文提出的Diff-KF算法中，操作是采用一個時間指標(biāo)實時進(jìn)行的，這樣可得到更好的適應(yīng)性和跟蹤能力，從而提高算法的整體性能。

3 Diff-KF算法性能分析

本節(jié)分析Diff-KF算法(算法1)的平均性能和均方性能。對于每個節(jié)點，給出其均方偏差(Mean-Square Deviation,MSD)的表達(dá)式，即對于節(jié)點k為：

(13)

MSD采用時間i和節(jié)點k作為指標(biāo)，是因為對于擴(kuò)散算法而言，通常不同的節(jié)點會得到不同的估計值，同時，由于模型的動態(tài)性可能會隨時間變化，故MSD又是時間i的函數(shù)。

(14)

(15)

將式(14)和式(15)結(jié)合到算法2的擴(kuò)散步驟中，得到：

(16)

對式(16)兩邊求數(shù)學(xué)期望，可以得到Diff-KF算法的估計期望值的遞推式如下：

(17)

Hi=diag{H1,i,H2,i,…,HN,i}

Pi|i=diag{P1,i|i,P2,i|i,…,PN,i|i}

Pi|i-1=diag{P1,i|i-1,P2,i|i-1,…,PN,i|i-1}

同時還考慮擴(kuò)展矩陣(根據(jù)式(9)和式(10))：

(18)

式中：?表示克羅內(nèi)克積；IM為M維的單位矩陣。

現(xiàn)在可以用一個全局形式來表示式(16)，這個全局形式捕捉整個網(wǎng)絡(luò)的演進(jìn)：

(19)

式(19)也可等價為：

(20)

(21)

式中：

(22)

式(22)利用了克羅內(nèi)克積(A?B)(C?D)=(AC)?(BD)的性質(zhì)。

為了分析均方穩(wěn)態(tài)性能，提出以下假設(shè)：

假設(shè)1模型式(1)中的矩陣是時不變的，即矩陣F、G、H、R和Q不依賴于時間i，而且F是穩(wěn)定的，即它的全部特征值都在單位圓內(nèi)。

在這些假設(shè)下，矩陣Ai、Bi和Di也在穩(wěn)態(tài)下收斂，且它們的穩(wěn)態(tài)值如下：

(23)

假設(shè)1和假設(shè)2足以保證Diff-KF算法收斂，即式(23)中的矩陣A是穩(wěn)定的，且式(22)收斂于李亞普諾夫方程的唯一解：

(24)

DRD*)

(25)

于是節(jié)點k的MSD現(xiàn)在可以表示為：

(26)

式中：Tr(·)表示求矩陣的跡；Ik是一個NM(NM的分塊矩陣，每個塊的大小為M×M，在塊(k,k)位置為單位矩陣，其他地方為零。最后，整個網(wǎng)絡(luò)上的平均MSD為：

(27)

引理1在假設(shè)1和假設(shè)2下，Diff-KF算法(算法1)是無偏且收斂的，而且每個節(jié)點的穩(wěn)態(tài)均方偏差由式(26)給出。

4 算法仿真

為了評價和比較算法性能，將本文提出的Diff-KF算法與文獻(xiàn)[11]的基于協(xié)商一致的算法、2.1節(jié)的局部Kalman濾波算法、文獻(xiàn)[1]的集中式Kalman濾波算法的性能仿真結(jié)果進(jìn)行比較。仿真中傳感器網(wǎng)絡(luò)嘗試跟蹤一個移動目標(biāo)的位置，系統(tǒng)的狀態(tài)就是目標(biāo)的未知位置，即第一個元素和第二個元素分別是x和y坐標(biāo)的一個二維向量。式(1)中的狀態(tài)空間模型矩陣為：

圖3 網(wǎng)絡(luò)拓?fù)溥B接示意圖

(28)

式中：|Nk|是節(jié)點k的閉鄰域的基(即包括它自身在內(nèi)的鄰居的數(shù)目)；選擇參數(shù)αk以使1×C=1*。仿真在NS2中進(jìn)行。

圖4為4種不同算法對于整個網(wǎng)絡(luò)仿真得到的平均MSD(即式(18)給出的MSDave)隨時間i(離散時刻)的變化關(guān)系。本文提出的Diff-KF算法在平均MSD性能方面幾乎接近文獻(xiàn)[1]的集中式Kalman濾波算法，分別優(yōu)于文獻(xiàn)[11]的基于協(xié)商一致的算法和2.1節(jié)的局部Kalman濾波算法約8.2和11.3 dB。最差的是2.1節(jié)的局部的Kalman濾波算法，因為這種算法沒有擴(kuò)散過程，每個節(jié)點盡管可以根據(jù)式(8)訪問其鄰居的數(shù)據(jù)，但每個節(jié)點都只能采用來自其鄰域的數(shù)據(jù)運(yùn)行傳統(tǒng)的卡爾曼濾波。

圖4 不同算法對整個網(wǎng)絡(luò)的平均MSD隨時間i的變化關(guān)系

圖5為4種算法仿真得到的穩(wěn)態(tài)MSD隨不同網(wǎng)絡(luò)節(jié)點數(shù)的變化關(guān)系?？梢钥吹剑疚奶岢龅腄iff-KF算法優(yōu)于其余的3種分布式解決方案，僅次于集中式解決方案。這說明本文提出的Diff-KF算法是魯棒和可擴(kuò)展的。

圖5 不同算法的穩(wěn)態(tài)MSD隨不同網(wǎng)絡(luò)節(jié)點數(shù)的變化關(guān)系

圖6為4種算法對于整個網(wǎng)絡(luò)仿真得到的平均能耗隨時間i(離散時刻)的變化關(guān)系以及平均能耗隨不同網(wǎng)絡(luò)節(jié)點數(shù)的變化關(guān)系。本文提出的Diff-KF算法盡管隨著時間和節(jié)點數(shù)目的增加，能耗也會隨之增加，但不如文獻(xiàn)[11]的基于協(xié)商一致的算法和文獻(xiàn)[1]的集中式Kalman濾波算法增加明顯，因為后者或者需要全連通的網(wǎng)絡(luò)，或需要數(shù)據(jù)融合中心。而局部Kalman濾波算法能耗增加最小，因為該算法中的每個節(jié)點僅與它的鄰居共享數(shù)據(jù)，并且僅使用來自于鄰域給定的數(shù)據(jù)來獲得目標(biāo)狀態(tài)的最佳估計，而無須網(wǎng)絡(luò)全連通和在整個網(wǎng)絡(luò)上廣播數(shù)據(jù)。

(a) 平均能耗隨時間的變化關(guān)系

5 結(jié) 語

本文提出一種用于線性系統(tǒng)的分布式狀態(tài)估計的擴(kuò)散卡爾曼濾波策略。算法要求每個節(jié)點與它的鄰居通信：首先共享數(shù)據(jù)，其次共享估計值。擴(kuò)散過程確保信息在整個網(wǎng)絡(luò)中傳播。給出了算法的穩(wěn)態(tài)均值和穩(wěn)態(tài)均方分析，并通過仿真結(jié)果表明，本文提出的Diff-KF算法性能優(yōu)于其他的局部式和分布式解決方案，可與最佳集中式解決方案相媲美。未來將把本文算法擴(kuò)展到運(yùn)動目標(biāo)和攝像機(jī)網(wǎng)絡(luò)的目標(biāo)跟蹤。