我是勤勞的搬運工，轉自：

1.http://blog.csdn.net/karen99/article/details/7771743

2.http://blog.csdn.net/tudouniurou/article/details/6277512

--------------------------------------------------------------------------------

一、卡爾曼最直白的解釋：

        卡爾曼濾波本來是控制系統課上學的，當時就沒學明白，也矇混過關了，以為以後也不用再見到它了，可惜沒這麼容易，後來學計算機視覺和影象處理，發現用它的地方更多了，沒辦法的時候只好耐心學習和理解了。一直很想把學習的過程記錄一下，讓大家少走彎路，可惜總也沒時間和機會，直到今天。

          我一直有一個願望，就是把抽象的理論具體化，用最直白的方式告訴大家--不提一個生澀的詞，不寫一個數學公式，像講故事一樣先把道理說明白，需要知道細節的同學可以自己去查所有需要知道的一切。因為學習的過程告訴我，最難的其實是最初和這個理論和應用背景親和的過程--這些理論它究竟是做什麼的，又是怎麼做到的。可惜我們能看到的關於這些理論的資料大多數都是公式的堆砌並且假定我們明白許多“基本的道理”，其實這些“基本的道理”往往是我們最難想象和超越的。以卡爾曼濾波為例，讓我們嘗試一種不同的學習方法。

         相信所有學習卡爾曼濾波的同學首先接觸的都是狀態方程和觀測方程，學過控制系統的同學可能不陌生，否則，先被那兩個看起來好深奧的公式給嚇跑了，關鍵是還不知道他們究竟是幹什麼的，什麼是狀態，什麼是觀測。。。。。。如果再看到後面的一大串遞迴推導增益，實在很暈很暈，更糟糕的是還沒整明白的時候就已經知道卡爾曼濾波其實已經不夠使了，需要extended kalmanfilter 和particle filter了。。。

其實我們完全不用理會這些公式。先來看看究竟卡爾曼濾波是做什麼的，理解了卡爾曼濾波，下面的就順其自然了。

         用一句最簡單的話來說，卡爾曼濾波是來幫助我們做測量的，大家一定不明白測量幹嘛搞那麼複雜？測量長度拿個尺子比一下，測量溫度拿溫度表測一下不就完了嘛。的確如此，如果你要測量的東西很容易測準確，沒有什麼隨機干擾，那真的不需要勞駕卡爾曼先生。但在有的時候，我們的測量因為隨機干擾，無法準確得到，卡爾曼先生就給我們想了個辦法，讓我們在干擾為高斯分佈的情況下，得到的測量均方誤差最小，也就是測量值擾動最小，看起來最平滑。

還是舉例子最容易明白。我最近養了只小兔子，忍不住拿小兔子做個例子嘻嘻。

         每天給兔子拔草，看她香甜地吃啊吃地，就忍不住關心一下她的體重增長情況。那麼我們就以小兔子的體重作為研究物件吧。假定我每週做一次觀察，我有兩個辦法可以知道兔子的體重，一個是拿體重計來稱：或許你有辦法一下子就稱準兔子的體重（獸醫通常都有這辦法），但現在為了體現卡爾曼先生理論的魅力，我們假定你的稱實在很糟糕，誤差很大，或者兔子太調皮，不能老實呆著，彈簧秤因為小兔子的晃動會產生很大誤差。儘管有誤差，那也是一個不可失去的渠道來得到兔子的體重。還有一個途徑是根據書本上的資料，和兔子的年齡，我可以估計一下我的小兔子應該會多重，我們把用稱稱出來的叫觀察量，用資料估計出來的叫估計值，無論是觀察值還是估計值顯然都是有誤差的，假定誤差是高斯分佈。現在問題就來了，按照書本上說我的兔子該3公斤重，稱出來卻只有2.5公斤，我究竟該信哪個呢？如果稱足夠準，兔子足夠乖，卡爾曼先生就沒有用武之地了呵呵，再強調一下是我們的現狀是兔兔不夠乖，稱還很爛呵呵。在這樣惡劣的情景下，卡爾曼先生告訴我們一個辦法，仍然可以估計出八九不離十的兔兔體重，這個辦法其實也很直白，就是加權平均，把稱稱出來的結果也就是觀測值和按照書本經驗估算出來的結果也就是估計值分別加一個權值，再做平均。當然這兩個權值加起來是等於一的。也就是說如果你有0.7分相信稱出來的體重，那麼就只有0.3分相信書上的估計。說到這裡大家一定更著急了，究竟該有幾分相信書上的，有幾分相信我自己稱的呢？都怪我的稱不爭氣，沒法讓我百分一百信賴它，還要根據書上的資料來做調整。好在卡爾曼先生也體會到了我們的苦惱，告訴我們一個辦法來決定這個權值，這個辦法其實也很直白，就是根據以往的表現來做決定，這其實聽起來挺公平的，你以前表現好，我就相信你多一點，權值也就給的高一點，以前表現不好，我就相信你少一點，權值自然給的低一點。那麼什麼是表現好表現不好呢，表現好意思就是測量結果穩定，方差很小，表現不好就是估計值或觀測值不穩定，方差很大。想象你用稱稱你的哦兔子，第一次1公斤第二次10公斤，第三次5公斤，你會相信你的稱嗎，但是如果第一次3公斤第二次3.2公斤，第三次2.8公斤，自然我就相信它多一點，給它一個大的權值了。

        有了這個權值，下面的事情就很好辦了。很顯然卡爾曼先生是利用多次觀察和估計來達到目的的，我們也只能一步一步地調整我們的觀察和估計值，來漸漸達到準確的測量，所以整個演算法是遞迴的，需要多次重複調整的。調整的過程也很簡單，就是把實測值（稱出來的體重）和估計值（書上得來的體重）比較一下，如果估計值比測量值小，那就把估計值加上他們之間的偏差作為新的估計值，當然前面要加個係數，就是我們前面說的加權係數，這個地方我要寫個公式，因為很簡單就能說明白。

         比如我們的觀查值是Z，估計值是X， 那麼新的估計值就應該是 Xnew  =  X  + K ( Z-X），從這個公式可以看到，如果X估計小了，那麼新的估計值會加上一個量K ( Z-X）, 如果估計值大了,大過Z了，那麼新的估計值就會減去一個量K ( Z-X），這就保證新的估計值一定比現在的準確，一次一次遞迴下去就會越來越準卻了，當然這裡面很有作用的也是這個K，也就是我們前面說的權值，書上都把他叫卡爾曼增益。。。(Xnew  =  X  + K ( Z-X） = X ×（1-K) + KZ ，也就是說估計值X的權值是1-k，而觀察值Z的權值是k，究竟k 取多大，全看估計值和觀察值以前的表現，也就是他們的方差情況了）

發現把一個問題講明白還真不是件容易的事情，誰聽明白了我佩服誰，因為我已經把自己講糊塗了哈

順便就把extended kalman filter和particle filter提一下，因為高斯模型有時不適用，於是有了extended kalman filter，而particle filter是用於多個物件的，比如除了兔子我還有隻貓，他們的體重有一個聯合概率模型，每一個物件就是一個particle。無論是卡爾曼濾波還是particle濾波，都是概率分佈傳遞的過程，卡爾曼傳遞的是高斯分佈，particle filter 傳遞的是高斯混合分佈，每一個峰代表一個動物在我們的例子。

------------------------------------------------華麗的分割線------------------------------------------------

二、卡爾曼濾波之數學建模

一直在看，一直不懂。 我這人學數學的毛病，就是需要非常細緻的知道每個變數的含義，誰變誰不變必須清清楚楚告訴我，否則我就沒有那個直覺。 anyway，從這篇文章入手吧：http://www.cs.unc.edu/~welch/kalman/media/pdf/kalman_intro_chinese.pdf

所謂濾波，實際上是要去掉自己不想要的訊號，保留想要的部分。一般來說，是把過程中的噪聲去掉。

卡爾曼濾波的預設假定是，世界充滿噪聲，任何測量結果都有噪聲，狀態轉移過程會有噪聲，你想知道系統的真實值麼？玩兒蛋去吧。

卡爾曼濾波另一個重要假定模型是這樣的，一個系統會處在各種不同的狀態，並且會在狀態之間轉化來轉化去。但是呢，倒黴的是我們誰也不知道該系統當前到底是在什麼狀態；但是呢，幸運的是我們可以通過測量的結果猜測到系統當前在一個什麼狀態。

那啥叫狀態呢？例如系統的速度，加速度，溫度，腦殘度都算。離散系統的話，我們可以假設一個黑盒，黑盒裡有許多顏色的燈（紅橙黃綠青藍紫），同時只能有一個顏色在亮著，ok，哪個燈在亮就是當前狀態。

下面就是建模：

z_t = H*x_t + v_t;                                                                                                                                         (1)

                             x_t = A*x_(t-1) + B*u_(t-1) + w_(t-1);                                                                                                                      (2)

        初看起這倆式子來，我也頭大，不過稍微分析一下也不太難。x_t是t時刻系統所在狀態，z_t是所謂觀測值，u_t是系統控制變數（已知，但我做的領域一般用不著），w_t , v_t都是噪聲。

         那麼式子(1)就是想說，觀測值和系統狀態的關係： 如果你看到種子發芽了，那麼它的狀態就是剛出生；如果你看到它開始長葉兒了，那狀態就叫生長期；如果丫開花了，就叫啥啥期；如果結果了，就叫成熟期；如果蔫兒了，就叫嗝屁期。

         哦，對了，個人理解一下，以上公式限定為了線性系統，傳說中卡爾曼濾波是線性的，來源就在這裡了，誰叫你是矩陣乘向量呢，你要是寫成f(x_t)，那有可能就是非線性的了。

        那麼式子(2)就是說，前一時刻狀態和下一時刻狀態之間的關係。我在這裡卡了好久，總是以為丫是馬爾科夫過程，所以就完全無法理解A這個係數是憑啥得來的。其實，就是一個固定的轉移方程，該方程完全沒有概率問題。

         所以！以上式子中，固定下來的是H, A, B，這三個矩陣千年不變，萬年不變，並且是事先設定好的，全都已知。未知的話....你自己編一個模型吧。 那麼w_t，v_t 在這裡限定為兩個高斯白噪聲N(0, Q)和N(0, R)。 哦，對，這裡要記得，Q,R都tm是協方差矩陣啊，因為，系統狀態和觀測值都是向量。我對協方差可鬱悶可鬱悶了。這裡提一句，我就完全無法理解協方差想表達什麼，為什麼倆隨機變數獨立，協方差一定為0，雖然我也知道怎麼推導，但就是不能直觀理解之，如果有人知道，還煩請告知。

          那繼續扯淡。卡爾曼濾波，本質是想要預測下一步的觀測值，或者實時監控一下系統所在狀態。但一般在我這個領域，大家還都是在玩兒預測，那咱就從預測角度分析。OK，直覺上，給定上一個位置的狀態x_(t-1)，式子(2)足夠了。但是，回到開始的預設假設，式子(2)得到的結果x^-_t那是各種不準確啊。不準確怎麼辦？那就去看觀測值唄，於是得到觀測值z_t，但是觀測值也不準確唉，那怎麼辦？噹噹噹當，卡爾曼告訴我們一個灰常牛B的事情，一個相對準確的系統值具有如下結構：

                                                                                                                    x&_t = x&-_t + K( z_t - H*x_(t-1) ) ;                                                                                                                       (3)

提一下，這裡" & "表示估計值，" - "表示是用前面式子算出來的估計值，不帶" - "表示是最後的估計值。 (3)這個式子是想說，你不是式子估計和觀測值都不準麼，那麼你把他倆加個權，求個和，那就可能更準確了。啥？你說這個式子不像加權？你把丫拆了，倒騰倒騰不就像了。 所以，最牛B就牛B在了這個“K”，傳說中的卡爾曼增益。 這個K怎麼得到的？我也不知道。 文章說法是，定義誤差 e_t = x_t - x&_t ，P_t為此誤差的協方差矩陣，目的是使這個誤差協方差矩陣最小化，把(3)代過去，於是折騰來折騰去，再求個導數為0，解得K，這個關鍵值的演算法：

                                                                                                             K = P-_t * H^T * ( H * P-_t * H^T + R) ^(-1);                                                                                                               (4)

哦，對了，另外注意一點，從此以後，我們就都在估計上做文章了，你只要記得，咱永遠看不到真實值就行了，於是我們的式子裡不是帶"&"就是帶"-"。

那麼式子(4)就是在說，你丫這麼著就把K求出來了。於是，問題就變成了這個P-_t怎麼個求法。

說到這裡，傳說中的卡爾曼濾波就算講完了。神馬？你說P-_k還沒求呢。是啊，卡爾曼濾波一共就倆需要求的玩意兒，還都tm是迭代求解，一個就是這個P-_t，另一個就是狀態x-_t。你隨便給個初始值，然後就迭著吧。

言歸正傳，我還得給出迭代的公式：

x-_t = A * x&_(t-1) + B * u_(t-1);                                                                                                                         (5)

P-_t = A * P_(t-1) * A^T + Q;                                                                                                                            (6)

大家一定別搞混Q和R誰是哪個公式冒出來的啊。 另外嚴重關切一下這裡"-","&"以及不加的關係。 注意到啥沒有？對了，(6)式中等號右邊的P_(t-1)不帶任何符號，嘿嘿，那自然是還差一個公式啦：

P_t = (I - K_t * H ) P-_(t-1);                                                                                                                           (7)

大功告成，以上就是傳說中的卡爾曼濾波的迭代求解方法，如果你要預測狀態呢，就用式子(5)的結果；如果預測觀測值呢，就把式子(5)的結果乘個H；如果是監測狀態呢，就用式子(3)的結果。

          至於一切式子中的推導過程，還有為神馬是這樣求出來的，咕~~(╯﹏╰)b，本人一概不知。淚奔告退。

          最後小注一下，文章指出，如果Q,R是常數，K會收斂，也即P也會收斂到常量。 另外，大家經常詬病卡爾曼濾波都是假定高斯分佈，我勒個去，這裡的高斯分佈到底說誰呢？噪聲項？雖然看上去應該是，但我打賭不是。可是其它又都是定值，唉，頭大。我本來就是為了理解這句話才來學習卡爾曼濾波的。 還得慢慢學，繼續淚奔。

PS：於是，果然，文中提到x_t是一個隨機變數，並且在已知z_t的情況下 p(x_t | z_t) 服從N( x&_t, E[(x_t - x&_t)(x_t - x&_t)])，切記切記，這裡所說的正態分佈，是指已知觀測值的情況下，系統狀態是一個高斯分佈的隨機變數。