我不得不說說卡爾曼濾波
，因為它能做到的事情簡直讓人驚歎!意(yi)外(wai)的(de)是(shi)很(hen)少(shao)有(you)軟(ruan)件(jian)工(gong)程(cheng)師(shi)和(he)科(ke)學(xue)家(jia)對(dui)對(dui)它(ta)有(you)所(suo)了(le)解(jie)，這(zhe)讓(rang)我(wo)感(gan)到(dao)沮(ju)喪(sang)
，因(yin)為(wei)卡(ka)爾(er)曼(man)濾(lv)波(bo)是(shi)一(yi)個(ge)如(ru)此(ci)強(qiang)大(da)的(de)工(gong)具(ju)
，能(neng)夠(gou)在(zai)不(bu)確(que)定(ding)性(xing)中(zhong)融(rong)合(he)信(xin)息(xi)
，與(yu)此(ci)同(tong)時(shi)，它(ta)提(ti)取(qu)精(jing)確(que)信(xin)息(xi)的(de)能(neng)力(li)看(kan)起(qi)來(lai)不(bu)可(ke)思(si)議(yi)。

 

什麼是卡爾曼濾波?

 

nikeyizairenhehanyoubuquedingxinxidedongtaixitongzhongshiyongkaermanlvbo，duixitongxiayibudezouxiangzuochuyougenjudeyuce，jishibansuizhegezhongganrao，kaermanlvbozongshinengzhichuzhenshifashengdeqingkuang。

 

在連續變化的係統中使用卡爾曼濾波是非常理想的
，它具有占用內存小的優點(除了前一個狀態量外

，不需要保留其它曆史數據)，並且速度很快
，很適合應用於實時問題和嵌入式係統。

 

在Googleshangzhaodaodedaduoshuguanyushixiankaermanlvbodeshuxuegongshikanqilaiyoudianhuisenandong
，zhegezhuangkuangyoudianzaogao。shijishang
，ruguoyizhengquedefangshikandaita，kaermanlvboshifeichangjiandanherongyilijiede
，xiamianwojiangyongpiaoliangdetupianhesecaiqingxidechanshuta
，nizhixuyaodongyixiejibendegailvhejuzhendezhishijiukeyile。

 

我們能用卡爾曼濾波做什麼?

 

用玩具舉例：你開發了一個可以在樹林裏到處跑的小機器人
，這個機器人需要知道它所在的確切位置才能導航。

 

 

我們可以說機器人有一個狀態，表示位置和速度：

 

 

注zhu意yi這zhe個ge狀zhuang態tai隻zhi是shi關guan於yu這zhe個ge係xi統tong基ji本ben屬shu性xing的de一yi堆dui數shu字zi
，它ta可ke以yi是shi任ren何he其qi它ta的de東dong西xi。在zai這zhe個ge例li子zi中zhong是shi位wei置zhi和he速su度du，它ta也ye可ke以yi是shi一yi個ge容rong器qi中zhong液ye體ti的de總zong量liang，汽qi車che發fa動dong機ji的de溫wen度du
，用yong戶hu手shou指zhi在zai觸chu摸mo板ban上shang的de位wei置zhi坐zuo標biao，或huo者zhe任ren何he你ni需xu要yao跟gen蹤zong的de信xin號hao。

 

這個機器人帶有GPS，精度大約為10米，還算不錯，但是
，它需要將自己的位置精確到10米以內。樹林裏有很多溝壑和懸崖，如果機器人走錯了一步，就有可能掉下懸崖，所以隻有GPS是不夠的。

 

 

或許我們知道一些機器人如何運動的信息：liru，jiqirenzhidaofasonggeidianjidezhiling
，zhidaozijishifouzaichaoyigefangxiangyidongbingqiemeiyourenganyu
，zaixiayigezhuangtai，jiqirenhenkenengchaozhexiangtongdefangxiangyidong。dangran，jiqirenduizijideyundongshiyiwusuozhide：它ta可ke能neng受shou到dao風feng吹chui的de影ying響xiang
，輪lun子zi方fang向xiang偏pian了le一yi點dian，或huo者zhe遇yu到dao不bu平ping的de地di麵mian而er翻fan倒dao。所suo以yi，輪lun子zi轉zhuan過guo的de長chang度du並bing不bu能neng精jing確que表biao示shi機ji器qi人ren實shi際ji行xing走zou的de距ju離li，預yu測ce也ye不bu是shi很hen完wan美mei。

 

GPS 傳chuan感gan器qi告gao訴su了le我wo們men一yi些xie狀zhuang態tai信xin息xi，我wo們men的de預yu測ce告gao訴su了le我wo們men機ji器qi人ren會hui怎zen樣yang運yun動dong
，但dan都dou隻zhi是shi間jian接jie的de，並bing且qie伴ban隨sui著zhe一yi些xie不bu確que定ding和he不bu準zhun確que性xing。但dan是shi
，如ru果guo使shi用yong所suo有you對dui我wo們men可ke用yong的de信xin息xi，我wo們men能neng得de到dao一yi個ge比bi任ren何he依yi據ju自zi身shen估gu計ji更geng好hao的de結jie果guo嗎ma?回答當然是YES
，這就是卡爾曼濾波的用處。

 

卡爾曼濾波是如何看到你的問題的

 

下麵我們繼續以隻有位置和速度這兩個狀態的簡單例子做解釋。

 

 

我們並不知道實際的位置和速度，它們之間有很多種可能正確的組合
，但其中一些的可能性要大於其它部分：

 

 

卡爾曼濾波假設兩個變量(位置和速度，在這個例子中)都是隨機的，並且服從高斯分布。每個變量都有一個均值 μ，表示隨機分布的中心(最可能的狀態)
，以及方差，表示不確定性。

 

 

在上圖中，位置和速度是不相關的，這意味著由其中一個變量的狀態無法推測出另一個變量可能的值。下麵的例子更有趣：位置和速度是相關的，觀測特定位置的可能性取決於當前的速度：

 

 

這(zhe)種(zhong)情(qing)況(kuang)是(shi)有(you)可(ke)能(neng)發(fa)生(sheng)的(de)，例(li)如(ru)，我(wo)們(men)基(ji)於(yu)舊(jiu)的(de)位(wei)置(zhi)來(lai)估(gu)計(ji)新(xin)位(wei)置(zhi)。如(ru)果(guo)速(su)度(du)過(guo)高(gao)，我(wo)們(men)可(ke)能(neng)已(yi)經(jing)移(yi)動(dong)很(hen)遠(yuan)了(le)。如(ru)果(guo)緩(huan)慢(man)移(yi)動(dong)
，則(ze)距(ju)離(li)不(bu)會(hui)很(hen)遠(yuan)。跟(gen)蹤(zong)這(zhe)種(zhong)關(guan)係(xi)是(shi)非(fei)常(chang)重(zhong)要(yao)的(de)
，因(yin)為(wei)它(ta)帶(dai)給(gei)我(wo)們(men)更(geng)多(duo)的(de)信(xin)息(xi)：其中一個測量值告訴了我們其它變量可能的值，這就是卡爾曼濾波的目的，盡可能地在包含不確定性的測量數據中提取更多信息!

 

這種相關性用協方差矩陣來表示
，簡而言之，矩陣中的每個元素表示第 i 個和第 j 個狀態變量之間的相關度。(你可能已經猜到協方差矩陣是一個對稱矩陣，這意味著可以任意交換 i 和 j)。協方差矩陣通常用“”來表示
，其中的元素則表示為“”。

 

 

使用矩陣來描述問題

 

我們基於高斯分布來建立狀態變量，所以在時刻 k 需要兩個信息：最佳估計(即均值，其它地方常用 μ 表示)，以及協方差矩陣。

 

     (1)

 

(當然，在這裏我們隻用到了位置和速度，實際上這個狀態可以包含多個變量


，代表任何你想表示的信息)。接下來，我們需要根據當前狀態(k-1 時刻)來預測下一狀態(k 時刻)。記住，我們並不知道對下一狀態的所有預測中哪個是“真實”的，但我們的預測函數並不在乎。它對所有的可能性進行預測，並給出新的高斯分布。

 

 

我們可以用矩陣來表示這個預測過程：

 

 

它ta將jiang我wo們men原yuan始shi估gu計ji中zhong的de每mei個ge點dian都dou移yi動dong到dao了le一yi個ge新xin的de預yu測ce位wei置zhi，如ru果guo原yuan始shi估gu計ji是shi正zheng確que的de話hua，這zhe個ge新xin的de預yu測ce位wei置zhi就jiu是shi係xi統tong下xia一yi步bu會hui移yi動dong到dao的de位wei置zhi。那na我wo們men又you如ru何he用yong矩ju陣zhen來lai預yu測ce下xia一yi個ge時shi刻ke的de位wei置zhi和he速su度du呢ne?下麵用一個基本的運動學公式來表示：

 

 

現(xian)在(zai)
，我(wo)們(men)有(you)了(le)一(yi)個(ge)預(yu)測(ce)矩(ju)陣(zhen)來(lai)表(biao)示(shi)下(xia)一(yi)時(shi)刻(ke)的(de)狀(zhuang)態(tai)，但(dan)是(shi)，我(wo)們(men)仍(reng)然(ran)不(bu)知(zhi)道(dao)怎(zen)麼(me)更(geng)新(xin)協(xie)方(fang)差(cha)矩(ju)陣(zhen)。此(ci)時(shi)，我(wo)們(men)需(xu)要(yao)引(yin)入(ru)另(ling)一(yi)個(ge)公(gong)式(shi)，如(ru)果(guo)我(wo)們(men)將(jiang)分(fen)布(bu)中(zhong)的(de)每(mei)個(ge)點(dian)都(dou)乘(cheng)以(yi)矩(ju)陣(zhen) A
，那麼它的協方差矩陣會怎樣變化呢?很簡單，下麵給出公式：

 

 

結合方程(4)和(3)得到：

 

 

外部控製量

 

我們並沒有捕捉到一切信息，可能存在外部因素會對係統進行控製，帶來一些與係統自身狀態沒有相關性的改變。

 

以yi火huo車che的de運yun動dong狀zhuang態tai模mo型xing為wei例li，火huo車che司si機ji可ke能neng會hui操cao縱zong油you門men，讓rang火huo車che加jia速su。相xiang同tong地di，在zai我wo們men機ji器qi人ren這zhe個ge例li子zi中zhong，導dao航hang軟ruan件jian可ke能neng會hui發fa出chu一yi個ge指zhi令ling讓rang輪lun子zi轉zhuan向xiang或huo者zhe停ting止zhi。如ru果guo知zhi道dao這zhe些xie額e外wai的de信xin息xi，我wo們men可ke以yi用yong一yi個ge向xiang量liang來表示

，將它加到我們的預測方程中做修正。

 

假設由於油門的設置或控製命令
，我們知道了期望的加速度
，根據基本的運動學方程可以得到：

 

 

以矩陣的形式表示就是：

 

 

稱為控製矩陣
，稱為控製向量(對於沒有外部控製的簡單係統來說
，這部分可以忽略)。讓我們再思考一下，如果我們的預測並不是100%準確的
，該怎麼辦呢?

 

外部幹擾

 

如果這些狀態量是基於係統自身的屬性或者已知的外部控製作用來變化的，則不會出現什麼問題。

 

但是
，如果存在未知的幹擾呢?liru
，jiashewomengenzongyigesixuanyifeixingqi
，takenenghuishoudaofengdeganrao，ruguowomengenzongyigelunshijiqiren，lunzikenenghuidahua，huozhelumianshangdexiaopohuirangtajiansu。zheyangdehuawomenjiubunengjixuduizhexiezhuangtaijinxinggenzong，ruguomeiyoubazhexiewaibuganraokaolvzainei，womendeyucejiuhuichuxianpiancha。

 

在每次預測之後
，我們可以添加一些新的不確定性來建立這種與“外界”(即我們沒有跟蹤的幹擾)之間的不確定性模型：

 

 

原始估計中的每個狀態變量更新到新的狀態後
，仍然服從高斯分布。我們可以說的每個狀態變量移動到了一個新的服從高斯分布的區域
，協方差為。換句話說就是
，我們將這些沒有被跟蹤的幹擾當作協方差為的噪聲來處理。

 

 

這產生了具有不同協方差(但是具有相同的均值)的新的高斯分布。

 

 

我們通過簡單地添加得到擴展的協方差，下麵給出預測步驟的完整表達式：

 

 

由上式可知，新的最優估計是根據上一最優估計預測得到的
，並加上已知外部控製量的修正。

 

而新的不確定性由上一不確定性預測得到
，並加上外部環境的幹擾。

 

好了，我們對係統可能的動向有了一個模糊的估計，用和來表示。如果再結合傳感器的數據會怎樣呢?

 

用測量值來修正估計值

 

我(wo)們(men)可(ke)能(neng)會(hui)有(you)多(duo)個(ge)傳(chuan)感(gan)器(qi)來(lai)測(ce)量(liang)係(xi)統(tong)當(dang)前(qian)的(de)狀(zhuang)態(tai)，哪(na)個(ge)傳(chuan)感(gan)器(qi)具(ju)體(ti)測(ce)量(liang)的(de)是(shi)哪(na)個(ge)狀(zhuang)態(tai)變(bian)量(liang)並(bing)不(bu)重(zhong)要(yao)，也(ye)許(xu)一(yi)個(ge)是(shi)測(ce)量(liang)位(wei)置(zhi)
，一(yi)個(ge)是(shi)測(ce)量(liang)速(su)度(du)
，每(mei)個(ge)傳(chuan)感(gan)器(qi)間(jian)接(jie)地(di)告(gao)訴(su)了(le)我(wo)們(men)一(yi)些(xie)狀(zhuang)態(tai)信(xin)息(xi)。

 

 

注意，傳感器讀取的數據的單位和尺度有可能與我們要跟蹤的狀態的單位和尺度不一樣
，我們用矩陣來表示傳感器的數據。

 

 

我們可以計算出傳感器讀數的分布，用之前的表示方法如下式所示：

 

 

卡ka爾er曼man濾lv波bo的de一yi大da優you點dian就jiu是shi能neng處chu理li傳chuan感gan器qi噪zao聲sheng
，換huan句ju話hua說shuo，我wo們men的de傳chuan感gan器qi或huo多duo或huo少shao都dou有you點dian不bu可ke靠kao，並bing且qie原yuan始shi估gu計ji中zhong的de每mei個ge狀zhuang態tai可ke以yi和he一yi定ding範fan圍wei內nei的de傳chuan感gan器qi讀du數shu對dui應ying起qi來lai。

 

 

從(cong)測(ce)量(liang)到(dao)的(de)傳(chuan)感(gan)器(qi)數(shu)據(ju)中(zhong)，我(wo)們(men)大(da)致(zhi)能(neng)猜(cai)到(dao)係(xi)統(tong)當(dang)前(qian)處(chu)於(yu)什(shen)麼(me)狀(zhuang)態(tai)。但(dan)是(shi)由(you)於(yu)存(cun)在(zai)不(bu)確(que)定(ding)性(xing)

，某(mou)些(xie)狀(zhuang)態(tai)可(ke)能(neng)比(bi)我(wo)們(men)得(de)到(dao)的(de)讀(du)數(shu)更(geng)接(jie)近(jin)真(zhen)實(shi)狀(zhuang)態(tai)。

 

 

我們將這種不確定性(例如：傳感器噪聲)用協方差表示，該分布的均值就是我們讀取到的傳感器數據
，稱之為。

 

現在我們有了兩個高斯分布
，一個是在預測值附近，一個是在傳感器讀數附近。

 

 

我們必須在預測值(粉紅色)和傳感器測量值(綠色)之間找到最優解。

 

那麼，我們最有可能的狀態是什麼呢?對於任何可能的讀數
，有兩種情況：(1)傳感器的測量值;(2)由前一狀態得到的預測值。如果我們想知道這兩種情況都可能發生的概率
，將這兩個高斯分布相乘就可以了。

 

 

剩下的就是重疊部分了，這個重疊部分的均值就是兩個估計最可能的值，也就是給定的所有信息中的最優估計。

 

瞧!這個重疊的區域看起來像另一個高斯分布。

 

 

如你所見

，把兩個具有不同均值和方差的高斯分布相乘，你會得到一個新的具有獨立均值和方差的高斯分布!下麵用公式講解。

 

融合高斯分布

 

先以一維高斯分布來分析比較簡單點，具有方差和 μ 的高斯曲線可以用下式表示：

 

 

如果把兩個服從高斯分布的函數相乘會得到什麼呢?

 

 

將式(9)代入到式(10)中(注意重新歸一化，使總概率為1)可以得到：

 

 

將式(11)中的兩個式子相同的部分用 k 表示：

 

 

下麵進一步將式(12)和(13)寫成矩陣的形式，如果 Σ 表示高斯分布的協方差，表示每個維度的均值
，則：

 

 

矩陣稱為卡爾曼增益，下麵將會用到。放鬆!我們快要完成了!

 

將所有公式整合起來

 

我們有兩個高斯分布
，預測部分，和測量部分，將它們放到式(15)中算出它們之間的重疊部分：

 

 

由式(14)可得卡爾曼增益為：

 

 

將式(16)和式(17)的兩邊同時左乘矩陣的逆(注意裏麵包含了)將其約掉，再將式(16)的第二個等式兩邊同時右乘矩陣的逆得到以下等式：

 

 

上式給出了完整的更新步驟方程。就是新的最優估計，我們可以將它和放到下一個預測和更新方程中不斷迭代。

 

 

總結

 

以上所有公式中

，你隻需要用到式(7)、(18)
、(19)。(如果忘了的話，你可以根據式(4)和(15)重新推導一下)

 

我們可以用這些公式對任何線性係統建立精確的模型，對於非線性係統來說，我們使用擴展卡爾曼濾波
，區別在於EKF多了一個把預測和測量部分進行線性化的過程。

 

本文轉載自電子工程專輯。

 

 

推薦閱讀：