多傳感器非線性系統(tǒng)的魯棒SCI-UKF濾波器

周 晗，孫小君,2,*

(1.黑龍江大學(xué) 電子工程學(xué)院，哈爾濱 150080；2.黑龍江省信息融合估計(jì)與檢測(cè)重點(diǎn)實(shí)驗(yàn)室，哈爾濱 150080)

0 引 言

非線性系統(tǒng)是生活中廣泛應(yīng)用的系統(tǒng)，它應(yīng)用于船舶導(dǎo)航、衛(wèi)星軌道姿態(tài)估計(jì)、車輛違規(guī)測(cè)速或雷達(dá)探測(cè)等系統(tǒng)[1-3]。非線性系統(tǒng)由于微積分的計(jì)算復(fù)雜，往往不能直接求得精確的最優(yōu)解。為此，相關(guān)學(xué)者提出了多種近似的非線性濾波方法[1-3]。文獻(xiàn)[5-6]提出了擴(kuò)展Kalman濾波(EKF),通過(guò)對(duì)非線性系統(tǒng)模型進(jìn)行泰勒展開(kāi)，將其近似轉(zhuǎn)化成線性系統(tǒng)，再套用線性系統(tǒng)的Kalman濾波方法，由于省略了高階項(xiàng)，從而產(chǎn)生模型誤差，嚴(yán)重還會(huì)導(dǎo)致濾波發(fā)散。文獻(xiàn)[7-8]提出了無(wú)跡Kalman濾波(UKF)，不需要對(duì)非線性系統(tǒng)進(jìn)行線性化近似，可避免非線性函數(shù)雅克比矩陣的復(fù)雜運(yùn)算，保證了非線性系統(tǒng)的普遍適應(yīng)性，此外還使得高斯噪聲得到抑制，其精度相當(dāng)于二階泰勒展開(kāi)，但相較于EKF算法其運(yùn)行速度會(huì)略慢一些[9]。

多傳感器信息融合是把多個(gè)相同或不同類型的傳感器或信息源所提供的局部數(shù)據(jù)或信息加以綜合、組合、融合，得到系統(tǒng)狀態(tài)更精確的估計(jì)[10]。近幾十年來(lái)，多傳感器信息融合已經(jīng)受到了廣泛關(guān)注并被應(yīng)用到導(dǎo)航、衛(wèi)星定位、無(wú)人機(jī)、目標(biāo)跟蹤和信號(hào)處理等許多高科技領(lǐng)域中，其中信息融合濾波理論是多傳感器信息融合的一個(gè)重要分支，主要集中于研究多傳感器信息融合Kalman濾波[1-3]。文獻(xiàn)[11]首次提出了協(xié)方差交叉(CI)信息融合方法，它是一種分布式融合算法。其優(yōu)點(diǎn)可以避免互協(xié)方差的計(jì)算，減小計(jì)算負(fù)擔(dān)，可擴(kuò)大信息融合技術(shù)適用范圍[12]。文獻(xiàn)[13]將CI融合技術(shù)運(yùn)用到Wiener濾波器中。文獻(xiàn)[14]給出了一種協(xié)方差交叉融合魯棒Kalman濾波器，并證明CI融合器的精度是接近于按矩陣加權(quán)最優(yōu)融合器的精度。文獻(xiàn)[15] 針對(duì)帶未知不確定估計(jì)誤差方差和互協(xié)方差的多傳感器融合估計(jì)問(wèn)題，在CI融合的基礎(chǔ)上提出許多魯棒融合Kalman濾波器，如改進(jìn)的CI魯棒融合器、批處理協(xié)方差交叉(BCI)融合魯棒估值器、序貫協(xié)方差交叉(SCI)融合魯棒估值器以及并行協(xié)方差(PCI)魯棒估值器。

近年來(lái)，對(duì)于非線性系統(tǒng)研究不斷加深，相關(guān)學(xué)者不斷地將信息融合技術(shù)應(yīng)用到非線性系統(tǒng)中，文獻(xiàn)[16]加權(quán)觀測(cè)融合算法與UKF相結(jié)合提出加權(quán)融合無(wú)跡Kalman濾波器。文獻(xiàn)[17]則是在文獻(xiàn)[16]的基礎(chǔ)上采用Gauss-Hermite逼近方法減小其計(jì)算負(fù)擔(dān)，但因加權(quán)觀測(cè)融合要求準(zhǔn)確的噪聲方差，且計(jì)算量仍舊繁重。文獻(xiàn)[18]將CI融合與粒子Kalman濾波器相結(jié)合，用來(lái)對(duì)非線性系統(tǒng)進(jìn)行濾波。針對(duì)帶不同觀測(cè)陣與未知互協(xié)方差的多傳感器非線性系統(tǒng)，在UKF濾波方法的基礎(chǔ)上，提出SCI融合魯棒無(wú)跡Kalman濾波器，將有效地解決帶未知噪聲統(tǒng)計(jì)的多傳感器非線性系統(tǒng)的狀態(tài)估計(jì)問(wèn)題。

1 問(wèn)題描述

設(shè)離散非線性系統(tǒng)模型的狀態(tài)方程與觀測(cè)方程為

x(k+1)=f(x(k),k)+w(k)

(1)

y(i)(k)=h(i)(k)(x(k),k)+v(i)(k),i=1,…,L

(2)

(3)

問(wèn)題是針對(duì)非線性系統(tǒng)，已知其噪聲保守上界，提出多傳感器非線性系統(tǒng)的SCI融合魯棒無(wú)跡Kalman濾波器。

2 UKF濾波方法實(shí)現(xiàn)

UKF濾波方法的基本原理是將UT(unscented transform)變換與經(jīng)典的Kalman濾波器相結(jié)合，其中Kalman濾波方法為非線性高斯濾波提供了一種次優(yōu)的遞推式實(shí)現(xiàn)方法[19]。

2.1 UT變換原理

UT變換就是用固定數(shù)量的參數(shù)去近似代替一個(gè)高斯分布，因?yàn)樵谶M(jìn)行Kalman濾波時(shí)，每一步的迭代過(guò)程中均需要求出隨機(jī)分布經(jīng)過(guò)非線性變換后的均值和方差，由于近似概率分布要比近似非線性函數(shù)更易獲得，只需在原狀態(tài)分布中通過(guò)Sigma點(diǎn)采樣策略規(guī)則取一些采樣點(diǎn)，使這些采樣點(diǎn)與原狀態(tài)分布具有相同的均值和協(xié)方差。這些采樣點(diǎn)也被稱為Sigma點(diǎn)，對(duì)其的要求是在獲取輸入變量x的分布特征的條件下，使逼近輸出的某些性能指標(biāo)的代價(jià)函數(shù)取最小[20]。

(4)

(5)

利用式(3)和(4)獲得一組Sigma點(diǎn)集及其對(duì)應(yīng)的權(quán)值[8]：

(6)

計(jì)算2n+1個(gè)Sigma點(diǎn)集的一步預(yù)測(cè)，l=1，…，2n+1。

x(l)(k+1|k)=f[x(l)(k|k),k]

(7)

(8)

根據(jù)一步預(yù)測(cè)值，二次使用UT變換產(chǎn)生新的Sigma點(diǎn)集：

(9)

2.2 無(wú)跡Kalman濾波器

將式(9)帶入到觀測(cè)方程中，得到系統(tǒng)的一步預(yù)測(cè)觀測(cè)，此時(shí)的l=1～2n+1。

y(l)(k+1|k)=h[x(l)(k+1|k),k+1]

(10)

通過(guò)加權(quán)求和得到量測(cè)輸出變量的方差陣:

(11)

計(jì)算協(xié)方差:

(12)

計(jì)算增益矩陣:

(13)

從而得到無(wú)跡Kalman濾波器。

引理 1[9]對(duì)于非線性系統(tǒng)(1)和(2)的噪聲滿足(3)的條件下，無(wú)跡Kalman濾波器為

i=1,…,l

(14)

(15)

3 多傳感器非線性系統(tǒng)的魯棒SCI-UKF濾波器

采用的是序貫協(xié)方差交叉(SCI)融合，它等價(jià)于若干個(gè)兩傳感器協(xié)方差交叉(CI)融合器，是一種快速遞推的兩傳感器CI融合器，本質(zhì)上還是一種按矩陣加權(quán)融合器，所以是次優(yōu)的，其濾波后的精度要小于觀測(cè)融合[15]。

(16)

(17)

(18)

其中，定義0≤ω≤1，則有

Ω?ΩP1∩ΩP2

(19)

則對(duì)于任意u∈ΩP1∩ΩP2有

(20)

由于0≤ω≤1，則有

(21)

則引出u∈Ω，故式(19)成立。此時(shí)為

(22)

(23)

則Ω就是PCI的協(xié)方差橢圓，即

(24)

(25)

則有

(26)

(27)

(28)

(29)

(30)

將式(28)帶入式(29)和(30)即可得到CI融合算法。

定理1對(duì)于不確定噪聲方差的非線性系統(tǒng)(1)和(2)在滿足(3)的條件下，得到的CI融合魯棒無(wú)跡Kalman濾波器(CI-UKF)為

(31)

(32)

證明基于無(wú)跡Kalman濾波器式(14)和(15)應(yīng)用CI融合算法式(29)和(30)，便容易得證式(31)與(32)。

trPCI≤trPi,i=1,2

(33)

序貫協(xié)方差交叉融合魯棒無(wú)跡Kalman濾波器(SCI-UKF)主要針對(duì)非線性多傳感器系統(tǒng)，現(xiàn)將局部傳感器1與局部傳感器2通過(guò)CI-UKF得到的結(jié)果再與傳感器3進(jìn)行融合，依次如下，直到第L個(gè)傳感器參與融合，故其融合精度應(yīng)高于任意一個(gè)局部濾波器的魯棒精度。在文獻(xiàn)[21]中還給出了仿真例子，說(shuō)明了SCI融合器的魯棒和其實(shí)際精度關(guān)于融合的次序不是很靈敏，更加便于實(shí)際操作[15]。

SCI-UKF估計(jì)原理可用如下L-1步實(shí)現(xiàn)：

…

(34)

(35)

重復(fù)利用兩傳感器CI-UKF式(31)和(32)，則SCI-UKF可用L-1個(gè)兩傳感器CI-UKF遞推實(shí)現(xiàn)[13]。便可證得如下定理。

定理2對(duì)于不確定噪聲方差的非線性系統(tǒng)(1)和(2)在滿足(3)的條件下，便可得到的SCI融合魯棒無(wú)跡Kalman濾波器為

(36)

(37)

4 仿真模型及結(jié)果分析

考慮文獻(xiàn)[22]中所提具有3個(gè)傳感器的跟蹤系統(tǒng)，其狀態(tài)方程為

(38)

傳感器量測(cè)方程為

y(i)(k)=h(i)(x(k))+v(i)(k),i=1,2,3

(39)

為驗(yàn)證本文理論結(jié)果的正確性，進(jìn)行了N=50次Monte-Carlo仿真實(shí)驗(yàn)，在時(shí)刻k處局部和融合Kalman濾波器的累計(jì)均方誤差(Accumulated mean square error，AMSE)定義為

(40)

仿真結(jié)果如下列各圖所示。局部UKF(Local filter 1～3，LF 1～3)與SCI-UKF濾波效果見(jiàn)圖1～圖4，實(shí)線表示真值，虛線表示估值。LF 1～3與SCI-UKF對(duì)位置追蹤的AMSE曲線見(jiàn)圖5，LF 1～3與SCI-UKF對(duì)速度追蹤的AMSE曲線見(jiàn)圖6。由圖5可見(jiàn)，傳感器2雖然對(duì)位置追蹤效果較好，其濾波精度甚至略高于融合后的濾波精度，這是由于CI融合其本質(zhì)上是只一種按矩陣加權(quán)融合器，屬于一種次優(yōu)濾波。但通過(guò)對(duì)比圖6可見(jiàn)傳感器2在對(duì)速度的追蹤存在較大的偏差。然而SCI-UKF能同時(shí)實(shí)現(xiàn)對(duì)位置與速度的跟蹤，且追蹤性能相較于任一局部UKF更加準(zhǔn)確，且操作簡(jiǎn)便，便于實(shí)時(shí)運(yùn)用。

圖1 局部傳感器1的濾波效果圖Fig.1 Filter effect diagram of the first local sensor

圖2 局部傳感器2的濾波效果圖Fig.2 Filter effect diagram of the second local sensor

圖3 局部傳感器3的濾波效果圖Fig.3 Filter effect diagram of the third local sensor

圖4 SIC融合魯棒UKF濾波效果圖Fig.4 Effect diagram of SCI-UKF

圖5 局部UKF與SCI-UKF對(duì)位置的ASME曲線Fig.5 AMSE curves of local UKF and SCI-UKF location tracking

圖6 局部UKF與SCI-UKF對(duì)速度的ASME曲線Fig.6 AMSE curves of local UKF and SCI-UKF velocity tracking

5 結(jié) 論

對(duì)帶不確定噪聲的多傳感器非線性系統(tǒng)，應(yīng)用SCI融合算法，提出一種基于SCI融合魯棒無(wú)跡Kalman濾波器。融合算法的引入，極大提高了噪聲統(tǒng)計(jì)不確定條件下，多傳感器非線性系統(tǒng)的狀態(tài)估計(jì)的精度。一個(gè)帶不同觀測(cè)陣的3傳感器二維跟蹤系統(tǒng)的實(shí)例也表明了所提出的算法的有效性和可行性。