______________________________________________________________________________________________________________精品資料Kalman濾波器的基本原理及仿真摘要：Kalman濾波是對線性最小均方誤差濾波的另一種處理方法，實(shí)際是維納濾波的一種遞推算法。它采用的遞推算法利用了前一時刻的估計值和新的觀測值，大大提高了處理的實(shí)時性，同時也能自動跟蹤隨機(jī)信號統(tǒng)計特性的非平穩(wěn)變化，對于解決很大部分的問題，他是最優(yōu)，效率最高甚至是最有用的，因此得到了廣泛的應(yīng)用。Kalman濾波的應(yīng)用包括機(jī)器人導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等等。近年來更被應(yīng)用于計算機(jī)圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。關(guān)鍵字：Kalman濾波線性最小均方誤差濾波估計值觀測值

一、Kalman濾波器的提出Kalman濾波器是源于匈牙利數(shù)學(xué)家RudolfEmilKalman的博士論文和1960年發(fā)表的論文《ANewApproachtoLinearFilteringandPredictionProblems》（線性濾波與預(yù)測問題的新方法）。在信號處理，通信和現(xiàn)代控制系統(tǒng)中，需要對一個隨機(jī)動態(tài)系統(tǒng)的狀態(tài)進(jìn)行估計，由一個測量裝置對系統(tǒng)狀態(tài)進(jìn)行測量，通過記錄的測量值對狀態(tài)進(jìn)行最優(yōu)估計例如：對一個一階AR模型的輸出狀態(tài)進(jìn)行估計。觀測方程是是測量引入的白噪聲，通過各的值估計。這類最優(yōu)估計問題稱為卡爾曼濾波。二、Kalman濾波器的基本思想利用觀測數(shù)據(jù)對狀態(tài)變量的預(yù)測估計進(jìn)行修正，以得到狀態(tài)變量的最優(yōu)估計，即最優(yōu)估計＝預(yù)測估計＋修正三、Kalman濾波器的特點(diǎn)（1）算法是遞推的，時域內(nèi)設(shè)計濾波器，適用于多維隨機(jī)過程的估計；（2）用遞推法計算，不需要知道全部過去的值。用狀態(tài)方程描述狀態(tài)變量的動態(tài)變化規(guī)律，因此，信號可以是平穩(wěn)的，也可以是非平穩(wěn)的；（3）誤差準(zhǔn)則仍為均方誤差最小準(zhǔn)則。四、有關(guān)Kalman濾波器的探索設(shè)我們要研究的對象是一個房間的溫度。根據(jù)經(jīng)驗(yàn)判斷，這個房間的溫度是恒定的，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度（假設(shè)我們用一分鐘來做時間單位）。假設(shè)你對你的經(jīng)驗(yàn)不是100%的相信，可能會有上下偏差幾度。我們把這些偏差看成是高斯白噪聲（WhiteGaussianNoise），也就是這些偏差跟前后時間是沒有關(guān)系的而且符合高斯分配（GaussianDistribution）。另外，我們在房間里放一個溫度計，但是這個溫度計也不準(zhǔn)確的，測量值會比實(shí)際值偏差。我們也把這些偏差看成是高斯白噪聲。

在對于某一分鐘我們有兩個有關(guān)于該房間的溫度值：你根據(jù)經(jīng)驗(yàn)的預(yù)測值（系統(tǒng)的預(yù)測值）和溫度計的值（測量值）。下面我們要用這兩個值結(jié)合他們各自的噪聲來估算出房間的實(shí)際溫度值。

現(xiàn)在要估算k時刻的是實(shí)際溫度值。首先你要根據(jù)k-1時刻的溫度值，來預(yù)測k時刻的溫度。因?yàn)槟阆嘈艤囟仁呛愣ǖ模阅銜玫絢時刻的溫度預(yù)測值是跟k-1時刻一樣的，假設(shè)是23度，同時該值的高斯噪聲的偏差是5度（5是這樣得到的：如果k-1時刻估算出的最優(yōu)溫度值的偏差是3，你對自己預(yù)測的不確定度是4度，他們平方相加再開方，就是5）。然后，你從溫度計那里得到了k時刻的溫度值，假設(shè)是25度，同時該值的偏差是4度。由于我們用于估算k時刻的實(shí)際溫度有兩個溫度值，分別是23度和25度。究竟實(shí)際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點(diǎn)，我們可以用他們的covariance來判斷。因?yàn)镵g^2=5^2/(5^2+4^2)，所以Kg=0.78，我們可以估算出k時刻的實(shí)際溫度值是：23+0.78*(25-23)=24.56度。可以看出，因?yàn)闇囟扔嫷腸ovariance比較?。ū容^相信溫度計），所以估算出的最優(yōu)溫度值偏向溫度計的值。

我們已經(jīng)得到k時刻的最優(yōu)溫度值了，下一步就是要進(jìn)入k+1時刻，進(jìn)行新的最優(yōu)估算。到現(xiàn)在為止，好像還沒看到什么自回歸的東西出現(xiàn)。對了，在進(jìn)入k+1時刻之前，我們還要算出k時刻那個最優(yōu)值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。這里的5就是上面的k時刻你預(yù)測的那個23度溫度值的偏差，得出的2.35就是進(jìn)入k+1時刻以后k時刻估算出的最優(yōu)溫度值的偏差（對應(yīng)于上面的3）。

就是這樣，卡爾曼濾波器就不斷的把covariance遞歸，從而估算出最優(yōu)的溫度值。他運(yùn)行的很快，而且它只保留了上一時刻的covariance。上面的Kg，就是卡爾曼增益（KalmanGain）。他可以隨不同的時刻而改變他自己的值五、Kalman濾波器的的基本理論5.1、標(biāo)量（一階遞歸模型）卡爾曼濾波器信號模型和測量模型為使卡爾曼濾波過程的物理意義明確，采用下列符號：（1）用代替，表示用n時刻及以前所有數(shù)據(jù)對所作的最佳線性估計。（2）用代替，表示用n-1時刻及以前所有數(shù)據(jù)對所作的最佳線性估計。圖一對于圖一所示的系統(tǒng)，他的迭代形式如下：這是Kalman濾波器的標(biāo)準(zhǔn)形式。5.1.1、物理意義一步預(yù)測：是利用n-1時刻及以前的數(shù)據(jù)對的預(yù)測。第二步預(yù)測：是對的最佳估計。新息（Innovation）:是對的預(yù)測誤差，代表中所含的無法預(yù)測的信息。選擇適當(dāng)?shù)南禂?shù)對新息進(jìn)行加權(quán)，作為對預(yù)測值的修正值，修正后得到對信號的最佳估計為：信號n時刻的最佳估計為n時刻的一步預(yù)測與新息的加權(quán)和。不同時間的最佳加權(quán)系數(shù)是不同的，相應(yīng)的均方誤差最小，即現(xiàn)在來求取最佳修正加權(quán)系數(shù)：濾波誤差及功率：令需使誤差和新息正交。設(shè)信號的一步預(yù)測誤差為：則：得到卡爾曼增益：上式表明：最佳加權(quán)系數(shù)，因預(yù)測越不準(zhǔn)確，利用進(jìn)行的修正就越多。一步預(yù)測誤差功率：一步預(yù)測誤差方程：圖二Kalman濾波的結(jié)構(gòu)框圖5.2、矢量Kalman濾波器實(shí)際應(yīng)用中常需要根據(jù)觀測數(shù)據(jù)同時估計若干個信號，或者估計一個高階自回歸過程，例：對象：q階AR過程：模型方程和量測方程：若同時估計若干個信號：信號矢量，噪聲矢量，參數(shù)矩陣如下：5.2.1矢量卡爾曼濾波器的計算公式濾波方程：預(yù)測誤差方程：增益方程：濾波誤差方程：六、卡爾曼濾波器算法流程：七、Kalman濾波器的matlab實(shí)現(xiàn)算法7.1、Kalman濾波器的設(shè)計離散系統(tǒng)：x［n+1］=Ax［n］+B(u［n］+w［n］)(7.1)y［n］=Cx［n］(7.2)其中,w［n］是在輸入端加入的高斯噪聲。狀態(tài)矩陣參數(shù)分別為A=［1.1269-0.49400.11291.00000001.00000］;B=［-0.38320.59190.5191］;C=［100］;我們的目標(biāo)是設(shè)計Kalman濾波器,在給定輸入u［n］和帶噪輸出測量值yv［n］=Cx［n］+v［n］的情況下估計系統(tǒng)的輸出。其中,v［n］是高斯白噪聲。1）離散Kalman濾波器上述問題的穩(wěn)態(tài)Kalman濾波器方程如下：測量值修正計算((7.4)(7.3)2）穩(wěn)態(tài)設(shè)計我們可以通過kalman函數(shù)設(shè)計上述穩(wěn)態(tài)濾波器。首先定義帶噪聲的系統(tǒng)模型：x［n+1］=Ax［n］+Bu［n］+Bw［n］(狀態(tài)方程)y［n］=Cx［n］(測量方程)具體的程序代碼如下：Plant=ss(A,［BB］,C,0,-1,′inputname′,{′u′′w′},′outputname′,′y′);假設(shè)Q=R=1,下面可以設(shè)計離散Kalman濾波器：Q=1;R=1;［kalmf,L,P,M］=kalman(Plant,Q,R);圖三Kalman濾波器濾波器的功能是在已知輸入噪聲方差的條件下盡可能消除輸出信號中的噪聲影響。圖四顯示了濾波前后的不同輸出信號。下面用程序來比較濾波后輸出信號與系統(tǒng)實(shí)際信號相對理想輸出的誤差。a=A;b=［BB0*B］;c=［C;C］;d=［000;001］;P=ss(a,b,c,d,-1,′inputname′,{′u′′w′′v′},′outputname′,{′y′′yv′});sys=parallel(P,kalmf,1,1,［］,［］)%創(chuàng)建并聯(lián)系統(tǒng)%將系統(tǒng)輸出yv正反饋到濾波器的輸入端,形成閉環(huán)系統(tǒng)SimModel=feedback(sys,1,4,2,1)%從I/O列表中刪除yvSimModel=SimModel(［13］,［123］)%產(chǎn)生高斯噪聲信號t=［0:100］′;u=sin(t/5);n=length(t)randn(′seed′,0)圖圖四濾波前后輸出信號的比較w=sqrt(Q)*randn(n,1);v=sqrt(R)*randn(n,1);%系統(tǒng)仿真［out,x］=lsim(SimModel,［w,v,u］);y=out(:,1);%系統(tǒng)真實(shí)（理想）輸出響應(yīng)ye=out(:,2);%濾波后的系統(tǒng)輸出yv=y+v;%系統(tǒng)輸出的測量值%比較結(jié)果subplot(211),plot(t,y,′--′,t,ye,′-′),xlabel(′No.ofsamples′),ylabel(′Output′)title(′Kalmanfilterresponse′)subplot(212),plot(t,y-yv,′-.′,t,y-ye,′-′),xlabel(′No.ofsamples′),ylabel(′Error′)比較結(jié)果如圖五所示。圖中上面的圖形顯示的是真實(shí)響應(yīng)y（虛線）和濾波后的輸出ye（實(shí)線），下面的圖形比較測量誤差（虛線）與估計誤差（實(shí)線）。該圖表明,濾波器最大程度地消除了系統(tǒng)輸出中的噪聲影響。這可以通過計算誤差的協(xié)方差進(jìn)行驗(yàn)證。MeasErr=y-yv;MeasErrCov=sum(MeasErr.*MeasErr)/length(MeasErr);EstErr=y-ye;EstErrCov=sum(EstErr.*EstErr)/length(EstErr);濾波前誤差（測量誤差）的協(xié)方差為MeasErrCovMeasErrCov=1.1138而濾波后的誤差（估計誤差）的協(xié)方差僅為EstErrCovEstErrCov=0.27227.2完整程序A=[1.1269-0.49400.11291.00000001.00000];B=[-0.38320.59190.5191];C=[100];Plant=ss(A,[BB],C,0,-1,'inputname',{'u''w'},'outputname','y');Q=1;R=1;[kalmf,L,P,M]=kalman(Plant,Q,R);kalmf=kalmf(1,:);a=A;b=[BB0*B];c=[C;C];d=[000;001];P=ss(a,b,c,d,-1,'inputname',{'u''w''v'},'outputname',{'y''yv'});sys=parallel(P,kalmf,1,1,[],[]);%Closelooparoundinput#4andoutput#2SimModel=feedback(sys,1,4,2,1);%DeleteyvfromI/OlistSimModel=SimModel([13],[123]);SimModelt=[0:100]';u=sin(t/5);n=length(t);randn('seed',0);w=sqrt(Q)*randn(n,1);v=sqrt(R)*randn(n,1);[out,x]=lsim(SimModel,[w,v,u]);y=out(:,1);%trueresponseye=out(:,2);%filteredresponseyv=y+v;%measuredresponsefigure(1);plot(t,y,'--',t,ye,'-'),xlabel('No.ofsamples'),ylabel('Output')title('Kalmanfilterresponse')figure(2);plot(t,(y-yv)/y,'--',t,(y-ye)/y,'-'),xlabel('No.ofsamples'),ylabel('Error')MeasEr