我不得不說說卡爾曼濾波
，因為它能做到的事情簡直讓人驚歎!yiwaideshihenshaoyouruanjiangongchengshihekexuejiaduiduitayousuolejie，zherangwogandaojusang，yinweikaermanlvboshiyigeruciqiangdadegongju
，nenggouzaibuquedingxingzhongronghexinxi，yucitongshi，tatiqujingquexinxidenenglikanqilaibukesiyi。

 

什麼是卡爾曼濾波?

 

你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是不夠的。

 

 

或許我們知道一些機器人如何運動的信息：例(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 傳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

，這就是卡爾曼濾波的用處。

 

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

 

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

 

 

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

 

 

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

，表示不確定性。

 

 

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

 

 

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

 

這種相關性用協方差矩陣來表示，簡而言之，矩陣中的每個元素表示第 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%準確的
，該怎麼辦呢?

 

外部幹擾

 

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

 

但是，如果存在未知的幹擾呢?例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。

 

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

 

 

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

 

 

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

 

 

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

 

 

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

 

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

 

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

 

用測量值來修正估計值

 

womenkenenghuiyouduogechuanganqilaiceliangxitongdangqiandezhuangtai
，nagechuanganqijuticeliangdeshinagezhuangtaibianliangbingbuzhongyao
，yexuyigeshiceliangweizhi，yigeshiceliangsudu
，meigechuanganqijianjiedigaosulewomenyixiezhuangtaixinxi。

 

 

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

 

 

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

 

 

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

 

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

 

 

推薦閱讀：