視覺(jué)SLAM十四講 第9講 卡爾曼濾波

• 1. 什么是卡爾曼濾波
• 2. SLAM的狀態(tài)估計(jì)
• 3. 卡爾曼濾波器的推導(dǎo)
• 4. 擴(kuò)展卡爾曼濾波

1. 什么是卡爾曼濾波

百度百科定義：卡爾曼濾波（Kalman filtering）是一種利用線(xiàn)性系統(tǒng)狀態(tài)方程，通過(guò)系統(tǒng)輸入輸出觀測(cè)數(shù)據(jù)，對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法。由于觀測(cè)數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，所以最優(yōu)估計(jì)也可以看作是濾波過(guò)程。

有簡(jiǎn)單一點(diǎn)的話(huà)術(shù)來(lái)總結(jié)：

卡爾曼濾波是一種系統(tǒng)狀態(tài)最優(yōu)估計(jì)的方法。

2. SLAM的狀態(tài)估計(jì)

在后端優(yōu)化中，我們可以根據(jù)所考慮時(shí)刻（幀數(shù)）信息的多少來(lái)進(jìn)行分類(lèi)：

• k時(shí)刻的狀態(tài)和前面所有時(shí)刻的信息有關(guān)，甚至用未來(lái)的信息來(lái)更新，這樣的處理方式稱(chēng)為“批量的（Batch）”。這種處理方式常用非線(xiàn)性?xún)?yōu)化方法。
• k時(shí)刻的狀態(tài)只和k-1時(shí)刻的狀態(tài)有關(guān)，即馬爾科夫性，并且是一階馬爾科夫性。這種處理方式稱(chēng)為濾波器方法（卡爾曼濾波器）。

3. 卡爾曼濾波器的推導(dǎo)

根本思路：記住卡爾曼濾波器是對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的方法，且利用的是線(xiàn)性系統(tǒng)狀態(tài)方程。

第一步：狀態(tài)估計(jì)：

根據(jù)運(yùn)動(dòng)方程的狀態(tài)方程（式9.3），我們用貝葉斯法則，利用0到k時(shí)刻的數(shù)據(jù)來(lái)估計(jì)k時(shí)刻狀態(tài)分布：
$$P(x_k|x_0,u_{1:k},z_{1:k})\propto P(z_k|x_k)P(x_k|x_0,u_{1:k},z_{1:k?1})$$

• 正比于符號(hào)$\propto$的左邊即k時(shí)刻的狀態(tài)分布（后驗(yàn)分布，它根據(jù)了$z_k$觀測(cè)數(shù)據(jù)來(lái)求狀態(tài)，是知果求因）
• 符號(hào)右邊第一項(xiàng)是似然（知因求果）
• 符號(hào)右邊第二項(xiàng)是先驗(yàn)（歷史求因）
  

實(shí)際上，整個(gè)式子就是如下公式：$$后驗(yàn)\propto 似然\times 先驗(yàn)$$
我們卡爾曼濾波器，求的就是這個(gè)式子，我們希望估計(jì)k時(shí)刻的后驗(yàn)分布，那么只要求得k時(shí)刻的似然和先驗(yàn)即可，整個(gè)推導(dǎo)過(guò)程牢記這個(gè)式子，以免在推導(dǎo)的過(guò)程中迷失了自己。（甚至可以粗暴的認(rèn)為，卡爾曼濾波器就是一個(gè)矩陣A，假設(shè)k-1時(shí)刻的后驗(yàn)為x，通過(guò)Ax=b我們就求得的k時(shí)刻的后驗(yàn)分布b。）

然后，我們將先驗(yàn)以$x_{k?1}$時(shí)刻為條件概率展開(kāi)：

x時(shí)刻的先驗(yàn)：（下面為了簡(jiǎn)便用A、B表示）
$$P(x_k|x_0,u_{1:k},z_{1:k?1})=P(A|B)=\int P(A|B,x_{k?1})P(x_{k?1}|B)d{x}_{k?1}$$
然后我們?cè)偌僭O(shè)馬爾科夫性，簡(jiǎn)化這個(gè)式子，會(huì)得到一個(gè)抽象關(guān)系式：
$$x時(shí)刻的先驗(yàn)=k時(shí)刻的運(yùn)動(dòng)方程\times (k?1)時(shí)刻的狀態(tài)分布（后驗(yàn)）$$
這兩步想說(shuō)明的是，我們實(shí)際在做的是“如何把k-1時(shí)刻的狀態(tài)分布推導(dǎo)至k時(shí)刻”，即我們用k-1時(shí)刻的后驗(yàn)分布，計(jì)算k時(shí)刻的先驗(yàn)，最后求得k時(shí)刻的后驗(yàn)（狀態(tài)分布）。我們只要維護(hù)一個(gè)狀態(tài)量（后驗(yàn)），對(duì)它不斷迭代和更新即可。
后面的證明不會(huì)用到這兩步。只需要知道，我們假設(shè)k-1時(shí)刻的后驗(yàn)已知，后驗(yàn)分布均值為${\hat{x}}_{k?1}$，后驗(yàn)分布協(xié)方差為${\hat{P}}_{k?1}$

第二步：線(xiàn)性高斯系統(tǒng)

我們知道卡爾曼濾波器利用的是線(xiàn)性系統(tǒng)狀態(tài)方程，所以我們要假設(shè)運(yùn)動(dòng)方程和觀測(cè)方程可以用線(xiàn)性方程描述：
$\{\begin{array}{l}{x}_k=A_k{x}_{k?1}+u_k+w_k\\ z_k=C_k{x}_k+v_k\end{array}k=1,...,N$
這里的噪聲服從零矩陣高斯分布$w_k～N(0,R).v_k～N(0,Q)$

實(shí)際上，運(yùn)動(dòng)方程這里隱藏的信息是：k時(shí)刻的先驗(yàn)=k-1時(shí)刻的后驗(yàn)的線(xiàn)性映射

第三步：預(yù)測(cè)

預(yù)測(cè)，意思是如何從上一個(gè)時(shí)刻的狀態(tài)（即后驗(yàn)，假設(shè)已知），根據(jù)輸入信息（有噪聲，服從高斯分布）推斷當(dāng)前時(shí)刻的狀態(tài)分布（先驗(yàn)）。然后根據(jù)線(xiàn)性方程即運(yùn)動(dòng)方程和附錄A.3（高斯系統(tǒng)復(fù)合即加、乘之后的均值和協(xié)方差的變化情況），可以將k時(shí)刻的先驗(yàn)分布求出來(lái)：
$$P(x_k|x_0,u_{1:k},z_{1:k?1})=N(A_k{\hat{x}}_{k?1}+u_k,A_k{\hat{P}}_{k?1}{A}_k^{?}+R)$$
我們記預(yù)測(cè)的兩個(gè)式子為預(yù)測(cè)：（k時(shí)刻的先驗(yàn)=線(xiàn)性k-1時(shí)刻的后驗(yàn)）
$${\check{x}}_k=A_k{\hat{x}}_{k?1}+uk,{\check{P}}_k=A_k{\hat{P}}_{k?1}{A}_k^{?}+R$$

第四步：卡爾曼增益和后驗(yàn)分布的推導(dǎo)

我們知道了先驗(yàn)分布怎么求，接下來(lái)求似然。根據(jù)觀測(cè)方程和附錄A.3（高斯系統(tǒng)復(fù)合的性質(zhì)）求出似然的均值和協(xié)方差：
$$P(z_k|x_k)=N(C_k{x}_k,Q)$$
至于這里為什么協(xié)方差不是附錄4.3中的形式即$C_k{\hat{P}}_k{C}_k^{?}+Q$，這個(gè)問(wèn)題，博客zkk9527給出了解釋。而我的理解是，回顧整個(gè)推導(dǎo)過(guò)程，$x_k$（既不是后驗(yàn)也不是先驗(yàn)）的作用是作為一個(gè)參考量，我們利用與$x_k$相關(guān)的一次項(xiàng)系數(shù)和二次項(xiàng)系數(shù)來(lái)求得預(yù)測(cè)和卡爾曼增益。它不存在于我們最后的卡爾曼濾波器5個(gè)公式中，所以這里的$x_k$是一個(gè)定值，如果是定值則沒(méi)有協(xié)方差之說(shuō)，而均值就等于它本身。

現(xiàn)在有了先驗(yàn)和似然，我們將兩項(xiàng)乘起來(lái)便得到k時(shí)刻的后驗(yàn)：
$$N({\hat{x}}_k,{\hat{P}}_k)=\eta N(C_k{x}_k,Q)?N({\check{x}}_k,{\check{P}}_k)$$

我們已經(jīng)知道等式兩側(cè)都是高斯分布，所以只需要比較指數(shù)部分，不需要比較高斯分布前面的因子部分
（回顧書(shū)上P236式9.5中的貝葉斯公式，分母evidence即$P(A|B)=\frac{P(B|A)P(A)}{P(B)}$中的$P(B)$被省略掉了，所以等式用了正比于符號(hào)$\propto$表示。而后推導(dǎo)出兩邊高斯分布的均值和協(xié)方差后，又把正比于符號(hào)用等號(hào)代替，增加了一個(gè)比例系數(shù)$\eta$，那么嚴(yán)格來(lái)說(shuō)這里的$\eta$，就是前面的evidence的倒數(shù)，即使知道了這個(gè)，書(shū)上的推導(dǎo)還是假設(shè)了兩邊的因子相等，不知道為什么）
所以接下來(lái)我們把指數(shù)部分展開(kāi)：

至此，我們已經(jīng)推導(dǎo)了卡爾曼濾波器的整個(gè)過(guò)程。再來(lái)總結(jié)一下，卡爾曼濾波器推導(dǎo)后有5個(gè)公式（2個(gè)預(yù)測(cè)即先驗(yàn)的均值和協(xié)方差，1個(gè)卡爾曼增益，還有2個(gè)即后驗(yàn)分布的均值和協(xié)方差）：

• 預(yù)測(cè)：
  • ${\check{x}}_k=A_k{\hat{x}}_{k?1},{\check{P}}_k=A_k{\hat{P}}_{k?1}{A}_k^{?}+R$
• 更新
  • 卡爾曼增益：
    • $K={\check{P}}_k{C}_k^{?}(C_k{\check{P}}_k{C}_k^{?}+Q_k{)}^{?1}$
  • 后驗(yàn)：
    • ${\hat{x}}_k={\check{x}}_k+K(z_k?C_k{\check{x}}_k)$
    • ${\hat{P}}_k=(I?K{C}_k){\check{P}}_k$

如何更好的記憶呢？
k時(shí)刻的先驗(yàn)和k-1時(shí)刻的后驗(yàn)成線(xiàn)性映射；k時(shí)刻后驗(yàn)的均值等于先驗(yàn)均值加上一個(gè)卡爾曼增益和觀測(cè)誤差的乘積。

---

卡爾曼的濾波全部推導(dǎo)完了，現(xiàn)在我們總結(jié)一下簡(jiǎn)化的步驟：

狀態(tài)估計(jì)：由貝葉斯定理得到狀態(tài)估計(jì)的后驗(yàn)正比于似然×先驗(yàn)

線(xiàn)性系統(tǒng)：運(yùn)動(dòng)方程和觀測(cè)方程的線(xiàn)性方程列寫(xiě)

預(yù)測(cè)：進(jìn)行貝葉斯方程的先驗(yàn)推導(dǎo)，利用運(yùn)動(dòng)線(xiàn)性方程和高斯方程加乘性質(zhì)

卡爾曼增益推導(dǎo)：

利用觀測(cè)方程的高斯分布性質(zhì)求似然。

后驗(yàn)=似然×先驗(yàn)

比較等式兩邊指數(shù)部分，得到卡爾曼增益K的定義（最后的K用SMW進(jìn)行相等）

更新：利用后驗(yàn)的等式和卡爾曼增益，推出后驗(yàn)的均值和協(xié)方差式子

4. 擴(kuò)展卡爾曼濾波

我們知道，卡爾曼濾波利用的是線(xiàn)性系統(tǒng)方程，而SLAM中的運(yùn)動(dòng)方程和觀測(cè)方程通常是非線(xiàn)性的，特別是加入了相機(jī)內(nèi)參模型和李代數(shù)表示的位姿，更不可能是一個(gè)線(xiàn)性系統(tǒng)。一個(gè)高斯分布，經(jīng)過(guò)非線(xiàn)性變換后，往往不再是高斯分布。

所以我們需要將卡爾曼濾波器的結(jié)果拓展到非線(xiàn)性系統(tǒng)中。處理方法是：在某個(gè)點(diǎn)附近考慮運(yùn)動(dòng)方程及觀測(cè)方程的一階泰勒展開(kāi)，只保留一階項(xiàng)，即線(xiàn)性的部分。然后按照線(xiàn)性系統(tǒng)進(jìn)行推導(dǎo)。總的來(lái)說(shuō)，就是把非線(xiàn)性近似到線(xiàn)性，再利用相似的推導(dǎo)推導(dǎo)出擴(kuò)展卡爾曼濾波。

流程：

非線(xiàn)性到線(xiàn)性：將運(yùn)動(dòng)方程（xk-1）和觀測(cè)方程（xk）一階泰勒展開(kāi)

狀態(tài)估計(jì)：即后驗(yàn)的貝葉斯方程，后驗(yàn)=n似然×先驗(yàn)

線(xiàn)性系統(tǒng)：第一步已經(jīng)給出

預(yù)測(cè)：利用線(xiàn)性運(yùn)動(dòng)方程和xk-1時(shí)刻的后驗(yàn)和高斯復(fù)合法則，求出k時(shí)刻的先驗(yàn)

卡爾曼增益的推導(dǎo)：計(jì)算出似然，得到貝葉斯方程兩邊的結(jié)果。比較指數(shù)部分，定義卡爾曼增益

更新：得到后驗(yàn)關(guān)于先驗(yàn)的映射關(guān)系

總結(jié)

以上是生活随笔為你收集整理的视觉SLAM十四讲 第9讲 卡尔曼滤波的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。