卡爾曼濾波器及matlab代碼

1、 信息融合大作業(yè) 維納最速卜降法濾波器，卡爾蛀濾波器設(shè)計(jì)及Matlab仿真 時(shí)間:2010-12-6 專業(yè)：信息工程 班級(jí)：09030702 學(xué)號(hào)：2007302171 姓名：馬志強(qiáng) 1 .濾波問題淺談 估計(jì)器或?yàn)V波器這一術(shù)語通常用來稱呼一個(gè)系統(tǒng)， 設(shè)計(jì)這樣的系統(tǒng)是為了從含有噪聲的數(shù)據(jù)中提取人們感興趣的，接近規(guī)定質(zhì)量的信息。由于這樣一個(gè)寬目標(biāo)，估計(jì)理論應(yīng)用于諸如通信、雷達(dá)、聲納、導(dǎo)航、地震學(xué)、生物醫(yī)學(xué)工程、金融工程等眾多不同的領(lǐng)域。例如，考慮一個(gè)數(shù)字通信系統(tǒng)，其基本形式由發(fā)射機(jī)、信道和接收機(jī)連接組成。發(fā)射機(jī)的作用是把數(shù)字源(例如計(jì)算機(jī))產(chǎn)生的0、1符號(hào)序列組成的消息信號(hào)變換成為適合于信道上傳送

2、的波形。而由于符號(hào)間干擾和噪聲的存在，信道輸出端收到的信號(hào)是含有噪聲的或失真的發(fā)送信號(hào)。接收機(jī)的作用是，操作接收信號(hào)并把原消息信號(hào)的一個(gè)可靠估值傳遞給系統(tǒng)輸出端的某個(gè)用戶。 隨著通信系統(tǒng)復(fù)雜度的提高， 對(duì)原消息信號(hào)的還原成為通信系統(tǒng)中最為重要的環(huán)節(jié)， 而噪聲是接收端需要排除的最主要的干擾， 人們也設(shè)計(jì)出了針對(duì)各種不同條件應(yīng)用的濾波器， 其中最速下降算法是一種古老的最優(yōu)化技術(shù)，而卡爾曼濾波器隨著應(yīng)用條件的精簡(jiǎn)成為了普適性的高效濾波器。 2 .維納最速下降算法濾波器 2.1 最速下降算法的基本思想 考慮一個(gè)代價(jià)函如(W)，它是某個(gè)未知向量W的連續(xù)可微分函數(shù)。函初(w)將W的元素映射為實(shí)數(shù)。這里，我

3、們要尋找一個(gè)最優(yōu)解W。使它滿足如下條件 /(Wo)W/(w) (2.1) 這也是無約束最優(yōu)化的數(shù)學(xué)表示。 特別適合于自適應(yīng)濾波的一類無約束最優(yōu)化算法基于局部迭代下降的算法:從某一初始猜想w(0)出發(fā).產(chǎn)生一系列權(quán)向量w(l),w(2),，使得代價(jià)函數(shù)/(w)在算法的每一次迭代都是下降的.即 /(ws+i)/(w(m) 其中w()是權(quán)向量的過去值，而w(+1)是其更新值。 我們希望算法最終收斂到最優(yōu)值迭代下降的一種簡(jiǎn)單形式是最速下降法， 該方法是沿最速下降方向連續(xù)調(diào)整權(quán)向量。為方便起見，我們將梯度向量 因此，最速下降法可以表示為 1w(n+1)=w(n)-jU叼5)|2 此式表明當(dāng)為正數(shù)時(shí)，/(

4、w(n+l)PHC=OI=B)GI=)C) 圖3.1線性動(dòng)態(tài)離散時(shí)間系統(tǒng)的信號(hào)流圖表示 “狀態(tài)”的概念是這種表示的基礎(chǔ)。狀態(tài)向量，簡(jiǎn)單地說狀態(tài)，定義為數(shù)據(jù)的最小集合，這組數(shù)據(jù)足以唯一地描述系統(tǒng)的自然動(dòng)態(tài)行為。換句話說，狀態(tài)由預(yù)測(cè)系統(tǒng)未來特性時(shí)所素要的，與系統(tǒng)的過去行為有關(guān)的最少的數(shù)據(jù)組成。典型地，比較有代表性的情況是，狀態(tài)無何)是未知的。為了估計(jì)它，我們使用一組觀測(cè)數(shù)據(jù)，在途中用向量y(n)表示。y(幾)成為觀測(cè)向量或者簡(jiǎn)稱觀測(cè)值，并假設(shè)它是N維的。 在數(shù)學(xué)上，圖3.1表示的信號(hào)流圖隱含著一下兩個(gè)方程： (1)過程方程 x(n+1)=F(n4-l,n)x(n)+%(八) (3.1) 式中，MX

5、I向量外(幾)表示噪聲過程，可建模為零均值的白噪聲過程，且其相關(guān)矩陣定義為 Eg現(xiàn)(k)卜肥,) (2)測(cè)量方程 y(n)=C(n)x(n)+v2(n) (3.2) 其中C()是已知的NXM測(cè)量矩陣。NX1向量也(幾)稱為測(cè)量噪聲，建模為零均值的白噪聲過程，其相關(guān)矩陣為 E出理(A)=25) (3.3) 測(cè)量方程(3.2)確立了可測(cè)系統(tǒng)輸出y(n)與狀態(tài)”5)之間的關(guān)系，如圖3.1所示。 3.2 新息過程 為了求解卡爾曼濾波問題，我們將應(yīng)用基于新息過程的方法。根據(jù)之前所述,用向量5(川外：T)表示幾=1時(shí)刻到n-1時(shí)刻所有觀測(cè)數(shù)據(jù)過去值給定的情況下,你時(shí)刻觀測(cè)數(shù)據(jù)y(n)的最小均方估計(jì)。過去的

6、值用觀測(cè)值y(l),y(2),y(n-1)表示，他們張成的向量空間用即一1表示。從而可以定義新息過程如下： =y(n)-y(n|yn,i) (3.4) 其中MX1向量?jī)x幾)表示觀測(cè)數(shù)據(jù)y(?i)的新息。 3.3 應(yīng)用新息過程進(jìn)行狀態(tài)估計(jì) 下面，我們根據(jù)信息過程導(dǎo)出狀態(tài)“。)的最小均方估計(jì)。根據(jù)推導(dǎo)，這個(gè)估計(jì)可以表示成為新息過程0(1),。(2),以幾)序列的線性組合，即 n 和1%)=2乩(4皿/0 k=l (3.5)其 中&(k)bi是一組待定的MXN矩陣。根據(jù)正交性原理，預(yù)測(cè)狀態(tài)誤差向量與新息過程正交，即 E(i,n)aH(m)=Ex(i)-xiynaH(m)=O (3.6) 將式

7、(3.5)代入式(3.6),并利用新息過程的正交性質(zhì)，即得 (3.7) 因此，式(3.7)兩邊同時(shí)右乘逆矩陣RT(m),可得4(m)的表達(dá)式為 =Ex(0aH(7n)?-1(m) (3.8) 最后，將式(3.8)帶入式(3.5),可得最小軍方差估計(jì)n k=l n-1 =WEx(i)QH(&)RT(A)廢(k)+EMi)(rH(n)RT(n)cr(n)k=l (3.9) 故對(duì)于i=71+1,有 n-1 x(n+1|%)=Ex(n+1)-飛)好)k=l +Exn+l)aH(n)J?-1(n)a(n) (3.10) 然而，n+1時(shí)刻的狀態(tài)+1)與幾時(shí)刻的狀態(tài)x(7i)的關(guān)系式由式可以推導(dǎo)出對(duì)

8、于0kn,有 Ex(n+l)a(k)=EF(n+1,n)x(n)+Vi(n)aH(k) =F(n+1,7i)Ex(n)/(k) (3.11) 其中2(A)只與觀測(cè)數(shù)據(jù)y(l),1y(2),y(k)有關(guān)。 因此可知，”i(n)與。(k)彼此正交(其中0kn)o利用式(3.11)以及當(dāng)i=n時(shí)以i|%)的計(jì)算公式，可將式(3.10)右邊的求和項(xiàng)改寫為 n-1n-1 WEQ+1)程(k)RT(k)a(k)=F(n+l,n)2Ex(n)程(k)RT(k)盤(k)k=Lk=l =F(n+1,幾)e(|yn_i) (3.12) 為了進(jìn)一步討論，引入如下基本定義。 3.4 卡爾曼增益 定義MxN矩陣 G(n

9、)=Ex(n+l)aH(n)L(k) (3.13) 其中EX(TI+1)。 ”(幾)是狀態(tài)向量無(幾+1)和新息過程。 (n)的互相關(guān)矩陣。 利用這一定義和式(3.12)的結(jié)果，可以將式(3.10)簡(jiǎn)單重寫為 x(n+l|yn)=F(n+l,n)x(n|yn_i)+G(n)a(n) (3.14) 式(3.14)具有明確的物理意義。它標(biāo)明：線性動(dòng)態(tài)系統(tǒng)狀態(tài)的最小均方估計(jì)x(n+1|%)可以由前一個(gè)估計(jì)e(川y_i)求得。為了表示對(duì)卡爾曼開創(chuàng)性貢獻(xiàn)的認(rèn)可，將矩陣G(n)稱為卡爾曼增益。 現(xiàn)在剩下唯一要解決的問題是，怎樣以一種便于計(jì)算的形式來表示卡爾曼增益GS)。為此，首先將“(九+1)與。(九)乘

10、積的期望表示為 Ex(n+l)aH(n)=F(n+1,ri)Ex(n)H(n,nl)CH(n) (3.15) 式中利用了狀態(tài)尤 6)與噪聲向量V2(n)互不相關(guān)這一事實(shí)。其次，由于預(yù)測(cè)狀態(tài)誤差向量(以幾-1)與估計(jì)(九|%1.1)正交，因此兄(幾、八.1)與”(耳幾-1)乘機(jī)的期望為零。這樣，用預(yù)測(cè)狀態(tài)誤差向量?jī)x1)代替相乘因子武九)，將不會(huì)引起式(3.15)變化，故有 Ex(n+l)aH(n)=F(n+l,n)Ee(n,n-l)EH(n,n-l)CH(n) (3.16) 由此，可將上式進(jìn)一步變化為 Ex(n4-l)aH(n)=F(n+l,n)K(n,n-l)CH(n) (3.17) 現(xiàn)在我們

11、重新定義卡爾曼增益。為此，將式(3.17)代入式(3.13)得 G(n)=F(n+l,n)K(n,nl)CH(n)Rk) (3.18) 現(xiàn)在我們已經(jīng)了解了卡爾曼濾波的整個(gè)過程和相應(yīng)的參數(shù)設(shè)置.，為了能夠更為方便利用計(jì)算機(jī)仿真實(shí)現(xiàn)，特將其中參數(shù)變量進(jìn)行小結(jié)。 卡爾曼變量和參數(shù)小結(jié) 變量 定義 維數(shù) 雙n) 門時(shí)刻狀態(tài) Mx1 yW n時(shí)刻狀態(tài)值 Nx1 F(n+l,n) 從川時(shí)刻到+1時(shí)刻的轉(zhuǎn)移矩陣 MxM C(n) n時(shí)刻的測(cè)量矩陣 NxM l&q2/Ryx最大特征值 hn=zeros(l,N); hn(:)=5; vg=O; Rxx=xcorr(l); Ryx=min(min(cor

12、rcoef(l,1+randn); echooff fori=l:N-l; %vg=2*Rxx*hn(:,i)-2*Ryx; %hn(:/i+l)=hn(:J)-l/2*q*vg; vg=2*Rxx*hn(i)-2*Ryx; hn(i+l)=hn(i)-l/2*q*vg; m(i)=l； end l=l:N-l; 后聯(lián)平滑濾波器的卡爾曼漉波器 clear cic; N=300; CON=5; x=zeros(l,N); x(l)=l； P=10； Q=andn(l,N)*0.2;%過程噪聲協(xié)方差 R=randn(l,N);%觀測(cè)噪聲協(xié)方差 y=R+CON;%加過程噪聲的狀態(tài)輸出 fork=2:

13、N Ql=cov(Q(l:k-l);%過程噪聲協(xié)方差 Q2=cov(R(l:k-l); x(k)=x(k-1);%預(yù)估計(jì)k時(shí)刻狀態(tài)變量的值 p=p+Q1;%對(duì)應(yīng)于預(yù)估值的協(xié)方差 kg=P/(P+Q2);%kalmangain x(k)=x(k)+kg*(y(k)-x伙)； p=(1-kg)p; end FilterWid=10; smooth_res=zeros(l,N); kalman_p=zeros。,N); fori=Filter_Wid+1:N tempsum=0; kalmanm=0; forj=i-Filter_Wid:i-1 tempsum=tempsum+y(j); kalman_m=kalman_m+x(j); end kalman_p(i)=kalman_m/Filter_Wid; smooth_res(i)=tempsum/FiltejWid;%平滑濾波end %figure(l); %hist(y); t=l:N; figure(l); expValue=zeros(lzN); fori=1:N expValue(i)=CON; end plot(t/e