在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)一起考慮。
如果采用濾波器方法，那一定會聽到一個(gè)如雷貫耳的名字——卡爾曼濾波(Kalman Filtering)。
我聽到過這個(gè)名字已經(jīng)很久了，可是一直沒有花時(shí)間弄懂究竟是什么東西，最近看了一些資料，就來總結(jié)一下，看看卡爾曼大佬究竟濾了誰？管中窺豹，還請多多指教！
在了解卡爾曼濾波之前，本科就學(xué)過一些濾波的方法，也就是從混合在一起的信號里提取出所需要的信號。
就好比吃辣子雞丁時(shí)只把雞丁挑出來啃光，而辣椒被你拋棄掉了！
例如，在模擬電路中，我們利用低通濾波的方法，可以將信號中摻雜的一些頻率較高的噪聲濾除掉，從而提取有效的較低頻信號。
同樣地，根據(jù)不同需要，還有高通濾波、帶通濾波和帶阻濾波。
再比如，在圖像處理中，如果存在椒鹽噪聲（不能吃的！是在圖像中隨機(jī)出現(xiàn)的黑白點(diǎn)），我們會選擇用中值濾波，將n*n個(gè)像素值的中值取出來以代替中心點(diǎn)的值，這樣就能濾除椒鹽噪聲。
但是，卡爾曼濾波和上述說的這些濾波略顯不同，它并沒有很直觀地從一些信號或者數(shù)據(jù)里面提取某些信號或數(shù)據(jù)。
它是在有干擾的條件下，通過數(shù)據(jù)的結(jié)合得到相對更準(zhǔn)確的估計(jì)數(shù)據(jù)。
卡爾曼濾波全程只關(guān)注兩個(gè)東西，一個(gè)是估計(jì)的最佳值，另一個(gè)是該值的不確定性（此處聯(lián)想一下高斯分布的兩個(gè)參數(shù)）。
打個(gè)比方，假設(shè)你蒙著眼睛在屋子里走，要從客廳走到臥室，你可以通過數(shù)步數(shù)來預(yù)測你當(dāng)前所在的位置。
但是，因?yàn)槟忝看芜~步的幅度和方向不是精準(zhǔn)的，所以你每多走一步所估計(jì)位置的不確定性就會越來越大，最后有可能走到浴室去了。
如果利用卡爾曼濾波，那么你對自己每一步的位置估計(jì)就會準(zhǔn)確很多，具體怎么做呢？賣個(gè)關(guān)子，后面再講，先解釋一下什么是狀態(tài)估計(jì)。
在SLAM中，運(yùn)用卡爾曼濾波是為了狀態(tài)估計(jì)，那什么才是需要估計(jì)的狀態(tài)呢？
狀態(tài)可以看作是機(jī)器人或者環(huán)境中可能會對未來產(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ù)和測量數(shù)據(jù)。
控制數(shù)據(jù)是機(jī)器人記錄自身運(yùn)動(dòng)的傳感器獲取的數(shù)據(jù)，比如IMU中的陀螺儀可以測量角速度、加速度計(jì)可以測量運(yùn)動(dòng)的加速度。
測量數(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è)測量數(shù)據(jù)為z_k，測量噪聲為delta_k，那么機(jī)器人的測量可以用一個(gè)觀測方程來表達(dá)，它表示在k時(shí)刻所在的位置觀測到路標(biāo)點(diǎn)產(chǎn)生測量數(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ù)和測量數(shù)據(jù)為條件的后驗(yàn)概率，即
而在獲得當(dāng)前時(shí)刻測量數(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á)觀測過程，則是
它表示第k時(shí)刻機(jī)器人的測量概率。
所以，如下圖所示，每一時(shí)刻的狀態(tài)x_k只與前一時(shí)刻的狀態(tài)x_k-1、當(dāng)前時(shí)刻的控制u_k有關(guān)，而每一時(shí)刻的測量z_k只與當(dāng)前時(shí)刻的狀態(tài)x_k有關(guān)。
這其實(shí)就是一個(gè)隱馬爾可夫模型，狀態(tài)不能直接得到，但是可以通過測量觀察到。
有了狀態(tài)分布的表達(dá)方式，還有運(yùn)動(dòng)方程和觀測方法的概率表示，接下來就可以名正言順地獻(xiàn)上著名的貝葉斯公式了(前方多式警告！)
因?yàn)榉帜负蜖顟B(tài)沒有半毛錢關(guān)系，因此可以用一個(gè)比例因子eta來表示，即
根據(jù)前面說到的置信度和觀測方程的概率表示(測量概率)，該式還可以表示為
對于狀態(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)，因此上式化簡得
可以看出，這是一個(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ù)測，得到綜合測量數(shù)據(jù)前的當(dāng)前時(shí)刻狀態(tài)分布。
然后，通過觀測方程將測量數(shù)據(jù)考慮進(jìn)來，再對狀態(tài)分布進(jìn)行調(diào)整更新，得到最當(dāng)前時(shí)刻最終的狀態(tài)估計(jì)。
因此，只要知道初始狀態(tài)分布、運(yùn)動(dòng)方程的狀態(tài)轉(zhuǎn)移概率和觀測方程的測量概率，貝葉斯濾波就可以濾起來了！
呼！有了前面一堆的鋪墊之后，終于迎來了重頭戲卡爾曼濾波。在這里先附上大牛的照片以表敬意，要知道當(dāng)年設(shè)計(jì)出的卡爾曼濾波器可是要上天的！
其實(shí)理解了貝葉斯濾波之后，卡爾曼濾波也不難明白。
因?yàn)榭柭鼮V波是一種特殊的貝葉斯濾波，它假定系統(tǒng)是線性高斯的，也就是說卡爾曼濾波=貝葉斯濾波+線性高斯系統(tǒng)。
這是什么意思呢？還記得前面提及的運(yùn)動(dòng)方程嗎，它在線性系統(tǒng)中的表達(dá)為
而觀測方程在線性系統(tǒng)中的表達(dá)為
與此同時(shí)，運(yùn)動(dòng)噪聲和測量噪聲都是隨機(jī)高斯噪聲，即
因此，運(yùn)動(dòng)方程的狀態(tài)轉(zhuǎn)移概率和觀測方程的測量概率都相應(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ù)測當(dāng)前時(shí)刻的均值和方差，然后再根據(jù)測量數(shù)據(jù)調(diào)整更新當(dāng)前時(shí)刻最終的均值和方差。
只不過卡爾曼濾波多了一個(gè)求卡爾曼增益K_k的過程。卡爾曼濾波和貝葉斯濾波的對比如下圖
由于篇幅原因，就不進(jìn)行公式推導(dǎo)了。如果覺得不夠直觀，那么就看一個(gè)栗子，用圖來解釋一下。
下面就用圖來解釋一下卡爾曼濾波，能有個(gè)更直觀的感受。
首先通過上一時(shí)刻的狀態(tài)預(yù)測得到當(dāng)前時(shí)刻的狀態(tài)分布(圖a)，然后通過傳感器得到測量數(shù)據(jù)(圖b加粗)。
結(jié)合測量數(shù)據(jù)調(diào)整更新，得到當(dāng)前時(shí)刻最終的狀態(tài)分布(圖c加粗)。然后通過控制數(shù)據(jù)，接著預(yù)測下一時(shí)刻的狀態(tài)分布(圖d加粗)。
獲取下一時(shí)刻的測量數(shù)據(jù)之后(圖e加粗)，綜合得到下一時(shí)刻估計(jì)的狀態(tài)分布(圖f加粗)。
到這，你知道卡爾曼濾波究竟濾了誰嗎？
在我看來，卡爾曼濾波可以看作是，通過測量數(shù)據(jù)將僅由控制數(shù)據(jù)進(jìn)行狀態(tài)估計(jì)而帶來不斷提高的噪聲(不確定性)濾除掉。同時(shí)，它更像是一種數(shù)據(jù)(傳感器)融合的方法。
還記得文章前面讓你蒙著眼在屋子里走嗎？學(xué)了卡爾曼濾波之后應(yīng)該知道怎么做能讓你更準(zhǔn)確地知道當(dāng)前位置了吧？很簡單，那就是睜開眼走路！
眼睛看到室內(nèi)環(huán)境就相當(dāng)于測量數(shù)據(jù)，綜合眼睛看到的景象就會讓你對自己所在的位置判斷更準(zhǔn)確啦。
當(dāng)然，如果你的鼻子夠靈，可以通過氣味判斷，或者有順風(fēng)耳可以聽到浴室滴水從而避免掉坑也是可以的！
我在知乎上也看到知友Kent Zeng對卡爾曼濾波有更入木三分的見解：
假設(shè)你有兩個(gè)傳感器，測的是同一個(gè)信號。可是它們每次的讀數(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)。怎么辦？
—— 把模型算出來的值，和傳感器測出的值，（就像兩個(gè)傳感器那樣），取加權(quán)平均。
OK，最后一點(diǎn)說明：你的模型其實(shí)只是一個(gè)步長的，也就是說，知道x(k)，我可以求x(k+1)。問題是x(k)是多少呢？答案：x(k)就是你上一步卡爾曼濾波得到的、所謂加權(quán)平均之后的那個(gè)、對x在k時(shí)刻的最佳估計(jì)值。
于是迭代也有了。
在此膜拜一下大佬！
想把卡爾曼濾波吃透不容易，但如果打算用濾波作為SLAM的后端部分，那還有大堆卡爾曼濾波的變體在撲向你~
看完卡爾曼濾波后，耳邊不禁響起一句:
最后，祝大家濾波開心！
