多傳感器狀態(tài)融合

1、§ 3多傳感器狀態(tài)融合Kalman濾波器和預(yù)報器3. 1問題描述考渥如卜不倉控制變吊的多傳感器線性離散定常隨機系統(tǒng):(3.1.1)(3.1.2)x(k + 1) = Ax(k) + Cnk)A伙)=H$(k) +耳伙)=1,上比中x(k)eRn是隨機狀態(tài)變炭，卩伙)w/T是第i個傳感器對狀態(tài)的隨機觀測信號， 噸)e Rr是輸入白噪聲，匕伙)e Rm'是第i個傳感器的觀測口噪聲，A g R»",Bw R,lxs, CwR”"，HjWR叭是己知矩陣。假設(shè)1：班約、叫伙)(21,，厶)是均值為零方差陣為2、0(心1,厶)的互不相 關(guān)(正交)白噪聲，即E

2、(噸)=0, E(w(i)w(/)r) = Q 8UE(耳伙)=0,E(v()v(/)r) = 2務(wù)E(噸)匕(j)r) = 0 , Eg(g(l)T) = 0(/ 豐 j)1假設(shè)2： *(0)均值為zv 方差陣為乙,并且與w(R)和V,(/:)(/= 1, - ,L)不相關(guān)(正 交)，即E(x(0)=兒,E(x(0) -)(x(0)-弘門=Po耳(*(0)-如w伙丫 = °, E(x(0)-兒)叫伙/= 0(/= 1,-,£)問題是求狀態(tài)x伙)的基第i個傳感器的局部最優(yōu)Kalman濾波器玄伙| k)和預(yù)報器 ±伙+ 1|燈0=1,上),并求在三種加權(quán)融合估計準則

3、F的狀態(tài)融合Kalman濾波器 xQ(kk)和預(yù)報器 x0(k + lk)。(/R) =珅)-£(/1 燈,心 o, 1,，厶，坊(屮)=耳乙(屮応(屮門,ij=o,l，上£(/1燈二 R(/1k) = £if(/|k戊(!k)TH =0,1, -,L3.2局部Kalman濾波器和預(yù)報器首先對第i個傳感器：(3.2.1)0-2.2)x(k + 1)= Ax(k) + Cw 伙)yi(k) = Hix(k)vi(k)利用第四章推論3.2.1馬上得到局部Kahmn濾波器：定理3.2.1(局部Kalman濾波器)系統(tǒng)(321丿和(3.2.2丿在假設(shè)1、2卜的遞推Kali

4、mn濾 波器為(323)k)= KiAk)xk-lk-l)+ Kk)yk)Pi(kk) = l-K,(k)HiP伙伙-1)p& I k) = K“)Pi伙 一 11R - 1)心伙廣 + 心伙)2心伙)丁 + K,伙)0K”r (3.2.4) 并且局部濾波誤差互協(xié)方差陣為(iH j)tP.k | k) = KkjP.伙一 1|R-1)K”(k)T + KiCk)QvKjC(k)7(325)其中心伙)= /-K,伙)HJA , K匯伙)= /-K,伙)HJC ,Q 伙)=H f£k | k-1)町 + Q , K)= Pk | k- 1)H 宓(k)P(kk-1)= AP,(k

5、-k- 1)AT + CQC1并且初值 x(0|0) = /4), P(0| 0)=o證明；只需證明(3.2.4)和(3.2.5).由(321)和(3.2.2)知，y伙)=H$(k) +匕(k)= / (Ar伙一 1)+ Cw伙一 1) +匕伙)，代 入(323)得到，xk k) = KiAxk 一11 R -1) + Kfk )H t Ax k -1) + H tCwk -l) + vk)(3.2.1)減去上式，得到Xj(k | k) = Ax(k -1)+ Cn>k 1)一 KiAxk 一 1| R 1)一 K£k)HiAx<k -1)4- HCm»(R 1

6、)+匕(R) =K認-1| R -1) + Klcwk -1) - Kk )vk)因此由假設(shè)1得到(3.2.4丿和(325)。再利用第四章定理3.3.1馬上得到局部Kalman預(yù)報器：定理322(局部Kalman預(yù)報器)系統(tǒng)(3.2.1)和(3.2.2)在假設(shè)1、2卜的遞推Kalnnn預(yù) 報器為：土伙 + 11 R) = K：伙)士 伙 | R -1) + 心伙”，伙)(3 2.6)£伙 + lk) = Kk)P, (kk-l)Ar + CQvCT或P& +11燈二 K：伙州伙 |k- 1)K：伙)丁 + CQU + Kip(k)Qv Kip(k)T (3.2.7) 并且局部

7、預(yù)報誤差互協(xié)方差陣為(/工j)：匚伙 + 1|*)= V伙比伙 +11 k)K：(ky + CQ.C(328)其中K：伙)=A/-K#)HJ, Kk)= AKk),a,伙)=H I k- 1)H： + 0 , Kg)= pt(kk-l)H,TQ；k)初值 i(l|0)= g + B”iO), P(11 0) = APqAt + CQCr o證明：只需證明(3.2.7)和(3.2.8)。將(322州入(3.2.6)得到，xk + 11 k) = K：'i'k | £ 一 1丿+ AKRk)Htxk)+ vk)(3.2.1)減去上式，得到xk + 11 R) = Axk)

8、+ C”(約一 KiAxk k -1)- AKk)Htxk)+ vk)=K；'壬 ilk | k-1)+ C 呱約一 Kipk)v.k)因此由假設(shè)1得到(3.2.7)和(328)。3.3 狀態(tài)融合Kalman濾波器首先，利用定理3.2.1并根據(jù)第二節(jié)定理2.2.1-2.2.3得到最優(yōu)狀態(tài)融Kalman濾波器。 定理3.3.1多傳感器系統(tǒng)(3.1.1)和(3.1.2)在假設(shè)1、2卜，按業(yè)陣加權(quán)址優(yōu)狀態(tài)融合 Kalimn濾波器為：站伙 | k) = (eTP(k | kyleyleTP(k R尸左伙 | k),廳伙 | k) = (R P伙 | 燈七尸并且有關(guān)系式：trP 伙 k)<

9、trPt 伙 | R)丿=1,，厶q、卑&)plL(kky其屮:,P(k | k)=,x(k | k)=i y厲伙1燈Pjk也玄伙|燈(心1,，厶)和號(R W1,L)由定理3.2.1確定。定理332多傳感器系統(tǒng)(3.1.1)和(3.1.2)在假設(shè)1、2 F,按標吊:加權(quán)最優(yōu)狀態(tài)融介 Kalman濾波器為：疋伙I燈二£4伙)£伙IR)，P：(k秋)二工£<伙)丐伙)號伙| k)i=l/=1 j=l并£L有關(guān)系式：f叱伙伙|燈,心1,L其中“'伙)= (q'伙)a (k) = erpkyie ' e = U 川'

10、;ftrPn(kk)trPlL(kky匕伙|燈= i1，土伙|燈(心1,，厶)和場伙|燈由定理321JrPlA(kk) - trPlM伙 |燈丿確定。定理333多傳感器系統(tǒng)(3.1.1)和(3.1.2)4假設(shè)1、2卜按對角陣加權(quán)最優(yōu)狀態(tài)融合 Kalman濾波器為：亀(kk) =工吋伙此伙| k),局伙| k) = (eT(Pi伙| k)le)' ,j = ,丄并且有關(guān)系式：pkk)<p(kk)J=9-丄誤差平方和為伙| R) = f住伙|燈，并且冇關(guān)系式：冃trP(k | k) <trPXk k),i =其中 夕)=(昭伙)吩伙心,:;鳥;,"(1】)， 叱伙段伙

11、秋)、P7(fcR)= ii ,匕刀伙|約為厶伙|燈 的第血)元素，艸k)P扛k也x伙|燈(/ = 1,，厶)和匚伙|燈由定理321確定。3.4狀態(tài)融合Kalman濾波器同樣，利用定理3.2.2并根據(jù)第二節(jié)定理2.2.1-2.2.3得到最優(yōu)狀態(tài)融Kalman預(yù)報器。 定理3.4.1多傳感器系統(tǒng)(3.1.1)和(3.1.2)在假設(shè)1、2卜，按矩陣加權(quán)最優(yōu)狀態(tài)融合 Kalman預(yù)報器為：對伙 + 11 燈=erP(k +11 k)le)-leTP(k + 1| k)lx(k +11 燈，廳(k +11R) = (eTP(k +11 k)-le)l并且有關(guān)系式：trP；'(k + k)<

12、;trP(k + lk)J=t - yL 英中爼伙+1|燈心伙+1|燈)r xt(k + 1|R)、C =:,P(k + lk) =J)A伙+1秋)乙伙+1|燈,x(k +11 i)=丸伙+屮)丿土伙+ 1|燈(心1廠、厶)和說伙+ l|R)(iJ=l,上)由定理322確定。定理3.4.2多傳感器系統(tǒng)(3.1.1)和(3.1.2丿在假設(shè)1、2卜，按標鼠加權(quán)最優(yōu)狀態(tài)融介 Kalman預(yù)報器為：疋伙+11 k) 一 X ©伙)乞伙十11H， 4伙十11燈一工工 < 伙)“;伙)坊伙十11 k) »«1 1-1 ;-1并且有關(guān)系式：trP(k + k)<tr

13、P 伙+ 1| 燈，心 1,乙其中*伙)=何伙)盜對=堤錯怎,“(11)T,'幾伙+ 1|R)叭(k + 1|燈、&伙+ 1|燈=，、巴伙+ 1|燈僉伙+ 1秋力 其中土伙+ 1|燈(心1,，厶)和伙+ 1|燈由定理3.2.2確定。定理343多傳感器系統(tǒng)(3.1.1)和(3.1.2)在假設(shè)1、2 F，按對角陣加權(quán)最優(yōu)狀態(tài)融合 Kalman濾波器為：眾伙 + 1R) = S伙此伙 +11«) +11R)二(孑(円伙 +11 燈“)1|«1并且有關(guān)系式:再伙+ 1|*)</>/伙+ 1|約,心1,，厶謀差平方和為空伙+11燈=£農(nóng)伙+11燈

14、，并且有關(guān)系式：7=1trP (k + k)< trP 伙 +11 燈,i = 1,丄其中"伙)=(洛伙)(PJ(k + lk)le"pj伙+ 1|燈尸ae = (l 1 )75#盤伙+ 1|燈比伙+ 1|燈、P( + 1|R)= 丨，即伙+ 1|燈為仇伙+ 1|R)的第ud元伙+ 1|燈P£(k + 1|燈丿素，x.(k + 1| *)(/ = !, ,£)和匚伙+ 1|燈由定理322確定。3.5集中式Kalman濾波和加權(quán)狀態(tài)Kalman濾波的精度關(guān)系系統(tǒng)(3.1.1.)和(3.1.2)的集中式融介系統(tǒng)為：(3.5.1 丿x 伙 + 1) =

15、A(k )x伙)+ C(k)w( k)J() = H (k )x(k) + v(k)其中'H©、W伙)、y(h =-,H 伙)=:設(shè)13 5.1丿的Kalman濾波器和預(yù)報器分別為乾伙k),x伙+ 1|燈，對應(yīng)的謀差方差陣分別 為&伙|約蟲伙+ 1比)。則有如下結(jié)論:定理3.5.1多傳感器系統(tǒng)(3丄歟3.1,2)的三種加權(quán)狀態(tài)融合Kalman估值器、集屮式融 合Kalman估值器和局部Kalman估值器有精度關(guān)系(N = 0,1 )：叱(k + NN) 伙 + N | N) 9叱/伙 + N | N)<trP(k + N N) SR(k + NN),i = l,

16、L證明：由注2.3.1 U接得第24個不等式。卜面證明第一個不等式。由J：集屮式融合是在所有傳感器提供的觀測數(shù)據(jù)形成的線性空間(記作厶丿屮尋優(yōu)，而炬 陣加權(quán)狀態(tài)融介是在其中某個特定的線性子空間(記作厶”J中尋優(yōu)，因此Lm C L ,故得第 一個不等式。§ 4多傳感器狀態(tài)融合穩(wěn)態(tài)Kalman濾波器和預(yù)報器同樣考虎不金控制變吊的多傳感器線性離散定常隨機系統(tǒng)(3.1.1)和(3.1.2)。假設(shè)3： (£0)(21,，厶)是完全可觀對，(AC)是完全可控。問題是求局部和融合的穩(wěn)態(tài)Kalman濾波器和預(yù)報器。4.1局部穩(wěn)態(tài)Kalman濾波器和預(yù)報器記Kt = liniKt(k), 2

17、7 =unP(k + lk). 27 = lunP (k + lk) 9 Pt = hmP(k k) JlT8衣 T8J 女 T8 IJ&T8Pti = linPy(kk).則爲=爲,R = P£滿足穩(wěn)態(tài)Ricatti 方程：£ = ASl -+ Q 尸HQ屮 + CQ.C(4.1.1)對第i個傳感器(321丿和(3.2.2),在定理3.2.1和3.2.2中令k T5 9上得到局部穩(wěn)態(tài) Kalman濾波器和預(yù)報器：定理411(局部穩(wěn)態(tài)Kalman濾波器)系統(tǒng)(321)和(322)在假設(shè)13卜的穩(wěn)態(tài)Kalman 濾波器為：£(上| R)=心丘“ -屮-1)+

18、 K(燈或£滿足Lyapunov方程：P產(chǎn) £APK* + Kjc2 K* + K2K：并IL局部濾波誤差互協(xié)方差陣町滿足Lyapunov方程(i工j )：P產(chǎn) K止KQK&K：其中 Ks伙)=/一£仏"，KlC = I-KiHiC, K, =, Q,=H/H： + Q、爲滿足穩(wěn)態(tài)Ricatd方程(4丄1),并且初值i(O|O) = jt4>, P(0|0)=化。定理412(局部穩(wěn)態(tài)Kalman預(yù)報器)系統(tǒng)(3.2.1)和(322)在假設(shè)13卜的穩(wěn)態(tài)Kalimn 預(yù)報器為：玄伙+11燈=K角伙k-)+ K伙)局部預(yù)報誤差方差陣£滿

19、足穩(wěn)態(tài)RLcatti方程(4.1.1),局部預(yù)報誤差互協(xié)方差陣2；滿足 Lyapunov 方程(/工 j)：爲(燈爲(K：)JCQL其中 K,A/-K“, Kip = AK, , K 嚴 5H：Q；，Q廣 H 厶H：倫,初值 i(l | 0)= A從 + B”(0), P(l| 0) = APqA1 + CQCT »4.2狀態(tài)融合穩(wěn)態(tài)Kalman濾波器記5 或$或,= hmP(kk)t Z? = lunRu( + l|)oR->8氏T8首先，利用定理4.1.1并根據(jù)第二節(jié)定理2.2.1-2.2.3得到繪優(yōu)狀態(tài)融合穩(wěn)態(tài)Kalnian濾 波器。定理4.2.1多傳感器系統(tǒng)(3.1.1

20、丿和(3.1.2)在假設(shè)13卜，按矩陣加權(quán)最優(yōu)狀態(tài)融合穩(wěn)態(tài) Kalman濾波器為：站伙 | 燈=(erp-leyleTP'lx(k k), P； = (/P%尸并且有關(guān)系式：trP" = 丄6(P P rllrlL其中e =:,p =,x(k | k)=-耳P-丸伙|燈,xt(kk)(i = l L)和Pit （/, j = 1,，厶）由定理4.1.1確定。定理422多傳感器系統(tǒng)（3丄1丿和（3丄2）在假設(shè)13 F，按標彊加權(quán)最優(yōu)狀態(tài)融合穩(wěn)態(tài)Kalman濾波器為：瑋伙I燈=f代述k |燈，C' = f f gqPjj1=1 /M并且有關(guān)系式：咗Wl，,L#土伙|燈（心

21、1,，厶）和匕由定理4.1.1確定。定理42.3多傳感器系統(tǒng)（3丄1丿和（3丄2）在假設(shè)13 F,按對角陣加權(quán)最優(yōu)狀態(tài)融介穩(wěn) 態(tài)Kalman濾波器為：鶴伙r）=e。角伙I町,硝=（e（p）七匸,丿=1,/并且仃關(guān)系式:吃 <卅,心1,，厶八1,/誤差平方和為"用=£%,并月.有關(guān)系式：>-1為Pif的第U0元索，x,（k I k）（i = 1,L）和P,由定理4.1.1確定。例4.2.1考慮三傳感器二維跟蹤系統(tǒng)：其中x(k) =I伙）衛(wèi)伙）,$伙）和，伙）分別是各自運動目標在時刻R7；的位置和速度,7；為采( 71>/ 0.57；訂1。1丿X伙)+<

22、乙Jx(k + 1)=w伙)% 伙)=(1 0) w伙)+ V.伙),i = 1,2,3樣周期，帆斤八叫伙人心123是均值為零的不相關(guān)的白噪聲。取7； =0.1, <r; = 4,瓦=4,< =9 , b = 25。求局部和三種加權(quán)狀態(tài)融合穩(wěn)態(tài)Kalrrnn濾波器球伙+ 1）二加或$或d丿。解：,門 1)(0.5)/、心，B = 0,C= H =0)2 = 4,0=4,0=9,0 = 25v 1/ Z故第i個傳感器的穩(wěn)態(tài)Ricatti方程為：Zt = AZl- HHZtHT + 2 尸HZtAT + CQt CT解得厶(可用迭代方法丿后可得K, = E,HtHE,Ht + 2丁和K

23、iA =(/-K,H )A :'0.13185) 、0.093175丿K“=U - K,H)A =0.868150.086815、-0.0931750.99068 y心二爲爲" + 2,T 0.10904、衛(wèi).062927 丿0.890260.089096、-0.0629270.99371 0.085553、0.038251,KiA=(I-KiH)A =0.914450.091445、-0.0382510.99617 ,故第/個傳感器的穩(wěn)態(tài)Kahmn濾波器為：左“ | R) = K后(R - 屮-1)+ K.ykPT KHg比中0.52740.37270.3727 0.546

24、04,=/-/C2HZ2 =0.98135 0.56634)0.56634 0.673117r 2.13880.95627、0.95627 0.48345,并H局部濾波誤差互協(xié)方差陣Pt)滿足Lyapunov方程(i豐j)：P汀K肩.JK也K<解徼可用迭代方法):0164250.21778,Pn =<0.220470.26154、0.330270.34701、,0.217780.444387衛(wèi).261540.48345；z0.347010.55974丿由定理421知，在矩陣加權(quán)融介準則卜，得融介穩(wěn)態(tài)Kalman濾波器:x； (kk) =曆士 伙 k)+Arx2 伙 | k) + A

25、；x3 伙 I 燈其中(W，Q)=(Rp-b)FpT, p= &Pl2常&,得'0.608360.91319、-0.02038 0.71606,< 0.12726'0.264370.48996、1-0.010935 0.32216丿-0.14031 )(0.031314 -0.038216,由定理422知，在標吊加權(quán)融合準則卜，得融介穩(wěn)態(tài)Kalman濾波器：x'q(kk) = a；*伙 | 燈 + atx2伙 | R) + a；兔伙 | k)卩_ P" e-/化七1，P,r =trP22 嗎°； J 7叫叱J其中,即 a： =

26、0.65215,a； = 0.27729 , a； = 0.070561。由定理4.2.3知，在對角陣加權(quán)融合準則卜，得融合穩(wěn)態(tài)Kahmn濾波器:眾（k |燈=工a角伙| k）, j = 192(1、_ (PX 1,pj = 2丿kJer(PJ)leJ丿兄P為,代為£的第uj元索，即其小述= 0.6822, a： = 0.2959,a = 0.64479 , a： = 0.27592 , a： = 0.079288 ,< = 0.21898.4.3狀態(tài)融合穩(wěn)態(tài)Kalman預(yù)報器同樣，利用定理4.1.2并根據(jù)第二節(jié)定理2.2.1-2.2 3得到最優(yōu)狀態(tài)融介穩(wěn)態(tài)Kalrrnn預(yù)報 器。定理431多傳感器系統(tǒng)（3.1.1丿和（3丄2）在假設(shè)13 F，按矩陣加權(quán)最優(yōu)狀態(tài)融介穩(wěn)態(tài) Kalman預(yù)報器為：x（