多傳感器集中式增量卡爾曼濾波融合算法

馬麗麗，張 曼，陳金廣

MA Lili,ZHANG Man,CHEN Jinguang

西安工程大學(xué) 計(jì)算機(jī)科學(xué)學(xué)院，西安 710048

School of Computer Science,Xi’an Polytechnic University,Xi’an 710048,China

1 引言

狀態(tài)估計(jì)被廣泛應(yīng)用于航空、航天、航海、工業(yè)過程控制、目標(biāo)跟蹤等各個(gè)領(lǐng)域。不同的估計(jì)準(zhǔn)則會(huì)產(chǎn)生不同的估計(jì)算法。其中最具代表性的是卡爾曼濾波算法，該算法采用遞推形式，數(shù)據(jù)存儲(chǔ)量小，適用范圍廣，具備其他濾波形式所不具備的優(yōu)點(diǎn)。該濾波理論一經(jīng)提出便得到了廣泛應(yīng)用。然而，在實(shí)際應(yīng)用過程中，由于周圍環(huán)境的影響、測(cè)量設(shè)備自身造成的誤差、模型和參數(shù)選取不當(dāng)?shù)仍颍瑫?huì)產(chǎn)生量測(cè)系統(tǒng)誤差，而使用傳統(tǒng)卡爾曼濾波算法難以消除該類系統(tǒng)誤差。

近年來，傅惠民等人針對(duì)該問題，提出了一系列增量濾波算法。在文獻(xiàn)[1]中，針對(duì)傳統(tǒng)卡爾曼濾波算法無法消除量測(cè)系統(tǒng)誤差的情況，提出了增量卡爾曼濾波算法，成功地消除了量測(cè)系統(tǒng)誤差，提高了濾波精度。隨后，為了將該思路擴(kuò)展到非線性系統(tǒng)，文獻(xiàn)[2]提出了擴(kuò)展增量卡爾曼濾波算法，文獻(xiàn)[3]提出了增量粒子濾波算法。然而，以上算法僅適用于單傳感器濾波系統(tǒng)，而實(shí)際中的許多估計(jì)問題（例如軍事指揮和工業(yè)控制等方面）都涉及多個(gè)傳感器。多傳感器數(shù)據(jù)融合算法通常分為集中式、分布式和混合式三類，已經(jīng)有許多可供借鑒的成熟算法[4]。人們對(duì)于多傳感器融合算法進(jìn)行了很多研究[5-8]，其中在序貫濾波[9-14]和擴(kuò)維濾波[15-16]方面的應(yīng)用研究也取得了成果。

考慮到傳統(tǒng)增量卡爾曼濾波算法不能解決多傳感器情況的估計(jì)問題，本文將傅惠民等人提出的增量卡爾曼濾波算法分別與傳統(tǒng)擴(kuò)維融合算法和傳統(tǒng)序貫融合算法相結(jié)合，提出增量卡爾曼濾波的擴(kuò)維融合算法和增量卡爾曼濾波的序貫融合算法。該算法將單傳感器增量卡爾曼濾波算法擴(kuò)展到多傳感器中，可以充分利用多個(gè)數(shù)據(jù)源的信息，能夠增強(qiáng)濾波的可靠性和系統(tǒng)的生存能力，克服了傳統(tǒng)增量卡爾曼濾波算法僅適用于單傳感器的情況，同時(shí)采用增量的濾波算法還能消除傳統(tǒng)擴(kuò)維融合算法和傳統(tǒng)序貫融合算法中量測(cè)系統(tǒng)誤差造成的影響，提高濾波精度。

2 問題描述

在多個(gè)傳感器的目標(biāo)跟蹤系統(tǒng)中，目標(biāo)運(yùn)動(dòng)的狀態(tài)方程一般表示為：

其中，xk∈?n表示目標(biāo)在k時(shí)刻的狀態(tài)向量，Φk∈?n×n表示系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，Γk∈?n×r表示系統(tǒng)噪聲分布矩陣，wk∈?r表示系統(tǒng)噪聲向量。假設(shè)wk服從高斯分布，其均值為0，方差為Qk。目標(biāo)初始狀態(tài)為 x0，且cov[wk，wj]=Qkδkj，cov[x0，wk]=0 。

假設(shè)有N個(gè)傳感器對(duì)同一目標(biāo)進(jìn)行量測(cè)，則相應(yīng)的量測(cè)方程可以表示為：

在式（2）的基礎(chǔ)上，構(gòu)造多傳感器增量量測(cè)方程為：

3 擴(kuò)維增量卡爾曼濾波

在擴(kuò)維融合（也稱并行濾波）算法中[4]，通常令：

那么在融合中心，對(duì)所有傳感器量測(cè)的量測(cè)方程可以統(tǒng)一表示為：

對(duì)所有傳感器的增量量測(cè)方程可以寫為：

根據(jù)式（2）的假設(shè)條件，可得：

則式（1）和式（10）分別為擴(kuò)維增量卡爾曼濾波的狀態(tài)方程和增量量測(cè)方程。假設(shè)k-1時(shí)刻的狀態(tài)估計(jì)值為相應(yīng)的協(xié)方差為Pk-1|k-1，則k時(shí)刻的狀態(tài)預(yù)測(cè)方程為：

相應(yīng)的協(xié)方差矩陣為：

根據(jù)卡爾曼濾波方程，狀態(tài)估計(jì)方程為：

相應(yīng)的協(xié)方差矩陣為：

其中：

注意到在式（16）中包含矩陣求逆運(yùn)算，由于該類運(yùn)算的時(shí)間復(fù)雜度隨著矩陣維數(shù)的增長(zhǎng)而呈指數(shù)增長(zhǎng)，因此，當(dāng)融合的傳感器數(shù)目過多時(shí)，時(shí)間復(fù)雜度會(huì)急劇增大。此外，多個(gè)傳感器融合之后的誤差性能與數(shù)據(jù)的冗余性和互補(bǔ)性密切相關(guān)。如文獻(xiàn)[5]所述，若多傳感器的量測(cè)數(shù)據(jù)之間為負(fù)相關(guān)時(shí)，則融合后的誤差性能較好；若多傳感器量測(cè)數(shù)據(jù)之間為正相關(guān)時(shí)，則融合后的誤差性能退化。

4 序貫增量卡爾曼濾波

序貫濾波是指當(dāng)各個(gè)傳感器在同一時(shí)刻的量測(cè)噪聲不相關(guān)時(shí)，可以依據(jù)量測(cè)噪聲方差的大小，由小到大對(duì)傳感器進(jìn)行編號(hào)，在融合中心按照傳感器的序號(hào)分別獲取各傳感器的量測(cè)值，依次對(duì)當(dāng)前狀態(tài)進(jìn)行更新。

對(duì)于式（1）和式（3）所描述的多傳感器增量濾波方程，融合中心按照傳感器1→N的順序?qū)δ繕?biāo)的運(yùn)動(dòng)狀態(tài)估計(jì)值進(jìn)行序貫更新。更新過程為：假設(shè)第i(1≤i≤N)個(gè)傳感器在k-1時(shí)刻的狀態(tài)估計(jì)值為相應(yīng)的協(xié)方差為那么根據(jù)系統(tǒng)狀態(tài)轉(zhuǎn)移方程，由式（12）和式（13）可以獲得k時(shí)刻的狀態(tài)預(yù)測(cè)值及其協(xié)方差矩陣當(dāng)處理中心接收到傳感器1的量測(cè)值后，由式（3）可以計(jì)算傳感器1的增量量測(cè)值，利用對(duì)狀態(tài)預(yù)測(cè)值及其協(xié)方差進(jìn)行更新，更新公式為：

假設(shè)利用第i個(gè)傳感器量測(cè)值進(jìn)行更新之后的狀態(tài)估計(jì)值及其協(xié)方差分別為，則當(dāng)處理中心接收到第i(1＜i≤N)個(gè)傳感器的量測(cè)值后，狀態(tài)更新方程為：

按照上述更新規(guī)則，依次對(duì)k時(shí)刻的N個(gè)量測(cè)值進(jìn)行更新，則最終融合中心對(duì)系統(tǒng)狀態(tài)的序貫濾波估計(jì)為，相應(yīng)的協(xié)方差為。

5 實(shí)驗(yàn)?zāi)Ｐ图敖Y(jié)果分析

本文用線性和非線性兩個(gè)例子來驗(yàn)證多傳感器增量卡爾曼濾波融合算法的有效性。

實(shí)驗(yàn)1假設(shè)一維線性離散系統(tǒng)為[1]：

其中，i表示傳感器序號(hào)，實(shí)驗(yàn)中采用3個(gè)傳感器。Wk和分別是k時(shí)刻的系統(tǒng)噪聲和量測(cè)噪聲，且均為獨(dú)立的高斯白噪聲。Wk的均值q=0，方差Q=0.1，的均值為0，方差R1=R2=R3=1，它們均為已知量。是量測(cè)系統(tǒng)誤差，為未知量，方便起見，假設(shè)。需要指出的是，該例中的。

分別采用傳統(tǒng)擴(kuò)維卡爾曼濾波、擴(kuò)維增量卡爾曼濾波、傳統(tǒng)序貫卡爾曼濾波和序貫增量卡爾曼濾波四種算法進(jìn)行融合。設(shè)初始狀態(tài) X0=10 ，相應(yīng)的協(xié)方差為P0=0.1。圖1是利用四種融合算法獲得的狀態(tài)估計(jì)結(jié)果。由圖可見，傳統(tǒng)擴(kuò)維卡爾曼濾波和傳統(tǒng)序貫卡爾曼濾波的結(jié)果相對(duì)于真實(shí)狀態(tài)都有較大的偏移，未能消除量測(cè)系統(tǒng)誤差。而擴(kuò)維增量卡爾曼濾波和序貫增量卡爾曼濾波的結(jié)果基本與真實(shí)狀態(tài)吻合得很好。圖1表明，擴(kuò)維增量卡爾曼濾波和序貫增量卡爾曼濾波能夠有效消除量測(cè)系統(tǒng)誤差，提高融合精度。圖2是四種融合算法得到的狀態(tài)估計(jì)結(jié)果的誤差。由圖可見，擴(kuò)維增量卡爾曼濾波和序貫增量卡爾曼濾波的波動(dòng)很小，誤差表現(xiàn)較為平穩(wěn)，基本在0附近。然而，傳統(tǒng)擴(kuò)維卡爾曼濾波和傳統(tǒng)序貫卡爾曼濾波與真值的誤差值一直很大。相比較而言，在多傳感器情況下，擴(kuò)維增量卡爾曼濾波和序貫增量卡爾曼濾波能夠獲得更高的狀態(tài)估計(jì)精度，能夠消除未知系統(tǒng)誤差對(duì)其造成的影響。

圖1 實(shí)驗(yàn)1中不同融合算法的狀態(tài)估計(jì)

圖2 實(shí)驗(yàn)1中不同融合算法的狀態(tài)估計(jì)誤差

為了說明幾種融合方法的運(yùn)算復(fù)雜度，表1列出了四種方法各運(yùn)行100次的平均運(yùn)行時(shí)間。由表1可知，時(shí)間復(fù)雜度由高到低依次為：序貫增量融合、標(biāo)準(zhǔn)序貫融合、擴(kuò)維增量融合和標(biāo)準(zhǔn)擴(kuò)維融合。考慮到濾波算法中有求逆運(yùn)算，因此隨著系統(tǒng)模型維數(shù)的增加，擴(kuò)維融合和序貫融合算法之間的時(shí)間復(fù)雜度關(guān)系會(huì)發(fā)生變化。

為了研究算法誤差對(duì)過程噪聲的敏感性，設(shè)定噪聲方差分別為0.1，0.2，0.5，1.0，1.1，1.2，采用 Monte Carlo方法重復(fù)執(zhí)行100次，計(jì)算相應(yīng)的均方根誤差如表2所示。由表2可知，隨著過程噪聲方差的增大，四種方法求得的誤差也隨之增大。

表1 不同方法的運(yùn)行時(shí)間

表2 不同過程噪聲方差下的均方根誤差

實(shí)驗(yàn)2假設(shè)一維非線性離散系統(tǒng)為[2]：

其中，i表示傳感器序號(hào)，實(shí)驗(yàn)中采用3個(gè)傳感器。Wk和分別是k時(shí)刻的系統(tǒng)噪聲和量測(cè)噪聲，且均為獨(dú)立的高斯白噪聲。Wk的均值q=0，方差Q=0.1，的均值為0，方差R1=R2=R3=1，它們均為已知量。是量測(cè)系統(tǒng)誤差，為未知量，方便起見，假設(shè)。

由于該模型的系統(tǒng)狀態(tài)方程是線性方程，因此只需將系統(tǒng)的非線性增量量測(cè)方程采用一階泰勒級(jí)數(shù)展開的辦法進(jìn)行線性化，則其中的Hk和Hk-1分別為：

分別采用傳統(tǒng)擴(kuò)維EKF、擴(kuò)維增量EKF、傳統(tǒng)序貫EKF和序貫增量EKF四種算法進(jìn)行融合。設(shè)初始狀態(tài)X0=10 ，相應(yīng)的協(xié)方差為P0=0.1。圖3是利用四種融合算法獲得的狀態(tài)估計(jì)結(jié)果。由圖可見，傳統(tǒng)擴(kuò)維EKF和傳統(tǒng)序貫EKF的結(jié)果相對(duì)于真實(shí)狀態(tài)都有一定量的偏移，未能消除系統(tǒng)誤差，而擴(kuò)維增量EKF和序貫增量EKF的濾波結(jié)果能夠與狀態(tài)真值較好的吻合，偏差很小，消除了系統(tǒng)誤差。圖4是四種融合算法得到的狀態(tài)估計(jì)結(jié)果的誤差。由圖可見，擴(kuò)維增量EKF和序貫增量EKF的濾波結(jié)果誤差較小，基本在0附近波動(dòng)。然而，傳統(tǒng)擴(kuò)維卡爾曼濾波和傳統(tǒng)序貫卡爾曼濾波與真值的誤差值一直很大。相比較而言，在多傳感器情況下，擴(kuò)維增量卡爾曼濾波和序貫增量卡爾曼濾波能夠獲得更高的狀態(tài)估計(jì)精度，能夠消除未知系統(tǒng)誤差對(duì)其造成的影響。

為了考察幾種融合算法的時(shí)間復(fù)雜度，表3列出了四種方法各運(yùn)行100次的平均運(yùn)行時(shí)間，由表3可知，時(shí)間復(fù)雜度由高到低依次為：序貫增量融合、標(biāo)準(zhǔn)序貫融合、擴(kuò)維增量融合和標(biāo)準(zhǔn)擴(kuò)維融合。這與實(shí)驗(yàn)1的結(jié)果相似。

圖3 實(shí)驗(yàn)2中不同融合算法的狀態(tài)估計(jì)

圖4 實(shí)驗(yàn)2中不同融合算法的狀態(tài)估計(jì)誤差

表3 不同方法的運(yùn)行時(shí)間

為了研究過程噪聲方差對(duì)算法誤差的敏感性，設(shè)定噪聲方差分別為 0.1，0.2，0.5，1.0，1.1，1.2，采用 Monte Carlo方法重復(fù)執(zhí)行100次，計(jì)算相應(yīng)的均方根誤差如表4所示。由表4可知，隨著過程噪聲方差的增大，四種方法求得的誤差也隨之增大。這與實(shí)驗(yàn)1的結(jié)果相似。

表4 不同過程噪聲方差下的均方根誤差

6 結(jié)論

本文將增量卡爾曼濾波分別與集中式融合算法中的并行濾波和序貫濾波相結(jié)合，提出多傳感器增量卡爾曼濾波融合算法，即：擴(kuò)維增量卡爾曼濾波和序貫增量卡爾曼濾波。該算法將單傳感器的增量卡爾曼濾波擴(kuò)展到具有多傳感器的增量卡爾曼濾波，與單傳感器比較，多傳感器能夠充分利用多個(gè)傳感器的資源，有利于時(shí)間和空間上信息的互補(bǔ)。例如當(dāng)某個(gè)傳感器不能利用或某個(gè)目標(biāo)不在該傳感器的覆蓋范圍內(nèi)時(shí)，則可由其他傳感器提供信息，增強(qiáng)了系統(tǒng)的健壯性和可靠性。由于采用增量融合的方法，因此本文算法保留了單傳感器增量卡爾曼濾波的優(yōu)點(diǎn)，即能夠成功消除系統(tǒng)誤差，提高濾波精度。本文僅給出了一維情況下的例子，對(duì)于狀態(tài)向量為多維的情況，還需要在今后的工作中結(jié)合實(shí)際應(yīng)用做進(jìn)一步的實(shí)驗(yàn)驗(yàn)證。

[1]傅惠民，吳云章，婁泰山.欠觀測(cè)條件下的增量Kalman濾波方法[J].機(jī)械強(qiáng)度，2012，34（1）：43-47.

[2]傅惠民，婁泰山，吳云章.欠觀測(cè)條件下的擴(kuò)展增量Kalman濾波方法[J].航空動(dòng)力學(xué)報(bào)，2012，27（4）：777-781.

[3]傅惠民，婁泰山，吳云章.增量粒子濾波方法[J].航空動(dòng)力學(xué)報(bào)，2013，28（6）：1201-1207.

[4]韓崇昭，朱洪艷，段戰(zhàn)勝.多源信息融合[M].2版.北京：清華大學(xué)出版社，2010.

[5]陳金廣，馬麗麗，陳亮.多傳感器量測(cè)噪聲對(duì)航跡融合性能影響分析[J].火力與指揮控制，2010，35（7）：123-125.

[6]朱安福，景占榮.多傳感器信息融合的目標(biāo)跟蹤研究[J].計(jì)算機(jī)工程與應(yīng)用，2009，45（25）：7-9.

[7]葛泉波，李文斌，孫若愚，等.基于EKF的集中式融合估計(jì)研究[J].自動(dòng)化學(xué)報(bào)，2013，39（6）：816-825.

[8]王永安，何光進(jìn)，劉毅.基于卡爾曼濾波的多傳感器跟蹤融合算法[J].船電技術(shù)，2013，33（2）：4-7.

[9]孟凡彬，郝燕玲，張崇猛，等.基于無跡粒子PHD濾波的序貫融合算法[J].系統(tǒng)工程與電子技術(shù)，2011，33（1）：30-34.

[10]章飛，孫睿.多傳感器序貫勢(shì)分布概率假設(shè)密度濾波[J].江蘇科技大學(xué)學(xué)報(bào)：自然科學(xué)版，2012，26（6）：587-592.

[11]郭路，康鵬.序貫濾波在雙站無源定位中的應(yīng)用研究[J].現(xiàn)代防御技術(shù)，2008，36（4）：116-119.

[12]王鵬，宋鵬云，張繼業(yè).標(biāo)量及對(duì)角陣加權(quán)的序貫估計(jì)融合算法[J].計(jì)算機(jī)工程與應(yīng)用，2012，48（32）：101-104.

[13]劉華，黃勝昔.基于序貫無跡卡爾曼濾波的雷達(dá)目標(biāo)跟蹤方法[J].計(jì)算機(jī)工程與應(yīng)用，2009，45（25）：202-204.

[14]黃樹峰，秦超英.序貫處理的多傳感器航跡融合算法研究[J].計(jì)算機(jī)工程與應(yīng)用，2010，46（16）：42-45.

[15]曹曉軍.異類傳感器集中式擴(kuò)維融合無跡卡爾曼濾波融合算法[J].計(jì)算機(jī)應(yīng)用與軟件，2012，29（8）：150-152.

[16]郭路，康鵬.基于信息融合和擴(kuò)維Kalman濾波技術(shù)的系統(tǒng)誤差估計(jì)算法研究[J].現(xiàn)代防御技術(shù)，2008，36（3）：74-77.