視覺SLAM十四講 第9講 卡爾曼濾波

• 1. 什么是卡爾曼濾波
• 2. SLAM的狀態估計
• 3. 卡爾曼濾波器的推導
• 4. 擴展卡爾曼濾波

1. 什么是卡爾曼濾波

百度百科定義：卡爾曼濾波（Kalman filtering）是一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法。由于觀測數據中包括系統中的噪聲和干擾的影響，所以最優估計也可以看作是濾波過程。

有簡單一點的話術來總結：

卡爾曼濾波是一種系統狀態最優估計的方法。

2. SLAM的狀態估計

在后端優化中，我們可以根據所考慮時刻（幀數）信息的多少來進行分類：

• k時刻的狀態和前面所有時刻的信息有關，甚至用未來的信息來更新，這樣的處理方式稱為“批量的（Batch）”。這種處理方式常用非線性優化方法。
• k時刻的狀態只和k-1時刻的狀態有關，即馬爾科夫性，并且是一階馬爾科夫性。這種處理方式稱為濾波器方法（卡爾曼濾波器）。

3. 卡爾曼濾波器的推導

根本思路：記住卡爾曼濾波器是對系統狀態進行最優估計的方法，且利用的是線性系統狀態方程。

第一步：狀態估計：

根據運動方程的狀態方程（式9.3），我們用貝葉斯法則，利用0到k時刻的數據來估計k時刻狀態分布：
$$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})$$

• 正比于符號$\propto$的左邊即k時刻的狀態分布（后驗分布，它根據了$z_k$觀測數據來求狀態，是知果求因）
• 符號右邊第一項是似然（知因求果）
• 符號右邊第二項是先驗（歷史求因）
  

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

然后，我們將先驗以$x_{k?1}$時刻為條件概率展開：

x時刻的先驗：（下面為了簡便用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}$$
然后我們再假設馬爾科夫性，簡化這個式子，會得到一個抽象關系式：
$$x時刻的先驗=k時刻的運動方程\times (k?1)時刻的狀態分布（后驗）$$
這兩步想說明的是，我們實際在做的是“如何把k-1時刻的狀態分布推導至k時刻”，即我們用k-1時刻的后驗分布，計算k時刻的先驗，最后求得k時刻的后驗（狀態分布）。我們只要維護一個狀態量（后驗），對它不斷迭代和更新即可。
后面的證明不會用到這兩步。只需要知道，我們假設k-1時刻的后驗已知，后驗分布均值為${\hat{x}}_{k?1}$，后驗分布協方差為${\hat{P}}_{k?1}$

第二步：線性高斯系統

我們知道卡爾曼濾波器利用的是線性系統狀態方程，所以我們要假設運動方程和觀測方程可以用線性方程描述：
$\{\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)$

實際上，運動方程這里隱藏的信息是：k時刻的先驗=k-1時刻的后驗的線性映射

第三步：預測

預測，意思是如何從上一個時刻的狀態（即后驗，假設已知），根據輸入信息（有噪聲，服從高斯分布）推斷當前時刻的狀態分布（先驗）。然后根據線性方程即運動方程和附錄A.3（高斯系統復合即加、乘之后的均值和協方差的變化情況），可以將k時刻的先驗分布求出來：
$$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)$$
我們記預測的兩個式子為預測：（k時刻的先驗=線性k-1時刻的后驗）
$${\check{x}}_k=A_k{\hat{x}}_{k?1}+uk,{\check{P}}_k=A_k{\hat{P}}_{k?1}{A}_k^{?}+R$$

第四步：卡爾曼增益和后驗分布的推導

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

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

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

至此，我們已經推導了卡爾曼濾波器的整個過程。再來總結一下，卡爾曼濾波器推導后有5個公式（2個預測即先驗的均值和協方差，1個卡爾曼增益，還有2個即后驗分布的均值和協方差）：

• 預測：
  • ${\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}$
  • 后驗：
    • ${\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時刻的先驗和k-1時刻的后驗成線性映射；k時刻后驗的均值等于先驗均值加上一個卡爾曼增益和觀測誤差的乘積。

---

卡爾曼的濾波全部推導完了，現在我們總結一下簡化的步驟：

狀態估計：由貝葉斯定理得到狀態估計的后驗正比于似然×先驗

線性系統：運動方程和觀測方程的線性方程列寫

預測：進行貝葉斯方程的先驗推導，利用運動線性方程和高斯方程加乘性質

卡爾曼增益推導：

利用觀測方程的高斯分布性質求似然。

后驗=似然×先驗

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

更新：利用后驗的等式和卡爾曼增益，推出后驗的均值和協方差式子

4. 擴展卡爾曼濾波

我們知道，卡爾曼濾波利用的是線性系統方程，而SLAM中的運動方程和觀測方程通常是非線性的，特別是加入了相機內參模型和李代數表示的位姿，更不可能是一個線性系統。一個高斯分布，經過非線性變換后，往往不再是高斯分布。

所以我們需要將卡爾曼濾波器的結果拓展到非線性系統中。處理方法是：在某個點附近考慮運動方程及觀測方程的一階泰勒展開，只保留一階項，即線性的部分。然后按照線性系統進行推導。總的來說，就是把非線性近似到線性，再利用相似的推導推導出擴展卡爾曼濾波。

流程：

非線性到線性：將運動方程（xk-1）和觀測方程（xk）一階泰勒展開

狀態估計：即后驗的貝葉斯方程，后驗=n似然×先驗

線性系統：第一步已經給出

預測：利用線性運動方程和xk-1時刻的后驗和高斯復合法則，求出k時刻的先驗

卡爾曼增益的推導：計算出似然，得到貝葉斯方程兩邊的結果。比較指數部分，定義卡爾曼增益

更新：得到后驗關于先驗的映射關系

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。