視覺slam-----第四篇 卡爾曼濾波

馬爾科夫和卡爾曼濾波

《視覺slam十四講》讀書筆記。

很多人都疑惑，馬爾科夫又是什麼意思？一個濾波問題怎麼才能用得上卡爾曼濾波呢？擴充卡爾曼濾波又和卡爾曼濾波有什麼關系？本篇部落格不講這些内容之間的推導關系，畢竟已經有無數人詳細講解了這個過程，這裡隻大概捋一捋它們之間的關系，已經我們該如何進行選擇。

馬爾科夫性質

一個馬爾科夫過程就是指過程中的每個狀态的轉移隻依賴于之前的 n個狀态，這個過程被稱為1個 n階的模型，其中 n是影響轉移狀态的數目。最簡單的馬爾科夫過程就是一階過程，每一個狀态的轉移隻依賴于其之前的那一個狀态。

在卡爾曼濾波中，我們就會使用馬爾科夫性質，而且是一階過程，我們不用關心它的推導過程。在卡爾曼濾波方法中，我們會從某時刻的狀态估計，推導到下一個時刻。另外一種方法是依然考慮 k 時刻狀态與之前所有狀态的關系，此時将得到非線性優化為主體的優化架構。

從視覺slam到卡爾曼濾波

SLAM 過程由運動方程和觀測方程來描述。那麼，假設在 t = 0 t = 0 t=0 到 t = N t = N t=N 的時間内，我們有 x 0 \boldsymbol { x } _ { 0 } x0​ 到 x N \boldsymbol { x } _ { N } xN​那麼多個位姿，并且有 y 1 , … , y M \boldsymbol { y } _ { 1 } , \dots , \boldsymbol { y } _ { M } y1​,…,yM​ 那麼多個路标。一般來說，視覺slam過程的運動和觀測方程為：

x k = f ( x k − 1 , u k ) + w k \boldsymbol { x } _ { k } = f \left( \boldsymbol { x } _ { k - 1 } , \boldsymbol { u } _ { k } \right) + \boldsymbol { w } _ { k } xk​=f(xk−1​,uk​)+wk​

z k , j = h ( y j , x k ) + v k , j \boldsymbol { z } _ { k , j } = h \left( \boldsymbol { y } _ { j } , \boldsymbol { x } _ { k } \right) + \boldsymbol { v } _ { k , j } zk,j​=h(yj​,xk​)+vk,j​

之前已經介紹了最大似然估計，把狀态估計轉換為最小二乘的做法。首先，由于位姿和路标點都是待估計的變量，我們改變一下記号，令 x k \boldsymbol { x } _ { k } xk​為 k k k時刻的所有未知量。它包含了目前時刻的相機位姿與 m m m 個路标點。在這種記号的意義下（雖然與之前稍有不同，但含義是清楚的），寫成：

x k ≜ { x k , y 1 , … , y m } \boldsymbol { x } _ { k } \triangleq \left\{ \boldsymbol { x } _ { k } , \boldsymbol { y } _ { 1 } , \dots , \boldsymbol { y } _ { m } \right\} xk​≜{xk​,y1​,…,ym​}

把 k k k時刻的所有觀測記作 z k z _ { k } zk​。于是，運動方程與觀測方程的形式可寫得更加簡潔。這裡不會出現 y \boldsymbol { y } y，但我們要明白這時 x \boldsymbol { x } x 中已經包含了之前的 y \boldsymbol { y } y 了:

{ x k = f ( x k − 1 , u k ) + w k z k = h ( x k ) + v k k = 1 , … , N \left\{ \begin{array} { l l } { \boldsymbol { x } _ { k } = f \left( \boldsymbol { x } _ { k - 1 } , \boldsymbol { u } _ { k } \right) + \boldsymbol { w } _ { k } } \\ { \boldsymbol { z } _ { k } = h \left( \boldsymbol { x } _ { k } \right) + \boldsymbol { v } _ { k } } \end{array} \right. k = 1 , \ldots , N {xk​=f(xk−1​,uk​)+wk​zk​=h(xk​)+vk​​k=1,…,N

現在考慮第 k \boldsymbol { k} k時刻的情況。我們希望用過去 0 到 k \boldsymbol { k} k中的資料，來估計現在的狀态分布:

P ( x k ∣ x 0 , u 1 : k , z 1 : k ) P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k } \right) P(xk​∣x0​,u1:k​,z1:k​)

下标 0 : k 0 : k 0:k 表示從 0 時刻到 k k k 時刻的所有資料。請注意 z k z _ { k } zk​ 來表達所有在 k k k時刻的觀測資料，注意它可能不止一個，隻是這種記法更加友善。

下面我們來看如何對狀态進行估計。按照 Bayes 法則，把 z k z _ { k } zk​ 與 x k x_ { k } xk​ 交換位置，有：

P ( x k ∣ x 0 , u 1 : k , z 1 : k ) ∝ P ( z k ∣ x k ) P ( x k ∣ x 0 , u 1 : k , z 1 : k − 1 ) P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k } \right) \propto P \left( \boldsymbol { z } _ { k } | \boldsymbol { x } _ { k } \right) P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k - 1 } \right) P(xk​∣x0​,u1:k​,z1:k​)∝P(zk​∣xk​)P(xk​∣x0​,u1:k​,z1:k−1​)

裡第一項稱為似然，第二項稱為先驗。似然由觀測方程給定，而先驗部分，我們要明白目前狀态 x k \boldsymbol { x } _ { k } xk​ 是基于過去所有的狀态估計得來的。至少，它會受 x k − 1 \boldsymbol { x } _ { k - 1 } xk−1​ 影響，于是按照 x k − 1 \boldsymbol { x } _ { k - 1 } xk−1​時刻為條件機率展開：

P ( x k ∣ x 0 , u 1 : k , z 1 : k − 1 ) = ∫ P ( x k ∣ x k − 1 , x 0 , u 1 : k , z 1 : k − 1 ) P ( x k − 1 ∣ x 0 , u 1 : k , z 1 : k − 1 ) d x k − 1 P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k - 1 } \right) = \int P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { k - 1 } , \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k - 1 } \right) P \left( \boldsymbol { x } _ { k - 1 } | \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k - 1 } \right) \mathrm { d } \boldsymbol { x } _ { k - 1 } P(xk​∣x0​,u1:k​,z1:k−1​)=∫P(xk​∣xk−1​,x0​,u1:k​,z1:k−1​)P(xk−1​∣x0​,u1:k​,z1:k−1​)dxk−1​

如果我們考慮更久之前的狀态，也可以繼續對此式進行展開，但現在我們隻關心 k k k 時刻和 k − 1 k - 1 k−1 時刻的情況。至此，我們給出了貝葉斯估計。接下來，我們根據一階馬爾科夫性，簡單的一階馬氏性認為， k k k 時刻狀态隻與 k − 1 k - 1 k−1 時刻狀态有關，而與再之前的無關。對于這種問題，我們可以用擴充卡爾曼濾波 EKF 來解決。

卡爾曼濾波

根據馬爾可夫性，目前時刻狀态隻和上一個時刻有關，上式中等式右側第一部分可進一步簡化：

P ( x k ∣ x k − 1 , x 0 , u 1 : k , z 1 : k − 1 ) = P ( x k ∣ x k − 1 , u k ) P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { k - 1 } , \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k - 1 } \right) = P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { k - 1 } , \boldsymbol { u } _ { k } \right) P(xk​∣xk−1​,x0​,u1:k​,z1:k−1​)=P(xk​∣xk−1​,uk​)

這裡，由于 k 時刻狀态與 k − 1 之前的無關，是以就簡化成隻與 xk−1 和 uk 有關的

形式，與 k 時刻的運動方程對應。第二部分可簡化為：

P ( x k ∣ x k − 1 , x 0 , u 1 : k , z 1 : k − 1 ) = P ( x k ∣ x k − 1 , u k ) P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { k - 1 } , \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k - 1 } \right) = P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { k - 1 } , \boldsymbol { u } _ { k } \right) P(xk​∣xk−1​,x0​,u1:k​,z1:k−1​)=P(xk​∣xk−1​,uk​)

到k 時刻的輸入量 u k \boldsymbol { u } _ { k } uk​ 與 k − 1 k-1 k−1 時刻的狀态無關，是以我們把 u k \boldsymbol { u } _ { k } uk​ 拿掉。可以看到，這一項實際是 k − 1 k-1 k−1 時刻的狀态分布。于是，這一系列方程說明了，我們實際在做的是“如何把 k − 1 k-1 k−1時刻的狀态分布推導至 k k k 時刻”這樣一件事。也就是說，在程式運作期間，我們隻要維護一個狀态量，對它進行不斷地疊代和更新即可。進一步，如果假設狀态量服從高斯分布，那我們隻需考慮維護狀态量的均值和協方差即可。

從形式最簡單的線性高斯系統開始，最後會得到卡爾曼濾波器。線性高斯系統是說，運動方程和觀測方程可以由線性方程來描述：

{ x k = A k x k − 1 + u k + w k z k = C k x k + v k k = 1 , … , N \left\{ \begin{array} { l l } { \boldsymbol { x } _ { k } = \boldsymbol { A } _ { k } \boldsymbol { x } _ { k - 1 } + \boldsymbol { u } _ { k } + \boldsymbol { w } _ { k } } \\ { \boldsymbol { z } _ { k } = \boldsymbol { C } _ { k } \boldsymbol { x } _ { k } + \boldsymbol { v } _ { k } } \end{array} \right. \quad k = 1 , \ldots , N {xk​=Ak​xk−1​+uk​+wk​zk​=Ck​xk​+vk​​k=1,…,N

并假設所有的狀态和噪聲均滿足高斯分布。記這裡的噪聲服從零均值高斯分布：

w k ∼ N ( 0 , R ) , v k ∼ N ( 0 , Q ) \boldsymbol { w } _ { k } \sim N ( \mathbf { 0 } , \boldsymbol { R } ) , \quad \boldsymbol { v } _ { k } \sim N ( \mathbf { 0 } , \boldsymbol { Q } ) wk​∼N(0,R),vk​∼N(0,Q)

為了簡潔我省略了 R R R和 Q Q Q 的下标。現在，利用馬爾可夫性，假設我們知道了 k − 1 k-1 k−1時刻的後驗（在 k − 1 k-1 k−1 時刻看來）狀态估計 x ^ k − 1 \hat { \boldsymbol { x } } _ { k - 1 } x^k−1​和它的協方差 P ^ k − 1 \hat { \boldsymbol { P } } _ { k - 1 } P^k−1​，現在要根據 k k k 時刻的輸入和觀測資料，确定 x k x _ { k } xk​ 的後驗分布。為區分推導中的先驗和後驗，我們在記号上作一點差別：以 x ^ k \hat { \boldsymbol { x } } _ { k } x^k​表示後驗，以 x ‾ \overline { x } x 表示先驗分布。

經典的卡爾曼濾波器主要分為兩個過程，預測和更新。

預測：

x ‾ k = A k x ^ k − 1 + u k , P ‾ k = A k P ^ k − 1 A k T + R \overline { \boldsymbol { x } } _ { k } = \boldsymbol { A } _ { k } \hat { \boldsymbol { x } } _ { k - 1 } + \boldsymbol { u } _ { k } , \quad \overline { \boldsymbol { P } } _ { k } = \boldsymbol { A } _ { k } \hat { \boldsymbol { P } } _ { k - 1 } \boldsymbol { A } _ { k } ^ { T } + \boldsymbol { R } xk​=Ak​x^k−1​+uk​,Pk​=Ak​P^k−1​AkT​+R

更新：先計算 K K K，它又稱為卡爾曼增益：

K = P ‾ k C k T ( C k P ‾ k C k T + Q ) − 1 \boldsymbol { K } = \overline { P } _ { k } C _ { k } ^ { T } \left( \boldsymbol { C _ { k } } \overline { \boldsymbol { P } } _ { k } \boldsymbol { C } _ { k } ^ { T } + \boldsymbol { Q } \right) ^ { - 1 } K=Pk​CkT​(Ck​Pk​CkT​+Q)−1

然後計算後驗機率的分布：

x ^ k = x ‾ k + K ( z k − C k x ‾ k ) P ^ k = ( I − K C k ) P ‾ k \begin{aligned} \hat { \boldsymbol { x } } _ { k } & = \overline { \boldsymbol { x } } _ { k } + \boldsymbol { K } \left( \boldsymbol { z } _ { k } - C _ { k } \overline { \boldsymbol { x } } _ { k } \right) \\ \hat { \boldsymbol { P } } _ { k } & = \left( \boldsymbol { I } - \boldsymbol { K } \boldsymbol { C } _ { k } \right) \overline { \boldsymbol { P } } _ { k } \end{aligned} x^k​P^k​​=xk​+K(zk​−Ck​xk​)=(I−KCk​)Pk​​

卡爾曼濾波器構成了線性系統的最優無偏估計。