授之以漁：卡爾曼濾波器 大瀉蜜 原創(chuàng)作者：highgear（一）一片綠油油的草地上有一條曲折的小徑，通向一棵大樹。一個要求被提出：從起點沿著小徑走到樹下?！昂芎唵??！盇說，于是他絲毫不差地沿著小徑走到了樹下?，F(xiàn)在，難度被增加了：蒙上眼?！耙膊浑y，我當過特種兵?！盉說，于是他歪歪扭扭地走到了樹 旁?！鞍Γ镁貌痪?生疏了?！薄翱次业?，我有DIY的GPS!”C說，于是他像個醉漢似地走到了樹 旁?！鞍?，這個GPS軟件沒做好，漂移太大。”“我來試試?！迸赃呉蝗四眠^GPS,蒙上眼，居然沿著小徑走到了樹下。“這么厲害!你是什么人?”“卡爾曼!”“卡爾曼？!你是卡爾曼？”眾人大吃一驚?！拔沂钦f這個GPS卡而慢。”這段時間研究了一下卡爾曼濾波器，有一些心得，寫出來與大家分享?？柭鼮V波器與我以前講過的FIR,IIR濾波器完全不一樣，與其說屬于濾波器，不如說是屬于最優(yōu)控制的范疇。下面的內(nèi)容涉及相當多的控制理論知識，對于在這方面不足的同學(xué)可能有些吃力。不過不要緊，大家關(guān)注結(jié)果，會應(yīng)用就夠了,那些晦澀的理論和推導(dǎo)可以忽略。我也會用圖片讓大家更直觀的理解卡爾曼濾波器。三)首先回顧一下傳統(tǒng)數(shù)字濾波器。對于一個線性時不變系統(tǒng)，施加一個輸入u(t)，我們可以得到一個輸出y(t).如果輸入是一個沖擊，則輸出y(t)被稱作沖擊響應(yīng)，用h(t)來表示，是系統(tǒng)的內(nèi)核。對于任意u(t),輸出y(t)可以通過u(t)與沖擊響應(yīng)h(t)的卷積得到，這是FIR濾波器的基本原理。我們還可以通過系統(tǒng)微分方程轉(zhuǎn)換為差分方程，或是通過laplace傳遞函數(shù)轉(zhuǎn)換到差分方程，最后得到一個遞推公式，這種形式的濾波器就是IIR濾波器。以前講過，一個系統(tǒng)可以用時域的微分方程來建立，然后可以用laplace的傳遞函數(shù)來處理，把解微分方程變?yōu)槎囗検匠朔?，可以簡單的求解。還有另外一種處理形式就是狀態(tài)空間，以矩陣形式來處理微分方程或微分方程組，利用矩陣變換求解，類同齊次方程組的矩陣形式。例如微分方程:y''+3y'+2y=u讓 X1=y, X2=y'=X1',則上式變?yōu)閄2'=-3X2—2XI-uX1'=X2矩陣形式為：通用形式為：X'=A*X+B*uY=C*X.可以看到，可以很輕易的微分方程或微分方程組轉(zhuǎn)換到狀態(tài)空間形式，而狀態(tài)空間與laplace傳遞函數(shù)之間可以相互轉(zhuǎn)換，事實上矩陣A的特征值就是s傳遞函數(shù)的極點。系統(tǒng)的傳遞函數(shù)(陣)可以通過矩陣變換得到：Y(s)=C*(s*I-A)-1*B同理，連續(xù)域的微分方程對應(yīng)了離散域的差分方程，s對應(yīng)了z,離散域狀態(tài)空間相應(yīng)的變?yōu)椋篨(k)=A*X(k-1)+B(u-1)Y(k)=C*X(k)（四）我們現(xiàn)在來看看蒙眼走小徑的走法問題。假設(shè)A走過的路徑是真真正的路徑，為Za;B是用自己的大腦作為預(yù)測估計器，走出了一個預(yù)測路徑，為Zb;C用測量器，走出了一條測量路徑，為Zc。用圖片來說明：UTaaz—_預(yù)測估計UTaaz—_預(yù)測估計測量輸出系統(tǒng)真實輸出系統(tǒng)真實輸出”是A走過的路徑：Za=C*X

“測量輸出”是Zb. Zb=Za+V這里V是噪聲，即GPS的漂移；“預(yù)測估計輸出”是Zc=C*X"XA是預(yù)測的狀態(tài)。T是采樣延時?，F(xiàn)在，蒙上眼的情況下有兩種選擇，GPS或大腦預(yù)測估計器。如果GPS很準而預(yù)測不準，那么

可以選擇GPS；如果預(yù)測準確而GPS不準，那么選擇預(yù)測估計器，等等，沒有反饋的預(yù)測估計器會因為累積

誤差而導(dǎo)致越來越不準。如果兩個都不準，該如何取舍？如何把兩者結(jié)合在一起呢？我們可以設(shè)置一個信心指數(shù)K，K在0與1之間，來說明對測量值還是預(yù)測值的信任程度：

(1)Z=K*Zb+(1—K)*Zc=Zc+K*(Zb—Zc)(1)可以看出，當K=1和0時，分別選擇了GPS或預(yù)測估計器.現(xiàn)在，可以把誤差Zb-Zc作

為反饋誤差，來修正預(yù)測估計器的結(jié)果。新的系統(tǒng)結(jié)構(gòu)圖如下：這個框圖，就是卡爾曼濾波器的基本構(gòu)造。學(xué)過現(xiàn)代控制理論的同學(xué)都這個圖應(yīng)該很熟悉，與狀態(tài)變量估計控制的圖形差不多，只是其中的K=1而且沒有噪聲項和系統(tǒng)反饋而已。而我們下面的任務(wù)，就是如何確定這個K值。以下略去三百字的方差，與協(xié)方差的介紹.自己看吧/wiki/%E5%8D%8F%E6%96%B9%E5%B7%AE以下略去五百字的kalmanFilterGainK的推導(dǎo)。自己看吧：/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2關(guān)于卡爾曼濾波器的推導(dǎo)過程，枯燥晦澀，我就略過，直接關(guān)注結(jié)果。五)計算過程：卡爾曼濾波是一種遞歸的估計，即只要獲知上一時刻狀態(tài)的估計值以及當前狀態(tài)的觀測值就可以計算出當前狀態(tài)的估計值，因此不需要記錄觀測或者估計的歷史信息?？柭鼮V波器的遞歸過程：估計時刻k的狀態(tài)：X(k)=A*X(k-1)+B*u(k)這里,u(k)是系統(tǒng)輸入計算誤差相關(guān)矩陣P,度量估計值的精確程度：P(k)=A*P(k-1)*A'+Q這里，Q=E{Wj2}是系統(tǒng)噪聲的協(xié)方差陣，即系統(tǒng)框圖中的Wj的協(xié)方差陣,Q應(yīng)該是不斷變化的，為了簡化，當作一個常數(shù)矩陣。計算卡爾曼增益,以下略去(k),即P=P(k),X=X(k)：K=P*C'*(C*P*C'+R)-1這里R=E{VjT},是測量噪聲的協(xié)方差(陣)，即系統(tǒng)框圖中的Vj的協(xié)方差，為了簡化，也當作一個常數(shù)矩陣。由于我們的系統(tǒng)一般是單輸入單輸出，所以R是一個1x1的矩陣，即一個常數(shù)，上面的公式可以簡化為:K=P*C'/(C*P*C'+R)狀態(tài)變量反饋的誤差量：e=Z(k)—C*X(k)這里的Z(k)是帶噪聲的測量量更新誤差相關(guān)矩陣PP=P—K*C*P更新狀態(tài)變量：X=X+K*e=X+K*(Z(k)—C*X(k))最后的輸出：Y=C*X現(xiàn)在的問題就是如何實現(xiàn)卡爾曼濾波，A，B，C，Q，R這些矩陣或量如何確定？六)仿真實例下面用仿真實例來觀察卡爾曼濾波器的效果。假設(shè)我們的系統(tǒng)是一個加熱系統(tǒng)，熱時間常數(shù)為60秒，100度時達到熱平衡。忽略系統(tǒng)的延遲，那么當系統(tǒng)加電后，溫度由0開始上升。這個上

升過程大家應(yīng)該很熟悉，這是一個指數(shù)函數(shù)：y(t)=100*(1-e-t/6p其laplace傳遞函數(shù)為:y(s)=100/(60*s-1)我們?nèi)藶榈募尤肓穗S機噪聲來模擬測量噪聲我們假定并不知道系統(tǒng)的傳遞函數(shù)，現(xiàn)在只是簡單的,隨便地構(gòu)造了一個預(yù)測系統(tǒng)。A=[1,0;0,1]B=[1;0]C=[1,0]

測量噪聲的協(xié)方差R=40,此為猜測值；系統(tǒng)噪聲Q=2,也是猜測值，預(yù)測模型越不準，Q值

應(yīng)越大?？柭鼮V波器的結(jié)果，紅色為濾波器輸出：可以看到，盡管我們使用了一個粗劣的預(yù)測估計器，Kalman濾波器還是相當?shù)钠?，基本上?/p>

除了噪聲.如果我們有一個相當精確的模型，結(jié)果會怎么樣呢？要建立一個精確的預(yù)測估計模型，我們還是要利用方差。如果一個估計的曲線與實際曲線完全重

合時，他們的方差為0.方差越小，擬合度越高，最小二乘法的原理便是如此。具體推導(dǎo)過程

還是省略，直接給出matlab的擬合程序，這是一個非常非常有用的程序。如果數(shù)學(xué)模型很精確，能不能直接數(shù)學(xué)模型的輸出作為濾波器的結(jié)果呢？不能，因為沒有反饋，數(shù)學(xué)模型的輸出會因為沒有反饋的校正造成誤差不斷累積，失之毫厘，謬之千里。下面是用最小二乘法獲得系統(tǒng)的模型并做為預(yù)測估計器，設(shè)定為3階系統(tǒng)，得出的數(shù)

學(xué)模型相當準確，所以Q值可以取一個小值，

這里Q=0.02,現(xiàn)在看看卡爾曼濾波器的結(jié)果：效果非常好，卡爾曼濾波器的輸出與實際系統(tǒng)的輸出（即無噪聲的系統(tǒng)輸出）幾乎重合

這是精確的預(yù)測估計模型帶來的好處?，F(xiàn)在比較兩個例子中卡爾曼增益的不同最小二乘法獲得系統(tǒng)的模型中的增益迅速地由大變小，最后小于不準確模型。K值較小,意味著誤差反饋量較小，使得預(yù)測輸出更