卡爾曼濾波器matlab代碼

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

2、端收到的信號是含有噪聲的或失真的發(fā)送信號。接收機(jī)的作用是，操作接收信號并把原信息信號的一個(gè)靠譜估值傳達(dá)給系統(tǒng)輸出端的某個(gè)用戶。跟著通訊系統(tǒng)復(fù)雜度的提升，對原信息信號的復(fù)原成為通訊系統(tǒng)中最為重要的環(huán)節(jié)，而噪聲是接收端需要清除的最主要的擾亂，人們也設(shè)計(jì)出了針對各樣不一樣條件應(yīng)用的濾波器，此中最速降落算法是一種古老的最優(yōu)化技術(shù)，而卡爾曼濾波器跟著應(yīng)用條件的精簡成為了普適性的高效濾波器。2維納最速降落算法濾波器2.1最速降落算法的基本思想考慮一個(gè)代價(jià)函數(shù)，它是某個(gè)未知向量的連續(xù)可微分函數(shù)。函數(shù)將的元素映照為實(shí)數(shù)。這里，我們要找尋一個(gè)最優(yōu)解。使它知足以下條件(2.1)這也是無拘束最優(yōu)化的數(shù)學(xué)表示。特別合

3、適于自適應(yīng)濾波的一類無拘束最優(yōu)化算法鑒于局部迭代降落的算法：從某一初始猜想出發(fā)，產(chǎn)生一系列權(quán)向量，使得代價(jià)函數(shù)在算法的每一次迭代都是降落的，即此中是權(quán)向量的過去值，而是其更新值。我們希望算法最后收斂到最優(yōu)值。迭代降落的一種簡單形式是最速降落法，該方法是沿最速降落方向連續(xù)調(diào)整權(quán)向量。為方便起見，我們將梯度向量表示為(2.2)所以，最速降落法能夠表示為(2.3)此中代表進(jìn)度，是正常數(shù)，稱為步長參數(shù)，1/2因子的引入是為了數(shù)學(xué)上處理方便。在從到的迭代中，權(quán)向量的調(diào)整量為(2.4)為了證明最速降落算法知足式(2.1)，在處進(jìn)行一階泰勒睜開，獲取(2.5)此式關(guān)于較小時(shí)是建立的。在式(2.4)中設(shè)為負(fù)值

4、向量，因此梯度向量負(fù)值向量，所以使用埃爾米特轉(zhuǎn)置。將式(2.4)用到式(2.5)中，獲取也為此式表示當(dāng)為正數(shù)時(shí)，數(shù)減小，當(dāng)時(shí)，代價(jià)函數(shù)趨于最小值。所以，跟著的增添，代價(jià)函。2.2最速降落算法應(yīng)用于維納濾波器考慮一個(gè)橫向?yàn)V波器，其抽頭輸入為，對應(yīng)的抽頭權(quán)值為。抽頭輸入是來自零均值、有關(guān)矩陣為的廣義安穩(wěn)隨機(jī)過程的抽樣值。除了這些輸入外，濾波器還要一個(gè)希望響應(yīng)，以便為最優(yōu)濾波供給一個(gè)參照。在時(shí)刻抽頭輸入向量表示為，濾波器輸出端希望響應(yīng)的預(yù)計(jì)值為,此中是由抽頭輸所張成的空間。空過比較希望響應(yīng)及其預(yù)計(jì)值，能夠獲取一個(gè)預(yù)計(jì)偏差，即(2.6)這里是抽頭權(quán)向量與抽頭輸入向量的內(nèi)積。能夠進(jìn)一步表示為相同，抽頭輸

5、入向量可表示為假如抽頭輸入向量在時(shí)刻的代價(jià)函數(shù)和希望響應(yīng)是結(jié)合安穩(wěn)的，此時(shí)均方偏差或許是抽頭權(quán)向量的二次函數(shù)，于是能夠獲取(2.7)此中，為目標(biāo)函數(shù)的方差，抽頭輸入向量與希望響應(yīng)關(guān)向量，及為抽頭輸入向量的有關(guān)矩陣。進(jìn)而梯度向量能夠?qū)憺榈南嗷?2.8)此中在列向量中部和虛部矩陣和互有關(guān)向量和分別是代價(jià)函數(shù)對應(yīng)第個(gè)抽頭權(quán)值的偏導(dǎo)數(shù)。對最速降落算法應(yīng)用而言，假定式(2.8)已知，則關(guān)于給定的抽頭權(quán)向量為的實(shí)中有關(guān)(2.9)它描繪了為那濾波中最速降落法的數(shù)學(xué)表達(dá)式。3.卡爾曼濾波器3.1卡爾曼濾波器的基本思想卡爾曼濾波器是用狀態(tài)空間觀點(diǎn)描繪其數(shù)學(xué)公式的，此外新奇的特色是，他的解遞歸運(yùn)算，能夠不加改正地

6、應(yīng)用于安穩(wěn)和非安穩(wěn)環(huán)境。特別是，其狀態(tài)的每一次更新預(yù)計(jì)都由前一次預(yù)計(jì)和新的輸入數(shù)據(jù)計(jì)算獲取，所以只要儲存前一次預(yù)計(jì)。除了不需要儲存過去的全部觀察數(shù)據(jù)外，卡爾曼濾波計(jì)算比直接根據(jù)濾波過程中每一步全部過去數(shù)據(jù)進(jìn)行估值的方法都更為有效。+圖3.1線性動向失散時(shí)間系統(tǒng)的信號流圖表示“狀態(tài)”的觀點(diǎn)是這類表示的基礎(chǔ)。狀態(tài)向量，簡單地說狀態(tài)，定義為數(shù)據(jù)的最小會合，這組數(shù)據(jù)足以獨(dú)一地描繪系統(tǒng)的自然動向行為。換句話說，狀態(tài)由展望系統(tǒng)將來特征時(shí)所素要的，與系統(tǒng)的過去行為有關(guān)的最少的數(shù)據(jù)構(gòu)成。典型地，比較有代表性的狀況是，狀態(tài)是未知的。為了預(yù)計(jì)它，我們使用一組觀測數(shù)據(jù)，在途頂用向量表示。成為觀察向量或許簡稱觀察值，

7、并假定它是維的。在數(shù)學(xué)上，圖3.1表示的信號流圖隱含著一下兩個(gè)方程：過程方程(3.1)式中，向量表示噪聲過程，可建模為零均值的白噪聲過程，且其有關(guān)矩陣定義為丈量方程(3.2)此中是已知的丈量矩陣。向量稱為丈量噪聲，建模為零均值的白噪聲過程，其有關(guān)矩陣為(3.3)丈量方程(3.2)確定了可測系統(tǒng)輸出與狀態(tài)之間的關(guān)系，如圖3.1所示。3.2新息過程為了求解卡爾曼濾波問題，我們將應(yīng)用鑒于新息過程的方法。依據(jù)以前所述，用向量表示時(shí)刻到時(shí)刻全部觀察數(shù)據(jù)過去值給定的狀況下，你時(shí)刻觀察數(shù)據(jù)的最小均方預(yù)計(jì)。過去的值用觀察值表示，他們張成的向量空間用表示。進(jìn)而能夠定義新息過程以下：(3.4)此中向量表示觀察數(shù)據(jù)

8、的新息。3.3應(yīng)用新息過程進(jìn)行狀態(tài)預(yù)計(jì)下邊，我們依據(jù)信息過程導(dǎo)出狀態(tài)的最小均方預(yù)計(jì)。依據(jù)推導(dǎo)，這個(gè)估計(jì)能夠表示成為新息過程序列的線性組合，即(3.5)此中是一組待定的矩陣。依據(jù)正交性原理，展望狀態(tài)偏差向量與新息過程正交，即(3.6)將式(3.5)代入式(3.6)，并利用新息過程的正交性質(zhì)，即得(3.7)所以，式(3.7)兩邊同時(shí)右乘逆矩陣，可得的表達(dá)式為(3.8)最后，將式(3.8)帶入式(3.5)，可得最小軍方差預(yù)計(jì)(3.9)故關(guān)于，有(3.10)但是，時(shí)刻的狀態(tài)與時(shí)刻的狀態(tài)的關(guān)系式由式能夠推導(dǎo)出對于，有(3.11)此中只與觀察數(shù)占有關(guān)。所以可知，與相互正交(此中)。利用式(3.11)以及當(dāng)

9、時(shí)的計(jì)算公式，可將式(3.10)右側(cè)的乞降項(xiàng)改寫為(3.12)為了進(jìn)一步議論，引入以下基本定義。3.4卡爾曼增益定義矩陣(3.13)此中是狀態(tài)向量和新息過程的互有關(guān)矩陣。利用這必定義和式(3.12)的結(jié)果，能夠?qū)⑹?3.10)簡單重寫為(3.14)式(3.14)擁有明確的物理意義。它注明：線性動向系統(tǒng)狀態(tài)的最小均方預(yù)計(jì)能夠由前一個(gè)預(yù)計(jì)求得。為了表示對卡爾曼創(chuàng)始性貢獻(xiàn)的認(rèn)同，將矩陣稱為卡爾曼增益。此刻剩下獨(dú)一要解決的問題是，如何以一種便于計(jì)算的形式來表示卡爾曼增益。為此，第一將與乘積的希望表示為(3.15)式中利用了狀態(tài)與噪聲向量互不有關(guān)這一事實(shí)。其次，因?yàn)檎雇麪顟B(tài)偏差向量與預(yù)計(jì)正交，所以與趁機(jī)

10、的希望為零。這樣，用展望狀態(tài)偏差向量取代相乘因子，將不會引起式(3.15)變化，故有(3.16)由此，可將上式進(jìn)一步變化為(3.17)此刻我們從頭定義卡爾曼增益。為此，將式(3.17)代入式(3.13)得(3.18)此刻我們已經(jīng)認(rèn)識了卡爾曼濾波的整個(gè)過程和相應(yīng)的參數(shù)設(shè)置，為了能夠更為方便利用計(jì)算機(jī)仿真切現(xiàn)，特將此中參數(shù)變量進(jìn)行小結(jié)?？柭兞亢蛥?shù)小結(jié)變量定義維數(shù)時(shí)刻狀態(tài)時(shí)刻狀態(tài)值從時(shí)刻到時(shí)刻的轉(zhuǎn)移矩陣時(shí)刻的丈量矩陣過程噪聲的有關(guān)矩陣過程噪聲的有關(guān)矩陣給定觀察值在時(shí)刻狀態(tài)的展望預(yù)計(jì)給定觀察值在時(shí)刻狀態(tài)的濾波預(yù)計(jì)時(shí)刻卡爾曼增益矩陣時(shí)刻新息向量新息向量的有關(guān)矩陣中偏差有關(guān)矩陣中偏差有關(guān)矩陣鑒于單步

11、展望的卡爾曼濾波器的小結(jié)觀察值=轉(zhuǎn)移矩陣=丈量矩陣=過程噪聲的有關(guān)矩陣=丈量噪聲的有關(guān)矩陣=4Matlab仿真為了簡化，這里只議論簡單的一維單輸入單輸出線性系統(tǒng)模型，此中加入白噪聲作為系統(tǒng)的擾動，詳細(xì)仿真結(jié)果能夠獲取以下4.1維納最速降落法濾波器仿真結(jié)果以上為最速降落法中不一樣的遞歸步長所致使的追蹤成效變化，關(guān)于最速降落法中的步長是影響其算法穩(wěn)固的重點(diǎn)，最速降落算法穩(wěn)固的充分必需條件是條件步長因子為小于輸入自有關(guān)矩陣的最大特色值倒數(shù)的2倍。上邊的序列分別從有關(guān)矩陣的隨大特色之2倍的0.4倍開始變化至其1倍，最后一幅圖象能夠看出其已經(jīng)不再收斂，下邊是大于輸入有關(guān)矩陣的最大特色值2倍步長時(shí)所表現(xiàn)的

12、追蹤結(jié)果能夠看出其已經(jīng)顯然發(fā)散，不再是我們所希望的濾波算法。所以能夠總結(jié)出，對于最速降落法來說，步長的選用是很重要的，依據(jù)不一樣條件的需求，選用正確的步長，能為算法的迅速高效供給基礎(chǔ)。4.2卡爾曼濾波器仿真結(jié)果從圖中能夠發(fā)現(xiàn)，卡爾曼濾波器能夠特別有效地在比較大的擾亂下比較正確地反應(yīng)真切值，假如觀察端加入擾亂較大時(shí)，卡爾曼濾波器能夠較為有效地進(jìn)行濾波，可是當(dāng)狀態(tài)端的擾亂增大時(shí)，卡爾曼濾波器的濾波成效也會隨之降落。以下列圖，是加大了狀態(tài)端的擾亂，所體現(xiàn)的濾波成效。如上圖所示，狀態(tài)端的擾亂致使?fàn)顟B(tài)不穩(wěn)固，卡爾曼濾波器的預(yù)計(jì)值也出現(xiàn)了比較大的顛簸。假如將狀態(tài)端的擾亂再增大，則會出現(xiàn)更為嚴(yán)重的濾波考驗(yàn)，

13、濾波成效以下。這是的狀態(tài)已經(jīng)很牽強(qiáng)了，所以，研究更為有效的多方法卡爾曼濾波器也顯得十分必需了。4.3一種不需初始化的卡爾曼濾波器仿真這類濾波器不過實(shí)現(xiàn)了無需對部分變量進(jìn)行初始化的設(shè)計(jì)，沒有特別意義上的改良經(jīng)典卡爾曼濾波器自己性能的特色。仿真圖以下4.4后聯(lián)光滑濾波的卡爾曼濾波器仿真不過在經(jīng)典卡爾曼濾波器后端聯(lián)接了光滑濾波器，對性能改良的成效其實(shí)不特別顯然，仿真圖以下如圖中所表示，即便光滑過的估值與觀察值之間的差異也不是特別令人滿意，所以，關(guān)于經(jīng)典卡爾曼濾波的研究還需要更深一步進(jìn)行，因?yàn)闀r(shí)間和能力有限，本次的作業(yè)關(guān)于卡爾曼及其余濾波器的研究只好達(dá)到這類程度，希望在此后的學(xué)習(xí)中，能發(fā)現(xiàn)更好的對經(jīng)典

14、卡爾曼濾波器的改良方法。5Matlab源代碼(部分參照自互聯(lián)網(wǎng))5.1經(jīng)典卡爾曼濾波器clearN=200;w(1)=0;x(1)=5;a=1;c=1;Q1=randn(1,N)*1;%過程噪聲Q2=randn(1,N);%丈量噪聲fork=2:N;x(k)=a*x(k-1)+Q1(k-1);end%狀態(tài)矩陣fork=1:N;Y(k)=c*x(k)+Q2(k);endp(1)=10;s(1)=1;fort=2:N;Rww=cov(Q1(1:t);Rvv=cov(Q2(1:t);p1(t)=a.2*p(t-1)+Rww;b(t)=c*p1(t)/(c.2*p1(t)+Rvv);%kalman增益

15、s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1);p(t)=p1(t)-c*b(t)*p1(t);endt=1:N;plot(t,s,r,t,Y,g,t,x,b);%紅色卡爾曼，綠色觀察值，藍(lán)色狀態(tài)值legend(kalmanestimate,ovservations,truth);5.2最速降落法clcclearallN=30;q=2.1;%q1&q2/Ryx最大特色值hn=zeros(1,N);hn(:)=5;vg=0;Rxx=xcorr(1);Ryx=min(min(corrcoef(1,1+randn);echoofffori=1:N-1;%vg=2*Rxx*hn

16、(:,i)-2*Ryx;%hn(:,i+1)=hn(:,i)-1/2*q*vg;vg=2*Rxx*hn(i)-2*Ryx;hn(i+1)=hn(i)-1/2*q*vg;m(i)=1;endt=1:N-1;plot(t,hn(t),r-,t,m(t),b-);5.3后聯(lián)光滑濾波器的卡爾曼濾波器clearclc;N=300;CON=5;x=zeros(1,N);x(1)=1;p=10;Q=randn(1,N)*0.2;%過程噪聲協(xié)方差R=randn(1,N);%觀察噪聲協(xié)方差y=R+CON;%加過程噪聲的狀態(tài)輸出fork=2:NQ1=cov(Q(1:k-1);%過程噪聲協(xié)方差Q2=cov(R(1:

17、k-1);x(k)=x(k-1);%預(yù)預(yù)計(jì)k時(shí)刻狀態(tài)變量的值p=p+Q1;%對應(yīng)于預(yù)估值的協(xié)方差kg=p/(p+Q2);%kalmangainx(k)=x(k)+kg*(y(k)-x(k);p=(1-kg)*p;endFilter_Wid=10;smooth_res=zeros(1,N);kalman_p=zeros(1,N);fori=Filter_Wid+1:Ntempsum=0;kalman_m=0;forj=i-Filter_Wid:i-1tempsum=tempsum+y(j);kalman_m=kalman_m+x(j);endkalman_p(i)=kalman_m/Filter_Wid;smooth_res(i)=tempsum/Filter_Wid;%光滑濾波endfigure(1);hist(