我不得不說說卡爾曼濾波
，因為它能做到的事情簡直讓人驚歎!意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。

 

在連續變化的係統中使用卡爾曼濾波是非常理想的，它具有占用內存小的優點(除了前一個狀態量外，不需要保留其它曆史數據)，並且速度很快，很適合應用於實時問題和嵌入式係統。

 

在Google上shang找zhao到dao的de大da多duo數shu關guan於yu實shi現xian卡ka爾er曼man濾lv波bo的de數shu學xue公gong式shi看kan起qi來lai有you點dian晦hui澀se難nan懂dong


，這zhe個ge狀zhuang況kuang有you點dian糟zao糕gao。實shi際ji上shang，如ru果guo以yi正zheng確que的de方fang式shi看kan待dai它ta

，卡ka爾er曼man濾lv波bo是shi非fei常chang簡jian單dan和he容rong易yi理li解jie的de，下xia麵mian我wo將jiang用yong漂piao亮liang的de圖tu片pian和he色se彩cai清qing晰xi的de闡chan述shu它ta，你ni隻zhi需xu要yao懂dong一yi些xie基ji本ben的de概gai率lv和he矩ju陣zhen的de知zhi識shi就jiu可ke以yi了le。

 

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

 

用玩具舉例：你開發了一個可以在樹林裏到處跑的小機器人

，這個機器人需要知道它所在的確切位置才能導航。

 

 

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

 

 

zhuyizhegezhuangtaizhishiguanyuzhegexitongjibenshuxingdeyiduishuzi，takeyishirenheqitadedongxi。zaizhegelizizhongshiweizhihesudu
，tayekeyishiyigerongqizhongyetidezongliang，qichefadongjidewendu，yonghushouzhizaichumobanshangdeweizhizuobiao
，huozherenhenixuyaogenzongdexinhao。

 

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

 

 

或許我們知道一些機器人如何運動的信息：例(li)如(ru)，機(ji)器(qi)人(ren)知(zhi)道(dao)發(fa)送(song)給(gei)電(dian)機(ji)的(de)指(zhi)令(ling)，知(zhi)道(dao)自(zi)己(ji)是(shi)否(fou)在(zai)朝(chao)一(yi)個(ge)方(fang)向(xiang)移(yi)動(dong)並(bing)且(qie)沒(mei)有(you)人(ren)幹(gan)預(yu)，在(zai)下(xia)一(yi)個(ge)狀(zhuang)態(tai)，機(ji)器(qi)人(ren)很(hen)可(ke)能(neng)朝(chao)著(zhe)相(xiang)同(tong)的(de)方(fang)向(xiang)移(yi)動(dong)。當(dang)然(ran)
，機(ji)器(qi)人(ren)對(dui)自(zi)己(ji)的(de)運(yun)動(dong)是(shi)一(yi)無(wu)所(suo)知(zhi)的(de)：它(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 chuanganqigaosulewomenyixiezhuangtaixinxi

，womendeyucegaosulewomenjiqirenhuizenyangyundong，dandouzhishijianjiede，bingqiebansuizheyixiebuquedinghebuzhunquexing。danshi，ruguoshiyongsuoyouduiwomenkeyongdexinxi，womennengdedaoyigebirenheyijuzishengujigenghaodejieguoma?回答當然是YES
，這就是卡爾曼濾波的用處。

 

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

 

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

 

 

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

 

 

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

 

 

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

，觀測特定位置的可能性取決於當前的速度：

 

 

zhezhongqingkuangshiyoukenengfashengde，liru
，womenjiyujiudeweizhilaigujixinweizhi。ruguosuduguogao

，womenkenengyijingyidonghenyuanle。ruguohuanmanyidong，zejulibuhuihenyuan。genzongzhezhongguanxishifeichangzhongyaode，yinweitadaigeiwomengengduodexinxi：其中一個測量值告訴了我們其它變量可能的值，這就是卡爾曼濾波的目的，盡可能地在包含不確定性的測量數據中提取更多信息!

 

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

 

 

使用矩陣來描述問題

 

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

 

     (1)

 

(當然，在這裏我們隻用到了位置和速度，實際上這個狀態可以包含多個變量，代表任何你想表示的信息)。接下來，我們需要根據當前狀態(k-1 時刻)來預測下一狀態(k 時刻)。記住
，我們並不知道對下一狀態的所有預測中哪個是“真實”的，但我們的預測函數並不在乎。它對所有的可能性進行預測
，並給出新的高斯分布。

 

 

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

 

 

tajiangwomenyuanshigujizhongdemeigediandouyidongdaoleyigexindeyuceweizhi
，ruguoyuanshigujishizhengquedehua，zhegexindeyuceweizhijiushixitongxiayibuhuiyidongdaodeweizhi。nawomenyouruheyongjuzhenlaiyucexiayigeshikedeweizhihesudune?下麵用一個基本的運動學公式來表示：

 

 

現(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)。

 

 

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

 

 

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

 

 

kaermanlvbodeyidayoudianjiushinengchulichuanganqizaosheng，huanjuhuashuo，womendechuanganqihuoduohuoshaodouyoudianbukekao，bingqieyuanshigujizhongdemeigezhuangtaikeyiheyidingfanweineidechuanganqidushuduiyingqilai。

 

 

從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多了一個把預測和測量部分進行線性化的過程。

 

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

 

 

推薦閱讀：