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

重磅干貨，第一時間送達

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

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

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

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

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

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

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

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

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

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

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

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

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

但是，因為你每次邁步的幅度和方向不是精準的，所以你每多走一步所估計位置的不確定性就會越來越大，最后有可能走到浴室去了。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

同時，如果用概率來表達運動過程的話，則是

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

而用概率來表達觀測過程，則是

它表示第k時刻機器人的測量概率。

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

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

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

因為分母和狀態(tài)沒有半毛錢關(guān)系，因此可以用一個比例因子eta來表示，即

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

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

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

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

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

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

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

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

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

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

因為卡爾曼濾波是一種特殊的貝葉斯濾波，它假定系統(tǒng)是線性高斯的，也就是說卡爾曼濾波=貝葉斯濾波+線性高斯系統(tǒng)。

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

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

與此同時，運動噪聲和測量噪聲都是隨機高斯噪聲，即

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

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

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

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

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

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

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

只不過卡爾曼濾波多了一個求卡爾曼增益K_k的過程。卡爾曼濾波和貝葉斯濾波的對比如下圖

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

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

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

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

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

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

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

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

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

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

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

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

在此膜拜一下大佬！

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

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

最后，祝大家濾波開心！

交流群

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