卡爾曼濾波器大泄密

1、授之以漁： 卡爾曼濾波器 .大瀉蜜 .原創(chuàng)作者：highgear（一）一片綠油油的草地上有一條曲折的小徑，通向一棵大樹。一個(gè)要求被提出：從起點(diǎn)沿著小徑走到樹下?！昂芎?jiǎn)單。” A說(shuō)，于是他絲毫不差地沿著小徑走到了樹下。現(xiàn)在，難度被增加了：蒙上眼?！耙膊浑y，我當(dāng)過(guò)特種兵?！?B說(shuō)，于是他歪歪扭扭地走到了樹 . 旁。“唉，好久不練，生疏了?！薄翱次业?，我有 DIY 的 GPS！” C說(shuō)，于是他像個(gè)醉漢似地走到了樹. 旁?！鞍?，這個(gè) GPS 軟件沒(méi)做好，漂移太大?！薄拔襾?lái)試試?！?旁邊一人拿過(guò) GPS,蒙上眼，居然沿著小徑走到了樹下?！斑@么厲害！你是什么人?”“卡爾曼 ! ”“卡爾曼？！你是卡爾曼？”

2、眾人大吃一驚?！拔沂钦f(shuō)這個(gè) GPS 卡而慢?！保ǘ┻@段時(shí)間研究了一下卡爾曼濾波器，有一些心得，寫出來(lái)與大家分享。卡爾曼濾波器與我以前講過(guò)的FIR, IIR 濾波器完全不一樣，與其說(shuō)屬于濾波器，不如說(shuō)是屬于最優(yōu)控制的范疇。下面的內(nèi)容涉及相當(dāng)多的控制理論知識(shí)，對(duì)于在這方面不足的同學(xué)可能有些吃力。不過(guò)不要緊，大家關(guān)注結(jié)果，會(huì)應(yīng)用就夠了,那些晦澀的理論和推導(dǎo)可以忽略。我也會(huì)用圖片讓大家更直觀的理解卡爾曼濾波器。（三）首先回顧一下傳統(tǒng)數(shù)字濾波器。對(duì)于一個(gè)線性時(shí)不變系統(tǒng)，施加一個(gè)輸入 u(t) ，我們可以得到一個(gè)輸出 y(t) . 如果輸入是一個(gè)沖擊，則輸出y(t) 被稱作沖擊響應(yīng)，用 h(t) 來(lái)表示

3、，是系統(tǒng)的內(nèi)核。對(duì)于任意 u(t), 輸出 y(t) 可以通過(guò) u(t) 與沖擊響應(yīng) h(t) 的卷積得到，這是 FIR 濾波器的基本原理。我們還可以通過(guò)系統(tǒng)微分方程轉(zhuǎn)換為差分方程，或是通過(guò) laplace 傳遞函數(shù)轉(zhuǎn)換到差分方程，最后得到一個(gè)遞推公式，這種形式的濾波器就是IIR 濾波器。以前講過(guò)，一個(gè)系統(tǒng)可以用時(shí)域的微分方程來(lái)建立，然后可以用 laplace 的傳遞函數(shù)來(lái)處理，把解微分方程變?yōu)槎囗?xiàng)式乘法，可以簡(jiǎn)單的求解。還有另外一種處理形式就是狀態(tài)空間，以矩陣形式來(lái)處理微分方程或微分方程組，利用矩陣變換求解，類同齊次方程組的矩陣形式。例如微分方程: y +3y + 2y = u讓 X1 =

4、y, X2 = y = X1, 則上式變?yōu)椋?X2 = -3 X2 2 X1- u X1 = X2矩陣形式為：下載 (3.04 KB)2011-12-7 21:35通用形式為： X = A*X + B*u Y = C*X.可以看到，可以很輕易的微分方程或微分方程組轉(zhuǎn)換到狀態(tài)空間形式，而狀態(tài)空間與laplace 傳遞函數(shù)之間可以相互轉(zhuǎn)換，事實(shí)上矩陣A 的特征值就是s傳遞函數(shù)的極點(diǎn)。系統(tǒng)的傳遞函數(shù)（陣）可以通過(guò)矩陣變換得到： Y(s) = C * (s * I - A) -1 * B同理，連續(xù)域的微分方程對(duì)應(yīng)了離散域的差分方程，s 對(duì)應(yīng)了z, 離散域狀態(tài)空間相應(yīng)的變?yōu)椋?X(k) = A*X(k-

5、1) + B(u-1) Y(k) = C*X(k)（四）我們現(xiàn)在來(lái)看看蒙眼走小徑的走法問(wèn)題。假設(shè)A 走過(guò)的路徑是真真正的路徑，為Za; B是用自己的大腦作為預(yù)測(cè)估計(jì)器，走出了一個(gè)預(yù)測(cè)路徑，為 Zb; C 用測(cè)量器，走出了一條測(cè)量路徑，為Zc。用圖片來(lái)說(shuō)明：下載 (10.92 KB)2011-12-7 21:47“系統(tǒng)真實(shí)輸出”是 A 走過(guò)的路徑： Za = C * X；“測(cè)量輸出”是Zb. Zb = Za + V，這里 V 是噪聲，即GPS 的漂移；“預(yù)測(cè)估計(jì)輸出”是 Zc = C * X,X是預(yù)測(cè)的狀態(tài)。T 是采樣延時(shí)?，F(xiàn)在，蒙上眼的情況下有兩種選擇，GPS 或大腦預(yù)測(cè)估計(jì)器。如果GPS很準(zhǔn)

6、而預(yù)測(cè)不準(zhǔn)，那么可以選擇GPS；如果預(yù)測(cè)準(zhǔn)確而GPS不準(zhǔn)，那么選擇預(yù)測(cè)估計(jì)器， 等等， 沒(méi)有反饋的預(yù)測(cè)估計(jì)器會(huì)因?yàn)槔鄯e誤差而導(dǎo)致越來(lái)越不準(zhǔn)。如果兩個(gè)都不準(zhǔn)，該如何取舍？如何把兩者結(jié)合在一起呢？ 我們可以設(shè)置一個(gè)信心指數(shù) K，K 在 0 與1之間，來(lái)說(shuō)明對(duì)測(cè)量值還是預(yù)測(cè)值的信任程度： Z = K * Zb + (1 K) * Zc= Zc + K*(Zb Zc) (1)可以看出， 當(dāng) K = 1 和 0 時(shí)，分別選擇了GPS 或預(yù)測(cè)估計(jì)器. 現(xiàn)在，可以把誤差 Zb -Zc 作為反饋誤差，來(lái)修正 預(yù)測(cè)估計(jì)器的結(jié)果。新的系統(tǒng)結(jié)構(gòu)圖如下：下載 (13.48 KB)2011-12-7 21:47這個(gè)框圖

7、，就是卡爾曼濾波器的基本構(gòu)造。學(xué)過(guò)現(xiàn)代控制理論的同學(xué)都這個(gè)圖應(yīng)該很熟悉，與狀態(tài)變量估計(jì)控制的圖形差不多，只是其中的 K = 1 而且沒(méi)有噪聲項(xiàng)和系統(tǒng)反饋而已。而我們下面的任務(wù)，就是如何確定這個(gè)K 值。.以下略去三百字的方差，與協(xié)方差的介紹.自己看吧： /wiki/%E5%8D%8F%E6%96%B9%E5%B7%AE.以下略去 五百字的kalman Filter Gain K 的推導(dǎo)。自己看吧： /wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2關(guān)于卡爾曼

8、濾波器的推導(dǎo)過(guò)程，枯燥晦澀，我就略過(guò)，直接關(guān)注結(jié)果。（五）計(jì)算過(guò)程：卡爾曼濾波是一種遞歸的估計(jì)，即只要獲知上一時(shí)刻狀態(tài)的估計(jì)值以及當(dāng)前狀態(tài)的觀測(cè)值就可以計(jì)算出當(dāng)前狀態(tài)的估計(jì)值，因此不需要記錄觀測(cè)或者估計(jì)的歷史信息?？柭鼮V波器的遞歸過(guò)程：1) 估計(jì)時(shí)刻k 的狀態(tài)： X(k) = A*X(k-1) + B*u(k) 這里, u(k) 是系統(tǒng)輸入2) 計(jì)算誤差相關(guān)矩陣P, 度量估計(jì)值的精確程度： P(k) = A*P(k-1)*A+ Q 這里, Q = E Wj2 是系統(tǒng)噪聲的協(xié)方差陣，即系統(tǒng)框圖中的Wj的協(xié)方差陣, Q 應(yīng)該是不斷變化的，為了簡(jiǎn)化，當(dāng)作一個(gè)常數(shù)矩陣。3) 計(jì)算卡爾曼增益, 以下略

9、去 (k),即 P = P(k), X = X(k)： K = P *C * (C * P * C + R) -1 這里 R = E Vj2 , 是測(cè)量噪聲的協(xié)方差(陣),即系統(tǒng)框圖中的 Vj 的協(xié)方差, 為了簡(jiǎn)化，也當(dāng)作一個(gè)常數(shù)矩陣。由于我們的系統(tǒng)一般是單輸入單輸出，所以 R是一個(gè) 1x1的矩陣，即一個(gè)常數(shù)，上面的公式可以簡(jiǎn)化為: K = P *C / (C * P * C + R) 4) 狀態(tài)變量反饋的誤差量： e = Z(k) C*X(k) 這里的 Z(k) 是帶噪聲的測(cè)量量5) 更新誤差相關(guān)矩陣P P = P K * C * P6) 更新狀態(tài)變量： X =X + K*e = X + K

10、* (Z(k) C*X(k)7) 最后的輸出： Y = C*X現(xiàn)在的問(wèn)題就是如何實(shí)現(xiàn)卡爾曼濾波， A, B, C, Q, R 這些矩陣或量如何確定？（六）仿真實(shí)例下面用仿真實(shí)例來(lái)觀察卡爾曼濾波器的效果。假設(shè)我們的系統(tǒng)是一個(gè)加熱系統(tǒng)，熱時(shí)間常數(shù)為 60 秒，100度時(shí)達(dá)到熱平衡。忽略系統(tǒng)的延遲，那么當(dāng)系統(tǒng)加電后，溫度由 0 開始上升。這個(gè)上升過(guò)程大家應(yīng)該很熟悉，這是一個(gè)指數(shù)函數(shù)： y(t) = 100 * (1 e-t/60)其 laplace 傳遞函數(shù)為: y(s) = 100 / (60 * s - 1)我們?nèi)藶榈募尤肓穗S機(jī)噪聲來(lái)模擬測(cè)量噪聲下載 (3.95 KB)2011-12-7 22:

11、37我們假定并不知道系統(tǒng)的傳遞函數(shù)，現(xiàn)在只是簡(jiǎn)單的, 隨便地構(gòu)造了一個(gè)預(yù)測(cè)系統(tǒng)。A = 1, 0; 0, 1B = 1; 0C = 1, 0這是一個(gè)二階系統(tǒng)，其輸出是一條直線，與實(shí)際的系統(tǒng)相去甚遠(yuǎn)：下載 (4.11 KB)2011-12-8 21:37測(cè)量噪聲的協(xié)方差 R = 40，此為猜測(cè)值; 系統(tǒng)噪聲 Q = 2，也是猜測(cè)值，預(yù)測(cè)模型越不準(zhǔn)，Q 值應(yīng)越大?？柭鼮V波器的結(jié)果，紅色為濾波器輸出：下載 (5.36 KB)2011-12-8 21:37可以看到，盡管我們使用了一個(gè)粗劣的預(yù)測(cè)估計(jì)器，Kalman 濾波器還是相當(dāng)?shù)钠?shí)，基本上消除了噪聲. 如果我們有一個(gè)相當(dāng)精確的模型，結(jié)果會(huì)怎么樣呢

12、？精確模型的建立要建立一個(gè)精確的預(yù)測(cè)估計(jì)模型，我們還是要利用方差。如果一個(gè)估計(jì)的曲線與實(shí)際曲線完全重合時(shí)，他們的方差為 0. 方差越小，擬合度越高， 最小二乘法的原理便是如此。具體推導(dǎo)過(guò)程還是省略，直接給出 matlab 的擬合程序，這是一個(gè)非常非常有用的程序。如果數(shù)學(xué)模型很精確， 能不能直接數(shù)學(xué)模型的輸出作為濾波器的結(jié)果呢？不能，因?yàn)闆](méi)有反饋，數(shù)學(xué)模型的輸出會(huì)因?yàn)闆](méi)有反饋的校正造成誤差不斷累積，失之毫厘，謬之千里。下面是用最小二乘法獲得系統(tǒng)的模型并做為預(yù)測(cè)估計(jì)器，設(shè)定為 3 階系統(tǒng)， 得出的數(shù)學(xué)模型相當(dāng)準(zhǔn)確，所以Q值可以取一個(gè)小值，這里 Q= 0.02, 現(xiàn)在看看卡爾曼濾波器的結(jié)果：下載 (4.77 KB)2011-12-8 21:37效果非常好，卡爾曼濾波器的輸出與實(shí)際系統(tǒng)的輸出 (即無(wú)噪聲的系統(tǒng)輸出) 幾乎重合，這是精確的預(yù)測(cè)估計(jì)模型帶來(lái)的好處。現(xiàn)在比較兩個(gè)例子中 卡爾曼增益的不同下載 (4.14 KB)2011-12-8 21:37最小二乘法獲得系統(tǒng)的模型中的增益迅速地由大變小，最后小于不準(zhǔn)確模型。K 值較小，意味著誤差反饋量較小， 使得預(yù)測(cè)輸出更偏重信任預(yù)測(cè)模型的結(jié)果， 通過(guò)上圖可以看