卡爾曼濾波器及matlab代碼

歐陽學(xué)文信息融合大作業(yè)——維納最速下降法濾波器，卡爾曼濾波器設(shè)計及Matlab仿真時間：126專業(yè)：信息工程班級：09030702學(xué)號：302171姓名：馬志強濾波問題淺談預(yù)計器或濾波器這一術(shù)語普通用來稱呼一種系統(tǒng)，設(shè)計這樣的系統(tǒng)是為了從含有噪聲的數(shù)據(jù)中提取人們感愛好的，靠近規(guī)定質(zhì)量的信息。由于這樣一種寬目的，預(yù)計理論應(yīng)用于諸如通信、雷達、聲納、導(dǎo)航、地震學(xué)、生物醫(yī)學(xué)工程、金融工程等眾多不同的領(lǐng)域。例如，考慮一種數(shù)字通信系統(tǒng)，其基本形式由發(fā)射機、信道和接受機連接構(gòu)成。發(fā)射機的作用是把數(shù)字源(例如計算機)產(chǎn)生的0、1符號序列構(gòu)成的消息信號變換成為適合于信道上傳送的波形。而由于符號間干擾和噪聲的存在，信道輸出端收到的信號是含有噪聲的或失真的發(fā)送信號。接受機的作用是，操作接受信號并把原消息信號的一種可靠估值傳遞給系統(tǒng)輸出端的某個顧客。隨著通信系統(tǒng)復(fù)雜度的提高，對原消息信號的還原成為通信系統(tǒng)中最為重要的環(huán)節(jié)，而噪聲是接受端需要排除的最重要的干擾，人們也設(shè)計出了針對多個不同條件應(yīng)用的濾波器，其中最速下降算法是一種古老的最優(yōu)化技術(shù)，而卡爾曼濾波器隨著應(yīng)用條件的精簡成為了普適性的高效濾波器。2．維納最速下降算法濾波器2.1最速下降算法的基本思想考慮一種代價函數(shù)，它是某個未知向量的持續(xù)可微分函數(shù)。函數(shù)將的元素映射為實數(shù)。這里，我們要尋找一種最優(yōu)解。使它滿足以下條件 (2.1)這也是無約束最優(yōu)化的數(shù)學(xué)表達。特別適合于自適應(yīng)濾波的一類無約束最優(yōu)化算法基于局部迭代下降的算法：從某一初始猜想出發(fā)，產(chǎn)生一系列權(quán)向量，使得代價函數(shù)在算法的每一次迭代都是下降的，即其中是權(quán)向量的過去值，而是其更新值。我們但愿算法最后收斂到最優(yōu)值。迭代下降的一種簡樸形式是最速下降法，該辦法是沿最速下降方向持續(xù)調(diào)節(jié)權(quán)向量。為方便起見，我們將梯度向量表達為(2.2)因此，最速下降法能夠表達為(2.3)其中代表進程，是正常數(shù)，稱為步長參數(shù)，1/2因子的引入是為了數(shù)學(xué)上解決方便。在從到的迭代中，權(quán)向量的調(diào)節(jié)量為(2.4)為了證明最速下降算法滿足式(2.1)，在處進行一階泰勒展開，得到(2.5)此式對于較小時是成立的。在式(2.4)中設(shè)為負值向量，因而梯度向量也為負值向量，因此使用埃爾米特轉(zhuǎn)置。將式(2.4)用到式(2.5)中，得到此式表明當為正數(shù)時，。因此，隨著的增加，代價函數(shù)減小，當時，代價函數(shù)趨于最小值。2.2最速下降算法應(yīng)用于維納濾波器考慮一種橫向濾波器，其抽頭輸入為，對應(yīng)的抽頭權(quán)值為。抽頭輸入是來自零均值、有關(guān)矩陣為的廣義平穩(wěn)隨機過程的抽樣值。除了這些輸入外，濾波器還要一種盼望響應(yīng)，方便為最優(yōu)濾波提供一種參考。在時刻抽頭輸入向量表達為，濾波器輸出端盼望響應(yīng)的預(yù)計值為,其中是由抽頭輸所張成的空間?？者^比較盼望響應(yīng)及其預(yù)計值，能夠得到一種預(yù)計誤差，即(2.6)這里是抽頭權(quán)向量與抽頭輸入向量的內(nèi)積。能夠進一步表達為同樣，抽頭輸入向量可表達為 如果抽頭輸入向量和盼望響應(yīng)是聯(lián)合平穩(wěn)的，此時均方誤差或者在時刻的代價函數(shù)是抽頭權(quán)向量的二次函數(shù)，于是能夠得到(2.7)其中，為目的函數(shù)的方差，抽頭輸入向量與盼望響應(yīng)的互有關(guān)向量，及為抽頭輸入向量的有關(guān)矩陣。從而梯度向量能夠?qū)憺?2.8)其中在列向量中和分別是代價函數(shù)對應(yīng)第個抽頭權(quán)值的實部和虛部的偏導(dǎo)數(shù)。對最速下降算法應(yīng)用而言，假設(shè)式(2.8)中有關(guān)矩陣和互有關(guān)向量已知，則對于給定的抽頭權(quán)向量為(2.9)它描述了為那濾波中最速下降法的數(shù)學(xué)體現(xiàn)式。3.卡爾曼濾波器3.1卡爾曼濾波器的基本思想卡爾曼濾波器是用狀態(tài)空間概念描述其數(shù)學(xué)公式的，另外新穎的特點是，他的解遞歸運算，能夠不加修改地應(yīng)用于平穩(wěn)和非平穩(wěn)環(huán)境。特別是，其狀態(tài)的每一次更新預(yù)計都由前一次預(yù)計和新的輸入數(shù)據(jù)計算得到，因此只需存儲前一次預(yù)計。除了不需要存儲過去的全部觀察數(shù)據(jù)外，卡爾曼濾波計算比直接根據(jù)濾波過程中每一步全部過去數(shù)據(jù)進行估值的辦法都更加有效。++++圖3.1線性動態(tài)離散時間系統(tǒng)的信號流圖表達“狀態(tài)”的概念是這種表達的基礎(chǔ)。狀態(tài)向量，簡樸地說狀態(tài)，定義為數(shù)據(jù)的最小集合，這組數(shù)據(jù)足以唯一地描述系統(tǒng)的自然動態(tài)行為。換句話說，狀態(tài)由預(yù)測系統(tǒng)將來特性時所素要的，與系統(tǒng)的過去行為有關(guān)的最少的數(shù)據(jù)構(gòu)成。典型地，比較有代表性的狀況是，狀態(tài)是未知的。為了預(yù)計它，我們使用一組觀察數(shù)據(jù)，在途中用向量表達。成為觀察向量或者簡稱觀察值，并假設(shè)它是維的。在數(shù)學(xué)上，圖3.1表達的信號流圖隱含著一下兩個方程：過程方程(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ù)據(jù)過去值給定的狀況下，你時刻觀察數(shù)據(jù)的最小均方預(yù)計。過去的值用觀察值表達，他們張成的向量空間用表達。從而能夠定義新息過程以下：(3.4)其中向量表達觀察數(shù)據(jù)的新息。3.3應(yīng)用新息過程進行狀態(tài)預(yù)計下面，我們根據(jù)信息過程導(dǎo)出狀態(tài)的最小均方預(yù)計。根據(jù)推導(dǎo)，這個預(yù)計能夠表達成為新息過程序列的線性組合，即 (3.5)其中是一組待定的矩陣。根據(jù)正交性原理，預(yù)測狀態(tài)誤差向量與新息過程正交，即(3.6)將式(3.5)代入式(3.6)，并運用新息過程的正交性質(zhì)，即得(3.7)因此，式(3.7)兩邊同時右乘逆矩陣，可得的體現(xiàn)式為 (3.8)最后，將式(3.8)帶入式(3.5)，可得最小軍方差預(yù)計 (3.9)故對于，有 (3.10)然而，時刻的狀態(tài)與時刻的狀態(tài)的關(guān)系式由式能夠推導(dǎo)出對于，有 (3.11)其中只與觀察數(shù)據(jù)有關(guān)。因此可知，與彼此正交(其中)。運用式(3.11)以及當時的計算公式，可將式(3.10)右邊的求和項改寫為 (3.12)為了進一步討論，引入以下基本定義。3.4卡爾曼增益定義矩陣 (3.13)其中是狀態(tài)向量和新息過程的互有關(guān)矩陣。運用這一定義和式(3.12)的成果，能夠?qū)⑹?3.10)簡樸重寫為 (3.14)式(3.14)含有明確的物理意義。它標明：線性動態(tài)系統(tǒng)狀態(tài)的最小均方預(yù)計能夠由前一種預(yù)計求得。為了表達對卡爾曼開創(chuàng)性奉獻的承認，將矩陣稱為卡爾曼增益?，F(xiàn)在剩余唯一要解決的問題是，如何以一種便于計算的形式來表達卡爾曼增益。為此，首先將與乘積的盼望表達為 (3.15)式中運用了狀態(tài)與噪聲向量互不有關(guān)這一事實。另首先，由于預(yù)測狀態(tài)誤差向量與預(yù)計正交，因此與乘機的盼望為零。這樣，用預(yù)測狀態(tài)誤差向量替代相乘因子，將不會引發(fā)式(3.15)變化，故有 (3.16)由此，可將上式進一步變化為 (3.17)現(xiàn)在我們重新定義卡爾曼增益。為此，將式(3.17)代入式(3.13)得 (3.18)現(xiàn)在我們已經(jīng)理解了卡爾曼濾波的整個過程和對應(yīng)的參數(shù)設(shè)立，為了能夠更為方便運用計算機仿真實現(xiàn)，特將其中參數(shù)變量進行小結(jié)?？柭兞亢蛥?shù)小結(jié)變量定義維數(shù)時刻狀態(tài)時刻狀態(tài)值從時刻屆時刻的轉(zhuǎn)移矩陣時刻的測量矩陣過程噪聲的有關(guān)矩陣過程噪聲的有關(guān)矩陣給定觀察值在時刻狀態(tài)的預(yù)測預(yù)計給定觀察值在時刻狀態(tài)的濾波預(yù)計時刻卡爾曼增益矩陣時刻新息向量新息向量的有關(guān)矩陣中誤差有關(guān)矩陣中誤差有關(guān)矩陣基于單步預(yù)測的卡爾曼濾波器的小結(jié)觀察值=轉(zhuǎn)移矩陣=測量矩陣=過程噪聲的有關(guān)矩陣=測量噪聲的有關(guān)矩陣=4Matlab仿真為了簡化，這里只討論簡樸的一維單輸入—單輸出線性系統(tǒng)模型，其中加入白噪聲作為系統(tǒng)的擾動，具體仿真成果能夠獲得以下4.1維納最速下降法濾波器仿真成果以上為最速下降法中不同的遞歸步長所造成的跟蹤效果變化，對于最速下降法中的步長是影響其算法穩(wěn)定的核心，最速下降算法穩(wěn)定的充足必要條件是條件步長因子為不大于輸入自有關(guān)矩陣的最大特性值倒數(shù)的2倍。上面的序列分別從有關(guān)矩陣的隨大特性之2倍的0.4倍開始變化至其1倍，最后一幅圖象能夠看出其已經(jīng)不再收斂，下面是不不大于輸入有關(guān)矩陣的最大特性值2倍步長時所體現(xiàn)的跟蹤成果能夠看出其已經(jīng)明顯發(fā)散，不再是我們所盼望的濾波算法。因此能夠總結(jié)出，對于最速下降法來說，步長的選用是很重要的，根據(jù)不同條件的需求，選用對的的步長，能為算法的快速高效提供基礎(chǔ)。4.2卡爾曼濾波器仿真成果從圖中能夠發(fā)現(xiàn)，卡爾曼濾波器能夠非常有效地在比較大的干擾下比較精確地反映真實值，如果觀察端加入干擾較大時，卡爾曼濾波器能夠較為有效地進行濾波，但是當狀態(tài)端的干擾增大時，卡爾曼濾波器的濾波效果也會隨之下降。以下圖，是加大了狀態(tài)端的干擾，所呈現(xiàn)的濾波效果。如上圖所示，狀態(tài)端的干擾造成狀態(tài)不穩(wěn)定，卡爾曼濾波器的預(yù)計值也出現(xiàn)了比較大的波動。如果將狀態(tài)端的干擾再增大，則會出現(xiàn)更為嚴峻的濾波考驗，濾波效果以下。這是的狀態(tài)已經(jīng)很勉強了，因此，研究更為有效的多辦法卡爾曼濾波器也顯得十分必要了。4.3一種不需初始化的卡爾曼濾波器仿真這種濾波器只是實現(xiàn)了無需對部分變量進行初始化的設(shè)計，沒有特別意義上的改善典型卡爾曼濾波器本身性能的特點。仿真圖以下4.4后聯(lián)平滑濾波的卡爾曼濾波器仿真只是在典型卡爾曼濾波器后端聯(lián)接了平滑濾波器，對性能改善的效果并不特別明顯，仿真圖以下如圖中所示，即使平滑過的估值與觀察值之間的差別也不是特別令人滿意，因此，對于典型卡爾曼濾波的研究還需要更深一步進行，由于時間和能力有限，本次的作業(yè)對于卡爾曼及其它濾波器的研究只能達成這種程度，但愿在后來的學(xué)習(xí)中，能發(fā)現(xiàn)更加好的對典型卡爾曼濾波器的改善辦法。5Matlab源代碼(部分參考自互聯(lián)網(wǎng))5.1典型卡爾曼濾波器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(k1)+Q1(k1);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(t1)+Rww;b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);%kalman增益s(t)=a*s(t1)+b(t)*(Y(t)a*c*s(t1));p(t)=p1(t)c*b(t)*p1(t);endt=1:N;plot(t,s,'r',t,Y,'g',t,x,'b');%紅色卡爾曼，綠色觀察值，藍色狀態(tài)值legend('kalmanestimate','ovservations','truth');5.2最速下降法clcclearallN=30;q=2.1;%q>1&&q<2/Ryx最大特性值hn=zeros(1,N);hn(:)=5;vg=0;Rxx=xcorr(1);Ryx=min(min(corrcoef(1,1+randn)));echoofffori=1:N1;%vg=2*Rxx*hn(:,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:N1;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:k1));%過程噪聲協(xié)方差Q2=cov(R(1:k1));x(k)=x(k1);%預(yù)預(yù)計k時刻狀態(tài)變量的值p=p+Q1;%對應(yīng)于預(yù)估值的協(xié)方差kg=p/(p+Q2);%kalmangainx(k)=x(k)+kg*(y(k)x(k));p=(1kg)*p;endFilter_Wid=10;smooth_res=zeros(1,N);kalman_p=zeros(1,N);fori=Filter_Wid+1:Ntempsum=0;kalman_m=0;forj=iFilter_Wid:i1tempsum=tempsum+y(j);kalman_m=kalman_m+x(j);endkalman_p(i)=kalman_m/Filter_Wid;smooth_res(i)=tempsum/Filter_Wid;%平滑濾波end%figure(1);%hist(y);t=1:N;figure(1);expValue=zeros