點(diǎn)擊上方“小白學(xué)視覺”，選擇加"星標(biāo)"或“置頂”

重磅干貨，第一時(shí)間送達(dá)

在SLAM系統(tǒng)中，后端優(yōu)化部分有兩大流派。

一派是基于馬爾科夫性假設(shè)的濾波器方法，認(rèn)為當(dāng)前時(shí)刻的狀態(tài)只與上一時(shí)刻的狀態(tài)有關(guān)。另一派是非線性優(yōu)化方法，認(rèn)為當(dāng)前時(shí)刻狀態(tài)應(yīng)該結(jié)合之前所有時(shí)刻的狀態(tài)一起考慮。

如果采用濾波器方法，那一定會(huì)聽到一個(gè)如雷貫耳的名字——卡爾曼濾波(Kalman Filtering)。

我聽到過這個(gè)名字已經(jīng)很久了，可是一直沒有花時(shí)間弄懂究竟是什么東西，最近看了一些資料，就來總結(jié)一下，看看卡爾曼大佬究竟濾了誰？管中窺豹，還請(qǐng)多多指教！

在了解卡爾曼濾波之前，本科就學(xué)過一些濾波的方法，也就是從混合在一起的信號(hào)里提取出所需要的信號(hào)。

就好比吃辣子雞丁時(shí)只把雞丁挑出來啃光，而辣椒被你拋棄掉了！

例如，在模擬電路中，我們利用低通濾波的方法，可以將信號(hào)中摻雜的一些頻率較高的噪聲濾除掉，從而提取有效的較低頻信號(hào)。

同樣地，根據(jù)不同需要，還有高通濾波、帶通濾波和帶阻濾波。

再比如，在圖像處理中，如果存在椒鹽噪聲（不能吃的！是在圖像中隨機(jī)出現(xiàn)的黑白點(diǎn)），我們會(huì)選擇用中值濾波，將n*n個(gè)像素值的中值取出來以代替中心點(diǎn)的值，這樣就能濾除椒鹽噪聲。

但是，卡爾曼濾波和上述說的這些濾波略顯不同，它并沒有很直觀地從一些信號(hào)或者數(shù)據(jù)里面提取某些信號(hào)或數(shù)據(jù)。

它是在有干擾的條件下，通過數(shù)據(jù)的結(jié)合得到相對(duì)更準(zhǔn)確的估計(jì)數(shù)據(jù)。

卡爾曼濾波全程只關(guān)注兩個(gè)東西，一個(gè)是估計(jì)的最佳值，另一個(gè)是該值的不確定性（此處聯(lián)想一下高斯分布的兩個(gè)參數(shù)）。

打個(gè)比方，假設(shè)你蒙著眼睛在屋子里走，要從客廳走到臥室，你可以通過數(shù)步數(shù)來預(yù)測(cè)你當(dāng)前所在的位置。

但是，因?yàn)槟忝看芜~步的幅度和方向不是精準(zhǔn)的，所以你每多走一步所估計(jì)位置的不確定性就會(huì)越來越大，最后有可能走到浴室去了。

如果利用卡爾曼濾波，那么你對(duì)自己每一步的位置估計(jì)就會(huì)準(zhǔn)確很多，具體怎么做呢？賣個(gè)關(guān)子，后面再講，先解釋一下什么是狀態(tài)估計(jì)。

在SLAM中，運(yùn)用卡爾曼濾波是為了狀態(tài)估計(jì)，那什么才是需要估計(jì)的狀態(tài)呢？

狀態(tài)可以看作是機(jī)器人或者環(huán)境中可能會(huì)對(duì)未來產(chǎn)生某些變化的因素。

比如說機(jī)器人的位姿R和t、機(jī)器人的運(yùn)動(dòng)速度v(這個(gè)在純視覺SLAM中是沒有的)、環(huán)境中的路標(biāo)點(diǎn)l等等，不同的SLAM系統(tǒng)擁有不同的狀態(tài)。

我們假設(shè)x_k為機(jī)器人的狀態(tài)，在SLAM的整個(gè)過程中，我們能獲取到兩種數(shù)據(jù)：控制數(shù)據(jù)和測(cè)量數(shù)據(jù)。

• 控制數(shù)據(jù)是機(jī)器人記錄自身運(yùn)動(dòng)的傳感器獲取的數(shù)據(jù)，比如IMU中的陀螺儀可以測(cè)量角速度、加速度計(jì)可以測(cè)量運(yùn)動(dòng)的加速度。

• 測(cè)量數(shù)據(jù)則是機(jī)器人記錄環(huán)境信息的傳感器獲取的數(shù)據(jù)，比如相機(jī)可以將環(huán)境轉(zhuǎn)化為二維的圖像像素、激光雷達(dá)捕捉環(huán)境中的信息生成點(diǎn)云。

假設(shè)控制數(shù)據(jù)為u_k，運(yùn)動(dòng)噪聲epsilon_k為，那么機(jī)器人的運(yùn)動(dòng)可以用一個(gè)運(yùn)動(dòng)方程來表達(dá)。

因?yàn)榧僭O(shè)了馬爾科夫性，所以當(dāng)前時(shí)刻狀態(tài)只與上一時(shí)刻有關(guān)，它表示從k-1時(shí)刻到k時(shí)刻機(jī)器人狀態(tài)發(fā)生了怎樣的變化。

假設(shè)測(cè)量數(shù)據(jù)為z_k，測(cè)量噪聲為delta_k，那么機(jī)器人的測(cè)量可以用一個(gè)觀測(cè)方程來表達(dá)，它表示在k時(shí)刻所在的位置觀測(cè)到路標(biāo)點(diǎn)產(chǎn)生測(cè)量數(shù)據(jù)。

那么，狀態(tài)估計(jì)其實(shí)就是用過去的數(shù)據(jù)來估計(jì)當(dāng)前的狀態(tài)。

由于狀態(tài)不是直接得到的，而且方程還受到噪聲的影響，因此狀態(tài)其實(shí)是符合某種概率分布的隨機(jī)變量，而狀態(tài)估計(jì)實(shí)際上是估計(jì)當(dāng)前的狀態(tài)分布。

我們用置信度bel(x_k)來表示當(dāng)前時(shí)刻的狀態(tài)分布，它是以控制數(shù)據(jù)和測(cè)量數(shù)據(jù)為條件的后驗(yàn)概率，即

而在獲得當(dāng)前時(shí)刻測(cè)量數(shù)據(jù)之前的狀態(tài)分布可以用橫杠bel(x_k)來表示，它表示的后驗(yàn)概率為

同時(shí)，如果用概率來表達(dá)運(yùn)動(dòng)過程的話，則是

這表示了從k-1時(shí)刻到k時(shí)刻機(jī)器人的狀態(tài)轉(zhuǎn)移概率。

而用概率來表達(dá)觀測(cè)過程，則是

它表示第k時(shí)刻機(jī)器人的測(cè)量概率。

所以，如下圖所示，每一時(shí)刻的狀態(tài)x_k只與前一時(shí)刻的狀態(tài)x_k-1、當(dāng)前時(shí)刻的控制u_k有關(guān)，而每一時(shí)刻的測(cè)量z_k只與當(dāng)前時(shí)刻的狀態(tài)x_k有關(guān)。

這其實(shí)就是一個(gè)隱馬爾可夫模型，狀態(tài)不能直接得到，但是可以通過測(cè)量觀察到。

有了狀態(tài)分布的表達(dá)方式，還有運(yùn)動(dòng)方程和觀測(cè)方法的概率表示，接下來就可以名正言順地獻(xiàn)上著名的貝葉斯公式了(前方多式警告！)

因?yàn)榉帜负蜖顟B(tài)沒有半毛錢關(guān)系，因此可以用一個(gè)比例因子eta來表示，即

根據(jù)前面說到的置信度和觀測(cè)方程的概率表示(測(cè)量概率)，該式還可以表示為

對(duì)于狀態(tài)分布橫杠bel(x_k)，用邊際概率公式可以得到

根據(jù)前一時(shí)刻的置信度和運(yùn)動(dòng)方程的概率表示(狀態(tài)轉(zhuǎn)移概率)，同時(shí)狀態(tài)x_k-1與u_k不相關(guān)，因此上式化簡(jiǎn)得

可以看出，這是一個(gè)遞歸的過程，每一時(shí)刻的狀態(tài)分布根據(jù)前一時(shí)刻的狀態(tài)分布計(jì)算得到，一直追溯到初始狀態(tài)x_0。

于是，我們就可以得到貝葉斯濾波算法了。

首先，根據(jù)上一時(shí)刻的狀態(tài)分布，機(jī)器人經(jīng)過運(yùn)動(dòng)方程的狀態(tài)轉(zhuǎn)移概率進(jìn)行預(yù)測(cè)，得到綜合測(cè)量數(shù)據(jù)前的當(dāng)前時(shí)刻狀態(tài)分布。

然后，通過觀測(cè)方程將測(cè)量數(shù)據(jù)考慮進(jìn)來，再對(duì)狀態(tài)分布進(jìn)行調(diào)整更新，得到最當(dāng)前時(shí)刻最終的狀態(tài)估計(jì)。

因此，只要知道初始狀態(tài)分布、運(yùn)動(dòng)方程的狀態(tài)轉(zhuǎn)移概率和觀測(cè)方程的測(cè)量概率，貝葉斯濾波就可以濾起來了！

呼！有了前面一堆的鋪墊之后，終于迎來了重頭戲卡爾曼濾波。在這里先附上大牛的照片以表敬意，要知道當(dāng)年設(shè)計(jì)出的卡爾曼濾波器可是要上天的！

其實(shí)理解了貝葉斯濾波之后，卡爾曼濾波也不難明白。

因?yàn)榭柭鼮V波是一種特殊的貝葉斯濾波，它假定系統(tǒng)是線性高斯的，也就是說卡爾曼濾波=貝葉斯濾波+線性高斯系統(tǒng)。

這是什么意思呢？還記得前面提及的運(yùn)動(dòng)方程嗎，它在線性系統(tǒng)中的表達(dá)為

而觀測(cè)方程在線性系統(tǒng)中的表達(dá)為

與此同時(shí)，運(yùn)動(dòng)噪聲和測(cè)量噪聲都是隨機(jī)高斯噪聲，即

因此，運(yùn)動(dòng)方程的狀態(tài)轉(zhuǎn)移概率和觀測(cè)方程的測(cè)量概率都相應(yīng)地滿足高斯分布

由于初始狀態(tài)分布也要滿足高斯分布，而且高斯分布相乘依然為高斯分布，所以在整個(gè)遞歸的濾波過程中，狀態(tài)估計(jì)始終滿足高斯分布，Amazing！

還記得貝葉斯濾波一直維護(hù)更新的是狀態(tài)分布嗎？在卡爾曼濾波中也是如此。

只不過因?yàn)榭柭鼮V波應(yīng)用在線性高斯系統(tǒng)中，狀態(tài)分布都滿足高斯分布，因此卡爾曼濾波關(guān)心的是均值和方差。

因此，卡爾曼濾波算法過程為

可以看到，卡爾曼濾波和貝葉斯濾波一樣也是分為兩個(gè)步驟。

先是根據(jù)前一時(shí)刻狀態(tài)分布的均值和方差還有控制數(shù)據(jù)預(yù)測(cè)當(dāng)前時(shí)刻的均值和方差，然后再根據(jù)測(cè)量數(shù)據(jù)調(diào)整更新當(dāng)前時(shí)刻最終的均值和方差。

只不過卡爾曼濾波多了一個(gè)求卡爾曼增益K_k的過程?？柭鼮V波和貝葉斯濾波的對(duì)比如下圖

由于篇幅原因，就不進(jìn)行公式推導(dǎo)了。如果覺得不夠直觀，那么就看一個(gè)栗子，用圖來解釋一下。

下面就用圖來解釋一下卡爾曼濾波，能有個(gè)更直觀的感受。

首先通過上一時(shí)刻的狀態(tài)預(yù)測(cè)得到當(dāng)前時(shí)刻的狀態(tài)分布(圖a)，然后通過傳感器得到測(cè)量數(shù)據(jù)(圖b加粗)。

結(jié)合測(cè)量數(shù)據(jù)調(diào)整更新，得到當(dāng)前時(shí)刻最終的狀態(tài)分布(圖c加粗)。然后通過控制數(shù)據(jù)，接著預(yù)測(cè)下一時(shí)刻的狀態(tài)分布(圖d加粗)。

獲取下一時(shí)刻的測(cè)量數(shù)據(jù)之后(圖e加粗)，綜合得到下一時(shí)刻估計(jì)的狀態(tài)分布(圖f加粗)。

到這，你知道卡爾曼濾波究竟濾了誰嗎？

在我看來，卡爾曼濾波可以看作是，通過測(cè)量數(shù)據(jù)將僅由控制數(shù)據(jù)進(jìn)行狀態(tài)估計(jì)而帶來不斷提高的噪聲(不確定性)濾除掉。同時(shí)，它更像是一種數(shù)據(jù)(傳感器)融合的方法。

還記得文章前面讓你蒙著眼在屋子里走嗎？學(xué)了卡爾曼濾波之后應(yīng)該知道怎么做能讓你更準(zhǔn)確地知道當(dāng)前位置了吧？很簡(jiǎn)單，那就是睜開眼走路！

眼睛看到室內(nèi)環(huán)境就相當(dāng)于測(cè)量數(shù)據(jù)，綜合眼睛看到的景象就會(huì)讓你對(duì)自己所在的位置判斷更準(zhǔn)確啦。

當(dāng)然，如果你的鼻子夠靈，可以通過氣味判斷，或者有順風(fēng)耳可以聽到浴室滴水從而避免掉坑也是可以的！

我在知乎上也看到知友Kent Zeng對(duì)卡爾曼濾波有更入木三分的見解：

假設(shè)你有兩個(gè)傳感器，測(cè)的是同一個(gè)信號(hào)。可是它們每次的讀數(shù)都不太一樣，怎么辦？
—— 取平均。
再假設(shè)你知道其中貴的那個(gè)傳感器應(yīng)該準(zhǔn)一些，便宜的那個(gè)應(yīng)該差一些。那有比取平均更好的辦法嗎？
——加權(quán)平均。
怎么加權(quán)？假設(shè)兩個(gè)傳感器的誤差都符合正態(tài)分布，假設(shè)你知道這兩個(gè)正態(tài)分布的方差，用這兩個(gè)方差值，（此處省略若干數(shù)學(xué)公式），你可以得到一個(gè)“最優(yōu)”的權(quán)重。
接下來，重點(diǎn)來了：假設(shè)你只有一個(gè)傳感器，但是你還有一個(gè)數(shù)學(xué)模型。模型可以幫你算出一個(gè)值，但也不是那么準(zhǔn)。怎么辦？
—— 把模型算出來的值，和傳感器測(cè)出的值，（就像兩個(gè)傳感器那樣），取加權(quán)平均。
OK，最后一點(diǎn)說明：你的模型其實(shí)只是一個(gè)步長(zhǎng)的，也就是說，知道x(k)，我可以求x(k+1)。問題是x(k)是多少呢？答案：x(k)就是你上一步卡爾曼濾波得到的、所謂加權(quán)平均之后的那個(gè)、對(duì)x在k時(shí)刻的最佳估計(jì)值。
于是迭代也有了。

在此膜拜一下大佬！

想把卡爾曼濾波吃透不容易，但如果打算用濾波作為SLAM的后端部分，那還有大堆卡爾曼濾波的變體在撲向你~

看完卡爾曼濾波后，耳邊不禁響起一句:

最后，祝大家濾波開心！

好消息，小白學(xué)視覺團(tuán)隊(duì)的知識(shí)星球開通啦，為了感謝大家的支持與厚愛，團(tuán)隊(duì)決定將價(jià)值149元的知識(shí)星球現(xiàn)時(shí)免費(fèi)加入。各位小伙伴們要抓住機(jī)會(huì)哦！

交流群

歡迎加入公眾號(hào)讀者群一起和同行交流，目前有SLAM、三維視覺、傳感器、自動(dòng)駕駛、計(jì)算攝影、檢測(cè)、分割、識(shí)別、醫(yī)學(xué)影像、GAN、算法競(jìng)賽等微信群（以后會(huì)逐漸細(xì)分），請(qǐng)掃描下面微信號(hào)加群，備注：”昵稱+學(xué)校/公司+研究方向“，例如：”張三 + 上海交大 + 視覺SLAM“。請(qǐng)按照格式備注，否則不予通過。添加成功后會(huì)根據(jù)研究方向邀請(qǐng)進(jìn)入相關(guān)微信群。請(qǐng)勿在群內(nèi)發(fā)送廣告，否則會(huì)請(qǐng)出群，謝謝理解~