姓名：周小蓬 16019110037

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

[嵌牛導讀]

我一直有一個愿望肘交，就是把抽象的理論具體化抢腐，用最直白的方式告訴大家--不提一個生澀的詞，不寫一個數(shù)學公式薄坏，像講故事一樣先把道理說明白兄渺，需要知道細節(jié)的同學可以自己去查所有需要知道的一切缝龄。因為學習的過程告訴我，最難的其實是最初和這個理論和應(yīng)用背景親和的過程--這些理論它究竟是做什么的，又是怎么做到的叔壤∠顾牵可惜我們能看到的關(guān)于這些理論的資料大多數(shù)都是公式的堆砌并且假定我們明白許多“基本的道理”，其實這些“基本的道理”往往是我們最難想象和超越的炼绘。以卡爾曼濾波為例嗅战，讓我們嘗試一種不同的學習方法。

[嵌牛鼻子]

計算機語言俺亮，計算機視覺圖像處理

[嵌牛提問]

什么事卡爾曼濾波算法

[嵌牛正文]

相信所有學習卡爾曼濾波的同學首先接觸的都是狀態(tài)方程和觀測方程驮捍，學過控制系統(tǒng)的同學可能不陌生，否則脚曾，先被那兩個看起來好深奧的公式給嚇跑了东且，關(guān)鍵是還不知道他們究竟是干什么的，什么是狀態(tài)本讥，什么是觀測苇倡。。囤踩。。晓褪。堵漱。如果再看到后面的一大串遞歸推導增益，實在很暈很暈涣仿，更糟糕的是還沒整明白的時候就已經(jīng)知道卡爾曼濾波其實已經(jīng)不夠使了勤庐，需要extended kalmanfilter 和particle filter了。好港。愉镰。

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

用一句最簡單的話來說拔莱，卡爾曼濾波是來幫助我們做測量的碗降，大家一定不明白測量干嘛搞那么復雜？測量長度拿個尺子比一下塘秦，測量溫度拿溫度表測一下不就完了嘛讼渊。的確如此，如果你要測量的東西很容易測準確尊剔，沒有什么隨機干擾爪幻，那真的不需要勞駕卡爾曼先生。但在有的時候，我們的測量因為隨機干擾挨稿，無法準確得到仇轻，卡爾曼先生就給我們想了個辦法，讓我們在干擾為高斯分布的情況下叶组，得到的測量均方誤差最小拯田，也就是測量值擾動最小，看起來最平滑甩十。

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

每天給兔子拔草鸭轮，看她香甜地吃啊吃地，就忍不住關(guān)心一下她的體重增長情況橄霉。那么我們就以小兔子的體重作為研究對象吧窃爷。假定我每周做一次觀察，我有兩個辦法可以知道兔子的體重姓蜂，一個是拿體重計來稱：或許你有辦法一下子就稱準兔子的體重（獸醫(yī)通常都有這辦法）按厘，但現(xiàn)在為了體現(xiàn)卡爾曼先生理論的魅力，我們假定你的稱實在很糟糕钱慢，誤差很大逮京，或者兔子太調(diào)皮，不能老實呆著束莫，彈簧秤因為小兔子的晃動會產(chǎn)生很大誤差懒棉。盡管有誤差，那也是一個不可失去的渠道來得到兔子的體重览绿。還有一個途徑是根據(jù)書本上的資料策严，和兔子的年齡，我可以估計一下我的小兔子應(yīng)該會多重饿敲，我們把用稱稱出來的叫觀察量妻导，用資料估計出來的叫估計值，無論是觀察值還是估計值顯然都是有誤差的怀各，假定誤差是高斯分布±跏現(xiàn)在問題就來了，按照書本上說我的兔子該3公斤重渠啤，稱出來卻只有2.5公斤狐肢，我究竟該信哪個呢？如果稱足夠準沥曹，兔子足夠乖份名，卡爾曼先生就沒有用武之地了呵呵碟联，再強調(diào)一下是我們的現(xiàn)狀是兔兔不夠乖，稱還很爛呵呵僵腺。在這樣惡劣的情景下鲤孵，卡爾曼先生告訴我們一個辦法，仍然可以估計出八九不離十的兔兔體重辰如，這個辦法其實也很直白普监，就是加權(quán)平均，把稱稱出來的結(jié)果也就是觀測值和按照書本經(jīng)驗估算出來的結(jié)果也就是估計值分別加一個權(quán)值琉兜，再做平均凯正。當然這兩個權(quán)值加起來是等于一的。也就是說如果你有0.7分相信稱出來的體重豌蟋，那么就只有0.3分相信書上的估計廊散。說到這里大家一定更著急了，究竟該有幾分相信書上的梧疲，有幾分相信我自己稱的呢允睹？都怪我的稱不爭氣，沒法讓我百分一百信賴它幌氮，還要根據(jù)書上的數(shù)據(jù)來做調(diào)整缭受。好在卡爾曼先生也體會到了我們的苦惱，告訴我們一個辦法來決定這個權(quán)值该互，這個辦法其實也很直白贯涎，就是根據(jù)以往的表現(xiàn)來做決定，這其實聽起來挺公平的慢洋，你以前表現(xiàn)好，我就相信你多一點陆盘，權(quán)值也就給的高一點普筹，以前表現(xiàn)不好，我就相信你少一點隘马，權(quán)值自然給的低一點太防。那么什么是表現(xiàn)好表現(xiàn)不好呢，表現(xiàn)好意思就是測量結(jié)果穩(wěn)定酸员，方差很小蜒车，表現(xiàn)不好就是估計值或觀測值不穩(wěn)定，方差很大幔嗦。想象你用稱稱你的哦兔子酿愧，第一次1公斤第二次10公斤，第三次5公斤邀泉，你會相信你的稱嗎嬉挡，但是如果第一次3公斤第二次3.2公斤钝鸽，第三次2.8公斤，自然我就相信它多一點庞钢，給它一個大的權(quán)值了拔恰。

有了這個權(quán)值，下面的事情就很好辦了基括。很顯然卡爾曼先生是利用多次觀察和估計來達到目的的颜懊，我們也只能一步一步地調(diào)整我們的觀察和估計值，來漸漸達到準確的測量风皿，所以整個算法是遞歸的河爹，需要多次重復調(diào)整的。調(diào)整的過程也很簡單揪阶，就是把實測值（稱出來的體重）和估計值（書上得來的體重）比較一下昌抠，如果估計值比測量值小，那就把估計值加上他們之間的偏差作為新的估計值鲁僚，當然前面要加個系數(shù)炊苫，就是我們前面說的加權(quán)系數(shù)，這個地方我要寫個公式冰沙，因為很簡單就能說明白侨艾。

比如我們的觀查值是Z，估計值是X拓挥， 那么新的估計值就應(yīng)該是 Xnew ?= ?X ?+ K ( Z-X）唠梨，從這個公式可以看到，如果X估計小了侥啤，那么新的估計值會加上一個量K ( Z-X）, 如果估計值大了,大過Z了当叭，那么新的估計值就會減去一個量K ( Z-X），這就保證新的估計值一定比現(xiàn)在的準確盖灸，一次一次遞歸下去就會越來越準卻了蚁鳖，當然這里面很有作用的也是這個K，也就是我們前面說的權(quán)值赁炎，書上都把他叫卡爾曼增益醉箕。撑毛。婿牍。(Xnew ?= ?X ?+ K ( Z-X） = X ×（1-K) + KZ 藤违，也就是說估計值X的權(quán)值是1-k盒揉，而觀察值Z的權(quán)值是k鸽心，究竟k 取多大瞒滴，全看估計值和觀察值以前的表現(xiàn)姨夹，也就是他們的方差情況了）

發(fā)現(xiàn)把一個問題講明白還真不是件容易的事情夸溶，誰聽明白了我佩服誰吴旋，因為我已經(jīng)把自己講糊涂了哈

順便就把extended kalman filter和particle filter提一下剧辐，因為高斯模型有時不適用寒亥，于是有了extended kalman filter，而particle filter是用于多個對象的荧关，比如除了兔子我還有只貓溉奕，他們的體重有一個聯(lián)合概率模型，每一個對象就是一個particle忍啤。無論是卡爾曼濾波還是particle濾波加勤，都是概率分布傳遞的過程，卡爾曼傳遞的是高斯分布同波，particle filter 傳遞的是高斯混合分布鳄梅，每一個峰代表一個動物在我們的例子。

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

二未檩、卡爾曼濾波之數(shù)學建模

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

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

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

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

那啥叫狀態(tài)呢赋访？例如系統(tǒng)的速度可都，加速度缓待，溫度，腦殘度都算渠牲。離散系統(tǒng)的話旋炒，我們可以假設(shè)一個黑盒，黑盒里有許多顏色的燈（紅橙黃綠青藍紫）签杈，同時只能有一個顏色在亮著瘫镇，ok，哪個燈在亮就是當前狀態(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時刻系統(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é)果了铲觉，就叫成熟期；如果蔫兒了吓坚，就叫嗝屁期撵幽。

哦，對了礁击，個人理解一下盐杂，以上公式限定為了線性系統(tǒng)，傳說中卡爾曼濾波是線性的哆窿，來源就在這里了链烈，誰叫你是矩陣乘向量呢，你要是寫成f(x_t)挚躯，那有可能就是非線性的了强衡。

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

所以眉尸！以上式子中域蜗，固定下來的是H, A, B，這三個矩陣千年不變噪猾，萬年不變霉祸，并且是事先設(shè)定好的，全都已知袱蜡。未知的話....你自己編一個模型吧丝蹭。 那么w_t，v_t 在這里限定為兩個高斯白噪聲N(0, Q)和N(0, R)坪蚁。 哦奔穿，對，這里要記得敏晤，Q,R都tm是協(xié)方差矩陣啊贱田，因為，系統(tǒng)狀態(tài)和觀測值都是向量嘴脾。我對協(xié)方差可郁悶可郁悶了男摧。這里提一句，我就完全無法理解協(xié)方差想表達什么译打，為什么倆隨機變量獨立，協(xié)方差一定為0奏司，雖然我也知道怎么推導韵洋，但就是不能直觀理解之竿刁，如果有人知道搪缨，還煩請告知。

那繼續(xù)扯淡勉吻〖嗌簦卡爾曼濾波齿桃，本質(zhì)是想要預測下一步的觀測值短纵，或者實時監(jiān)控一下系統(tǒng)所在狀態(tài)香到。但一般在我這個領(lǐng)域，大家還都是在玩兒預測千绪，那咱就從預測角度分析荸型。OK瑞妇，直覺上辕狰，給定上一個位置的狀態(tài)x_(t-1)蔓倍，式子(2)足夠了。但是柬脸，回到開始的默認假設(shè)倒堕，式子(2)得到的結(jié)果x^-_t那是各種不準確啊垦巴。不準確怎么辦铭段？那就去看觀測值唄序愚，于是得到觀測值z_t，但是觀測值也不準確唉望门，那怎么辦筹误？當當當當厨剪，卡爾曼告訴我們一個灰常牛B的事情祷膳，一個相對準確的系統(tǒng)值具有如下結(jié)構(gòu)：

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

提一下钾唬，這里" & "表示估計值抡秆，" - "表示是用前面式子算出來的估計值儒士，不帶" - "表示是最后的估計值着撩。 (3)這個式子是想說拖叙，你不是式子估計和觀測值都不準么薯鳍，那么你把他倆加個權(quán)挖滤，求個和浅役，那就可能更準確了。啥惧盹？你說這個式子不像加權(quán)钧椰？你把丫拆了演侯，倒騰倒騰不就像了秒际。 所以娄徊，最牛B就牛B在了這個“K”寄锐，傳說中的卡爾曼增益橄仆。 這個K怎么得到的盆顾？我也不知道您宪。 文章說法是宪巨，定義誤差 e_t = x_t - x&_t 捏卓，P_t為此誤差的協(xié)方差矩陣天吓，目的是使這個誤差協(xié)方差矩陣最小化龄寞，把(3)代過去物邑，于是折騰來折騰去色解，再求個導數(shù)為0科阎，解得K锣笨，這個關(guān)鍵值的算法：

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

哦错英，對了椭岩，另外注意一點判哥，從此以后塌计，我們就都在估計上做文章了夺荒，你只要記得技扼，咱永遠看不到真實值就行了剿吻，于是我們的式子里不是帶"&"就是帶"-"丽旅。

那么式子(4)就是在說邪狞，你丫這么著就把K求出來了帆卓。于是剑令，問題就變成了這個P-_t怎么個求法吁津。

說到這里碍脏，傳說中的卡爾曼濾波就算講完了潮酒。神馬急黎？你說P-_k還沒求呢勃教。是啊故源，卡爾曼濾波一共就倆需要求的玩意兒，還都tm是迭代求解门驾，一個就是這個P-_t奶是，另一個就是狀態(tài)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誰是哪個公式冒出來的啊豪嗽。 另外嚴重關(guān)切一下這里"-","&"以及不加的關(guān)系龟梦。 注意到啥沒有计贰？對了躁倒，(6)式中等號右邊的P_(t-1)不帶任何符號秧秉，嘿嘿，那自然是還差一個公式啦：

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

大功告成砾淌，以上就是傳說中的卡爾曼濾波的迭代求解方法拇舀，如果你要預測狀態(tài)呢骄崩，就用式子(5)的結(jié)果；如果預測觀測值呢脱惰，就把式子(5)的結(jié)果乘個H拉一；如果是監(jiān)測狀態(tài)呢蔚润，就用式子(3)的結(jié)果嫡纠。

至于一切式子中的推導過程叉橱，還有為神馬是這樣求出來的窃祝，咕~~(╯﹏╰)b粪小，本人一概不知糕再。淚奔告退殴蹄。

最后小注一下袭灯，文章指出稽荧，如果Q,R是常數(shù)姨丈，K會收斂翁潘，也即P也會收斂到常量拜马。 另外俩莽，大家經(jīng)常詬病卡爾曼濾波都是假定高斯分布价淌，我勒個去蝉衣，這里的高斯分布到底說誰呢病毡？噪聲項？雖然看上去應(yīng)該是僧家，但我打賭不是八拱〖〉荆可是其它又都是定值，唉诺凡，頭大腹泌。我本來就是為了理解這句話才來學習卡爾曼濾波的穷娱。 還得慢慢學，繼續(xù)淚奔

。

PS：于是，果然左敌，文中提到x_t是一個隨機變量矫限，并且在已知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)是一個高斯分布的隨機變量。