卡爾曼濾波器和matlab代碼(編輯修改稿)
2024-07-17 03:42 本頁(yè)面
【文章內(nèi)容簡(jiǎn)介】 ) ()其中Bi(k)k=1n是一組待定的MN矩陣。根據(jù)正交性原理，預(yù)測(cè)狀態(tài)誤差向量與新息過(guò)程正交，即Eεi,nαHm=ExixiynαH(m)=O ()將式()代入式()，并利用新息過(guò)程的正交性質(zhì)，即得ExiαHm=BimEαmαH(m)=BimR(m) ()因此，式()兩邊同時(shí)右乘逆矩陣R1(m)，可得Bim的表達(dá)式為Bim=ExiαHmR1(m) ()最后，將式()帶入式()，可得最小軍方差估計(jì)xiyn=k=1nExiαHkR1(k)αk=k=1n1ExiαHkR1(k)αk+ExiαHnR1(n)αn ()故對(duì)于i=n+1，有xn+1yn=k=1n1Exn+1αHkR1(k)αk+Exn+1αHnR1(n)αn ()然而，n+1時(shí)刻的狀態(tài)x(n+1)與n時(shí)刻的狀態(tài)x(n)的關(guān)系式由式可以推導(dǎo)出對(duì)于0≤k≤n，有Exn+1αHk=EFn+1,nxn+v1nαHk=Fn+1,nE[xnαH(k)] ()其中αk只與觀測(cè)數(shù)據(jù)y1,y2,?,y(k)有關(guān)。因此可知，v1n與αk彼此正交(其中0≤k≤n)。利用式()以及當(dāng)i=n時(shí)xiyn的計(jì)算公式，可將式()右邊的求和項(xiàng)改寫為k=1n1Exn+1αHkR1(k)αk=Fn+1,nk=1n1ExnαHkR1kαk=F(n+1,n) xnyn1 ()為了進(jìn)一步討論，引入如下基本定義。 卡爾曼增益定義MN矩陣Gn=E[x(n+1)αH(n)] R1(k) ()其中E[x(n+1)αH(n)]是狀態(tài)向量x(n+1)和新息過(guò)程α(n)的互相關(guān)矩陣。利用這一定義和式()的結(jié)果，可以將式()簡(jiǎn)單重寫為xn+1yn= Fn+1,nxnyn1+Gnα(n) ()式()具有明確的物理意義。它標(biāo)明：線性動(dòng)態(tài)系統(tǒng)狀態(tài)的最小均方估計(jì)xn+1yn可以由前一個(gè)估計(jì)xnyn1求得。為了表示對(duì)卡爾曼開(kāi)創(chuàng)性貢獻(xiàn)的認(rèn)可，將矩陣Gn稱為卡爾曼增益。現(xiàn)在剩下唯一要解決的問(wèn)題是，怎樣以一種便于計(jì)算的形式來(lái)表示卡爾曼增益Gn。為此，首先將x(n+1)與αH(n)乘積的期望表示為Exn+1αHn=Fn+1,nExnεHn,n1CH(n) ()式中利用了狀態(tài)xn與噪聲向量v2(n)互不相關(guān)這一事實(shí)。其次，由于預(yù)測(cè)狀態(tài)誤差向量ε(n,n1)與估計(jì)xnyn1正交，因此xnyn1與εHn,n1乘機(jī)的期望為零。這樣，用預(yù)測(cè)狀態(tài)誤差向量ε(n,n1)代替相乘因子xn，將不會(huì)引起式()變化，故有Exn+1αHn=Fn+1,nEε(n,n1)εHn,n1CH(n) ()由此，可將上式進(jìn)一步變化為Exn+1αHn=Fn+1,nK(n,n1)CH(n) ()現(xiàn)在我們重新定義卡爾曼增益。為此，將式()代入式()得Gn=Fn+1,nK(n,n1)CH(n) R1(k) ()現(xiàn)在我們已經(jīng)了解了卡爾曼濾波的整個(gè)過(guò)程和相應(yīng)的參數(shù)設(shè)置，為了能夠更為方便利用計(jì)算機(jī)仿真實(shí)現(xiàn)，特將其中參數(shù)變量進(jìn)行小結(jié)?？柭兞亢蛥?shù)小結(jié)變量定義維數(shù)x(n)n時(shí)刻狀態(tài)M1y(n)n時(shí)刻狀態(tài)值N1F(n+1,n)從n時(shí)刻到n+1時(shí)刻的轉(zhuǎn)移矩陣MMC(n)n時(shí)刻的測(cè)量矩陣NMQ1(n)過(guò)程噪聲v1(n)的相關(guān)矩陣MMQ2(n)過(guò)程噪聲v2(n)的相關(guān)矩陣NNxnyn1給定觀測(cè)值y1,y2,?,y(n1)在n時(shí)刻狀態(tài)的預(yù)測(cè)估計(jì)M1xnyn給定觀測(cè)值y1,y2,?,y(n)在n時(shí)刻狀態(tài)的濾波估計(jì)M1G(n)n時(shí)刻卡爾曼增益矩陣MNα
數(shù)學(xué)相關(guān)推薦
鄂ICP備17016276號(hào)-1