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

 

什麼是卡爾曼濾波?

 

你ni可ke以yi在zai任ren何he含han有you不bu確que定ding信xin息xi的de動dong態tai係xi統tong中zhong使shi用yong卡ka爾er曼man濾lv波bo，對dui係xi統tong下xia一yi步bu的de走zou向xiang做zuo出chu有you根gen據ju的de預yu測ce，即ji使shi伴ban隨sui著zhe各ge種zhong幹gan擾rao，卡ka爾er曼man濾lv波bo總zong是shi能neng指zhi出chu真zhen實shi發fa生sheng的de情qing況kuang。

 

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

 

在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)。

 

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

 

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

 

 

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

 

 

注(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：takenengshoudaofengchuideyingxiang，lunzifangxiangpianleyidian
，huozheyudaobupingdedimianerfandao。suoyi，lunzizhuanguodechangdubingbunengjingquebiaoshijiqirenshijixingzoudejuli
，yuceyebushihenwanmei。

 

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 時刻)。記住，我們並不知道對下一狀態的所有預測中哪個是“真實”的
，但我們的預測函數並不在乎。它對所有的可能性進行預測，並給出新的高斯分布。

 

 

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

 

 

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

 

 

xianzai，womenyouleyigeyucejuzhenlaibiaoshixiayishikedezhuangtai
，danshi，womenrengranbuzhidaozenmegengxinxiefangchajuzhen。cishi，womenxuyaoyinrulingyigegongshi，ruguowomenjiangfenbuzhongdemeigediandouchengyijuzhen 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%準確的
，該怎麼辦呢?

 

外部幹擾

 

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

 

但是

，如果存在未知的幹擾呢?例(li)如(ru)
，假(jia)設(she)我(wo)們(men)跟(gen)蹤(zong)一(yi)個(ge)四(si)旋(xuan)翼(yi)飛(fei)行(xing)器(qi)

，它(ta)可(ke)能(neng)會(hui)受(shou)到(dao)風(feng)的(de)幹(gan)擾(rao)，如(ru)果(guo)我(wo)們(men)跟(gen)蹤(zong)一(yi)個(ge)輪(lun)式(shi)機(ji)器(qi)人(ren)，輪(lun)子(zi)可(ke)能(neng)會(hui)打(da)滑(hua)，或(huo)者(zhe)路(lu)麵(mian)上(shang)的(de)小(xiao)坡(po)會(hui)讓(rang)它(ta)減(jian)速(su)。這(zhe)樣(yang)的(de)話(hua)我(wo)們(men)就(jiu)不(bu)能(neng)繼(ji)續(xu)對(dui)這(zhe)些(xie)狀(zhuang)態(tai)進(jin)行(xing)跟(gen)蹤(zong)，如(ru)果(guo)沒(mei)有(you)把(ba)這(zhe)些(xie)外(wai)部(bu)幹(gan)擾(rao)考(kao)慮(lv)在(zai)內(nei)，我(wo)們(men)的(de)預(yu)測(ce)就(jiu)會(hui)出(chu)現(xian)偏(pian)差(cha)。

 

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

 

 

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

 

 

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

 

 

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

 

 

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

 

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

 

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

 

用測量值來修正估計值

 

我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。

 

 

congceliangdaodechuanganqishujuzhong，womendazhinengcaidaoxitongdangqianchuyushenmezhuangtai。danshiyouyucunzaibuquedingxing，mouxiezhuangtaikenengbiwomendedaodedushugengjiejinzhenshizhuangtai。

 

 

我們將這種不確定性(例如：傳感器噪聲)用協方差表示

，該分布的均值就是我們讀取到的傳感器數據，稱之為。

 

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

 

 

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

 

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

 

 

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

 

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

 

 

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

 

融合高斯分布

 

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

 

 

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

 

 

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

 

 

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

 

 

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

 

 

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

 

將所有公式整合起來

 

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

 

 

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

 

 

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

 

 

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

 

 

總結

 

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

 

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

 

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

 

 

推薦閱讀：