姓名：周小蓬 16019110037

轉(zhuǎn)載自：http://blog.csdn.net/MangZuo/article/details/71171137

[嵌牛導(dǎo)讀]

我一直有一個(gè)愿望，就是把抽象的理論具體化，用最直白的方式告訴大家--不提一個(gè)生澀的詞，不寫一個(gè)數(shù)學(xué)公式，像講故事一樣先把道理說明白，需要知道細(xì)節(jié)的同學(xué)可以自己去查所有需要知道的一切。因?yàn)閷W(xué)習(xí)的過程告訴我，最難的其實(shí)是最初和這個(gè)理論和應(yīng)用背景親和的過程--這些理論它究竟是做什么的，又是怎么做到的?？上覀兡芸吹降年P(guān)于這些理論的資料大多數(shù)都是公式的堆砌并且假定我們明白許多“基本的道理”，其實(shí)這些“基本的道理”往往是我們最難想象和超越的。以卡爾曼濾波為例，讓我們嘗試一種不同的學(xué)習(xí)方法。

[嵌牛鼻子]

計(jì)算機(jī)語言，計(jì)算機(jī)視覺圖像處理

[嵌牛提問]

什么事卡爾曼濾波算法

[嵌牛正文]

相信所有學(xué)習(xí)卡爾曼濾波的同學(xué)首先接觸的都是狀態(tài)方程和觀測方程，學(xué)過控制系統(tǒng)的同學(xué)可能不陌生，否則，先被那兩個(gè)看起來好深?yuàn)W的公式給嚇跑了，關(guān)鍵是還不知道他們究竟是干什么的，什么是狀態(tài)，什么是觀測。。。。。。如果再看到后面的一大串遞歸推導(dǎo)增益，實(shí)在很暈很暈，更糟糕的是還沒整明白的時(shí)候就已經(jīng)知道卡爾曼濾波其實(shí)已經(jīng)不夠使了，需要extended kalmanfilter 和particle filter了。。。

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

用一句最簡單的話來說，卡爾曼濾波是來幫助我們做測量的，大家一定不明白測量干嘛搞那么復(fù)雜？測量長度拿個(gè)尺子比一下，測量溫度拿溫度表測一下不就完了嘛。的確如此，如果你要測量的東西很容易測準(zhǔn)確，沒有什么隨機(jī)干擾，那真的不需要?jiǎng)隈{卡爾曼先生。但在有的時(shí)候，我們的測量因?yàn)殡S機(jī)干擾，無法準(zhǔn)確得到，卡爾曼先生就給我們想了個(gè)辦法，讓我們?cè)诟蓴_為高斯分布的情況下，得到的測量均方誤差最小，也就是測量值擾動(dòng)最小，看起來最平滑。

還是舉例子最容易明白。我最近養(yǎng)了只小兔子，忍不住拿小兔子做個(gè)例子嘻嘻。

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

有了這個(gè)權(quán)值，下面的事情就很好辦了。很顯然卡爾曼先生是利用多次觀察和估計(jì)來達(dá)到目的的，我們也只能一步一步地調(diào)整我們的觀察和估計(jì)值，來漸漸達(dá)到準(zhǔn)確的測量，所以整個(gè)算法是遞歸的，需要多次重復(fù)調(diào)整的。調(diào)整的過程也很簡單，就是把實(shí)測值（稱出來的體重）和估計(jì)值（書上得來的體重）比較一下，如果估計(jì)值比測量值小，那就把估計(jì)值加上他們之間的偏差作為新的估計(jì)值，當(dāng)然前面要加個(gè)系數(shù)，就是我們前面說的加權(quán)系數(shù)，這個(gè)地方我要寫個(gè)公式，因?yàn)楹芎唵尉湍苷f明白。

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

發(fā)現(xiàn)把一個(gè)問題講明白還真不是件容易的事情，誰聽明白了我佩服誰，因?yàn)槲乙呀?jīng)把自己講糊涂了哈

順便就把extended kalman filter和particle filter提一下，因?yàn)楦咚鼓Ｐ陀袝r(shí)不適用，于是有了extended kalman filter，而particle filter是用于多個(gè)對(duì)象的，比如除了兔子我還有只貓，他們的體重有一個(gè)聯(lián)合概率模型，每一個(gè)對(duì)象就是一個(gè)particle。無論是卡爾曼濾波還是particle濾波，都是概率分布傳遞的過程，卡爾曼傳遞的是高斯分布，particle filter 傳遞的是高斯混合分布，每一個(gè)峰代表一個(gè)動(dòng)物在我們的例子。

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

二、卡爾曼濾波之?dāng)?shù)學(xué)建模

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

所謂濾波，實(shí)際上是要去掉自己不想要的信號(hào)，保留想要的部分。一般來說，是把過程中的噪聲去掉。

卡爾曼濾波的默認(rèn)假定是，世界充滿噪聲，任何測量結(jié)果都有噪聲，狀態(tài)轉(zhuǎn)移過程會(huì)有噪聲，你想知道系統(tǒng)的真實(shí)值么？玩兒蛋去吧。

卡爾曼濾波另一個(gè)重要假定模型是這樣的，一個(gè)系統(tǒng)會(huì)處在各種不同的狀態(tài)，并且會(huì)在狀態(tài)之間轉(zhuǎn)化來轉(zhuǎn)化去。但是呢，倒霉的是我們誰也不知道該系統(tǒng)當(dāng)前到底是在什么狀態(tài)；但是呢，幸運(yùn)的是我們可以通過測量的結(jié)果猜測到系統(tǒng)當(dāng)前在一個(gè)什么狀態(tài)。

那啥叫狀態(tài)呢？例如系統(tǒng)的速度，加速度，溫度，腦殘度都算。離散系統(tǒng)的話，我們可以假設(shè)一個(gè)黑盒，黑盒里有許多顏色的燈（紅橙黃綠青藍(lán)紫），同時(shí)只能有一個(gè)顏色在亮著，ok，哪個(gè)燈在亮就是當(dāng)前狀態(tài)。

下面就是建模：

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時(shí)刻系統(tǒng)所在狀態(tài)，z_t是所謂觀測值，u_t是系統(tǒng)控制變量（已知，但我做的領(lǐng)域一般用不著），w_t , v_t都是噪聲。

那么式子(1)就是想說，觀測值和系統(tǒng)狀態(tài)的關(guān)系： 如果你看到種子發(fā)芽了，那么它的狀態(tài)就是剛出生；如果你看到它開始長葉兒了，那狀態(tài)就叫生長期；如果丫開花了，就叫啥啥期；如果結(jié)果了，就叫成熟期；如果蔫兒了，就叫嗝屁期。

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

那么式子(2)就是說，前一時(shí)刻狀態(tài)和下一時(shí)刻狀態(tài)之間的關(guān)系。我在這里卡了好久，總是以為丫是馬爾科夫過程，所以就完全無法理解A這個(gè)系數(shù)是憑啥得來的。其實(shí)，就是一個(gè)固定的轉(zhuǎn)移方程，該方程完全沒有概率問題。

所以！以上式子中，固定下來的是H, A, B，這三個(gè)矩陣千年不變，萬年不變，并且是事先設(shè)定好的，全都已知。未知的話....你自己編一個(gè)模型吧。 那么w_t，v_t 在這里限定為兩個(gè)高斯白噪聲N(0, Q)和N(0, R)。 哦，對(duì)，這里要記得，Q,R都tm是協(xié)方差矩陣啊，因?yàn)椋到y(tǒng)狀態(tài)和觀測值都是向量。我對(duì)協(xié)方差可郁悶可郁悶了。這里提一句，我就完全無法理解協(xié)方差想表達(dá)什么，為什么倆隨機(jī)變量獨(dú)立，協(xié)方差一定為0，雖然我也知道怎么推導(dǎo)，但就是不能直觀理解之，如果有人知道，還煩請(qǐng)告知。

那繼續(xù)扯淡。卡爾曼濾波，本質(zhì)是想要預(yù)測下一步的觀測值，或者實(shí)時(shí)監(jiān)控一下系統(tǒng)所在狀態(tài)。但一般在我這個(gè)領(lǐng)域，大家還都是在玩兒預(yù)測，那咱就從預(yù)測角度分析。OK，直覺上，給定上一個(gè)位置的狀態(tài)x_(t-1)，式子(2)足夠了。但是，回到開始的默認(rèn)假設(shè)，式子(2)得到的結(jié)果x^-_t那是各種不準(zhǔn)確啊。不準(zhǔn)確怎么辦？那就去看觀測值唄，于是得到觀測值z(mì)_t，但是觀測值也不準(zhǔn)確唉，那怎么辦？當(dāng)當(dāng)當(dāng)當(dāng)，卡爾曼告訴我們一個(gè)灰常牛B的事情，一個(gè)相對(duì)準(zhǔn)確的系統(tǒng)值具有如下結(jié)構(gòu)：

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

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

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

哦，對(duì)了，另外注意一點(diǎn)，從此以后，我們就都在估計(jì)上做文章了，你只要記得，咱永遠(yuǎn)看不到真實(shí)值就行了，于是我們的式子里不是帶"&"就是帶"-"。

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

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

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

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

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

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

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

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

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

最后小注一下，文章指出，如果Q,R是常數(shù)，K會(huì)收斂，也即P也會(huì)收斂到常量。 另外，大家經(jīng)常詬病卡爾曼濾波都是假定高斯分布，我勒個(gè)去，這里的高斯分布到底說誰呢？噪聲項(xiàng)？雖然看上去應(yīng)該是，但我打賭不是?？墒瞧渌侄际嵌ㄖ?，唉，頭大。我本來就是為了理解這句話才來學(xué)習(xí)卡爾曼濾波的。 還得慢慢學(xué)，繼續(xù)淚奔

。

PS：于是，果然，文中提到x_t是一個(gè)隨機(jī)變量，并且在已知z_t的情況下 p(x_t | z_t) 服從N( x&_t, E[(x_t - x&_t)(x_t - x&_t)])，切記切記，這里所說的正態(tài)分布，是指已知觀測值的情況下，系統(tǒng)狀態(tài)是一個(gè)高斯分布的隨機(jī)變量。