【SLAM】卡爾曼濾波：究竟濾了誰？

• 2019 年 12 月 24 日
• 筆記

一派是基於馬爾科夫性假設的濾波器方法，認為當前時刻的狀態只與上一時刻的狀態有關。另一派是非線性優化方法，認為當前時刻狀態應該結合之前所有時刻的狀態一起考慮。

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

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

1 什麼是濾波？

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

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

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

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

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

但是，卡爾曼濾波和上述說的這些濾波略顯不同，它並沒有很直觀地從一些訊號或者數據裡面提取某些訊號或數據。

它是在有干擾的條件下，通過數據的結合得到相對更準確的估計數據。

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

打個比方，假設你蒙著眼睛在屋子裡走，要從客廳走到卧室，你可以通過數步數來預測你當前所在的位置。

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

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

2 狀態估計

在SLAM中，運用卡爾曼濾波是為了狀態估計，那什麼才是需要估計的狀態呢？

狀態可以看作是機器人或者環境中可能會對未來產生某些變化的因素。

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

我們假設x_k為機器人的狀態，在SLAM的整個過程中，我們能獲取到兩種數據：控制數據和測量數據。

• 控制數據是機器人記錄自身運動的感測器獲取的數據，比如IMU中的陀螺儀可以測量角速度、加速度計可以測量運動的加速度。
• 測量數據則是機器人記錄環境資訊的感測器獲取的數據，比如相機可以將環境轉化為二維的影像像素、光學雷達捕捉環境中的資訊生成點雲。

假設控制數據為u_k，運動雜訊epsilon_k為，那麼機器人的運動可以用一個運動方程來表達。

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

假設測量數據為z_k，測量雜訊為delta_k，那麼機器人的測量可以用一個觀測方程來表達，它表示在k時刻所在的位置觀測到路標點產生測量數據。

那麼，狀態估計其實就是用過去的數據來估計當前的狀態。

由於狀態不是直接得到的，而且方程還受到雜訊的影響，因此狀態其實是符合某種概率分布的隨機變數，而狀態估計實際上是估計當前的狀態分布。

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

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

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

這表示了從k-1時刻到k時刻機器人的狀態轉移概率。

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

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

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

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

3 貝葉斯濾波

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

因為分母和狀態沒有半毛錢關係，因此可以用一個比例因子eta來表示，即

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

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

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

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

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

首先，根據上一時刻的狀態分布，機器人經過運動方程的狀態轉移概率進行預測，得到綜合測量數據前的當前時刻狀態分布。

然後，通過觀測方程將測量數據考慮進來，再對狀態分布進行調整更新，得到最當前時刻最終的狀態估計。

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

4 卡爾曼濾波

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

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

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

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

而觀測方程在線性系統中的表達為

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

因此，運動方程的狀態轉移概率和觀測方程的測量概率都相應地滿足高斯分布

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

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

只不過因為卡爾曼濾波應用在線性高斯系統中，狀態分布都滿足高斯分布，因此卡爾曼濾波關心的是均值和方差。

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

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

先是根據前一時刻狀態分布的均值和方差還有控制數據預測當前時刻的均值和方差，然後再根據測量數據調整更新當前時刻最終的均值和方差。

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

由於篇幅原因，就不進行公式推導了。如果覺得不夠直觀，那麼就看一個栗子，用圖來解釋一下。

5 舉個栗子

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

首先通過上一時刻的狀態預測得到當前時刻的狀態分布(圖a)，然後通過感測器得到測量數據(圖b加粗)。

結合測量數據調整更新，得到當前時刻最終的狀態分布(圖c加粗)。然後通過控制數據，接著預測下一時刻的狀態分布(圖d加粗)。

獲取下一時刻的測量數據之後(圖e加粗)，綜合得到下一時刻估計的狀態分布(圖f加粗)。

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

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

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

眼睛看到室內環境就相當於測量數據，綜合眼睛看到的景象就會讓你對自己所在的位置判斷更準確啦。

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

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

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

在此膜拜一下大佬！

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

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

「SLAM是一道光，濾到你發慌！」

最後，祝大家濾波開心！

參考資料：

1. 《視覺SLAM十四講：從理論到實踐 第2版》高翔等人著

2. 《概率機器人》Sebastian Thrun等人著

3. 《策略不給力，來一發卡爾曼濾波》知乎Fitz Hoo文章

4. 《如何通俗並儘可能詳細地解釋卡爾曼濾波？》知乎Kent Zeng回答

5. 《圖說卡爾曼濾波，一份通俗易懂的教程》知乎論智文章