卡爾曼濾波：從入門到精通

05-26

卡爾曼濾波：從入門到精通

來自專欄 SLAM學習

最早接觸卡爾曼濾波是在衛星導航課中，GPS 和IMU 結合時常會用到卡爾曼濾波。但學完了也只明白了數學推導，不過是「會做題的機器」。最近在學習SLAM 時想要重新好好溫習一下卡爾曼濾波，雖然現在SLAM 的主流趨勢是利用圖優化，但卡爾曼濾波仍然為我們提供了一個很好的參考。

導論

卡爾曼濾波本質上是一個數據融合演算法，將具有同樣測量目的、來自不同感測器、(可能) 具有不同單位 (unit) 的數據融合在一起，得到一個更精確的目的測量值。

卡爾曼濾波的局限性在於其只能擬合線性高斯系統。但其最大的優點在於計算量小，能夠利用前一時刻的狀態（和可能的測量值）來得到當前時刻下的狀態的最優估計。

本文雖然是小白教程，但還是需要各位至少知道高斯分布，一點點線性代數，還有狀態向量這樣的名詞。

簡述

考慮一個SLAM 問題，它由一個運動方程：

$mathbf{x}_t=f(mathbf{x}_{t-1},mathbf{u}_t)+omega_t$

和一個觀測方程組成：

$mathbf{z}_{t,j}=h(mathbf{y}_j,mathbf{x}_t) + v_{t,j}$

就把它當作一個線性系統吧（非線性系統請看下一講擴展卡爾曼濾波），並且為了簡化推導，忽略路標的下標j，並把路標y 併入到狀態向量一起優化，那麼運動方程就可以寫為：

$mathbf{x}_t = mathbf{F}_t mathbf{x}_{t-1} + mathbf{B}_t mathbf{u}_t + omega_t$

其中，

? $mathbf{x}_t$ 為t 時刻的狀態向量，包括了相機位姿、路標坐標等信息，也可能有速度、朝向等信息；
? $mathbf{u}_t$ 為運動測量值，如加速度，轉向等等；
? $mathbf{F}_t$ 為狀態轉換方程，將t-1 時刻的狀態轉換至t 時刻的狀態；
? $mathbf{B}_t$ 是控制輸入矩陣，將運動測量值? $mathbf{u}_t$ 的作用映射到狀態向量上；
? $omega_t$ 是預測的高斯雜訊，其均值為0，協方差矩陣為? $mathbf{Q}_t$ 。

這一步在卡爾曼濾波中也稱為預測 (predict)。

類似地，測量方程可以寫為：

$mathbf{z}_t = mathbf{H}_t mathbf{x}_t + mathbf{v}_t$

其中，

? $mathbf{z}_t$ 為感測器的測量值；
? $mathbf{H}_t$ 為轉換矩陣，它將狀態向量映射到測量值所在的空間中；
? $mathbf{v}_t$ 為測量的高斯雜訊，其均值為0，協方差矩陣為? $mathbf{R}_t$ 。

而卡爾曼濾波就是預測 - 測量之間不斷循環迭代。當然，對於某些情況，如GPS + IMU，由於IMU 測量頻率遠比GPS 高，在只有IMU 測量值時，只執行運動更新，在有GPS 測量值時再進行測量更新。

一個小例子

用一個在解釋卡爾曼濾波時最常用的一維例子：小車追蹤。如下圖所示：

狀態向量為小車的位置和速度：

$mathbf{x}_t = egin{bmatrix} x_t \ dot x_t end{bmatrix}$

而司機要是踩了剎車或者油門，小車就會具有一個加速度， $mathbf{u}_t = frac{f_t}{m}$ ?。

假設t 和t-1 時刻之間的時間差為? $Delta t$ 。根據物理知識，有：

$egin{cases} x_t = x_{t-1} + dot x_{t-1} Delta t + frac{1}{2} frac{f_t}{m} Delta t^2 \ dot x_t = dot x_{t-1} + frac{f_t}{m} Delta t end{cases}$

寫成矩陣形式就有

$egin{bmatrix} x_t \ dot x_t end{bmatrix} = egin{bmatrix} 1 & Delta t \ 0 & 1 end{bmatrix} egin{bmatrix} x_{t-1} \ dot x_{t-1} end{bmatrix} + egin{bmatrix} frac{Delta t^2}{2} \ Delta t end{bmatrix} frac{f_t}{m}$

跟之前的運動方程對比，就知道

$mathbf{F}_t =egin{bmatrix} 1 & Delta t \ 0 & 1 end{bmatrix}, mathbf{B}_t = egin{bmatrix} frac{Delta t^2}{2} \ Delta t end{bmatrix}$

上式就寫為

$mathbf{hat x}_{t|t-1} = mathbf{F}_t mathbf{hat x}_{t-1} + mathbf{B}_t mathbf{u}_t$

? $mathbf{hat x}_{t-1}$ 表示t-1 時刻卡爾曼濾波的狀態估計； $mathbf{hat x}_{t|t-1}$ ? 則表示中t-1 到t 時刻，預測更新所得的預測值。

再利用運動模型對狀態向量進行更新後，還要繼續更新狀態向量的協方差矩陣P，公式為：

$mathbf{P}_{t|t-1} = mathbf{F}_t mathbf{P}_{t-1} mathbf{F}^T_t + mathbf{Q}_t$

假設 $mathbf{x}_t$ ? 為t 時刻下狀態向量的真值（自然是永遠未知的），由之前的現形運動方程（式(3)）給出，將式(3) 與式(9) 相減可得：

$mathbf{x}_t - mathbf{hat x}_{t|t-1} = mathbf{F} (mathbf{x}_{t-1} - mathbf{hat x}_{t|t-1}) + omega_t$

而

$egin{align} mathbf{P}_{t|t-1} & = E[(mathbf{x}_t - mathbf{hat x}_{t|t-1})(mathbf{x}_t - mathbf{hat x}_{t|t-1})^T]\ & = E[(mathbf{F} (mathbf{x}_{t-1} - mathbf{hat x}_{t|t-1}) + omega_t) cdot (mathbf{F} (mathbf{x}_{t-1} - mathbf{hat x}_{t|t-1}) + omega_t)^T] \ & = mathbf{F} E[(mathbf{x}_{t-1} - mathbf{hat x}_{t|t-1}) cdot (mathbf{x}_{t-1} - mathbf{hat x}_{t|t-1})^T]mathbf{F}^T \ & + mathbf{F} E[(mathbf{x}_{t-1} - mathbf{hat x}_{t|t-1}) omega_t^T] mathbf{F}^T \ & + mathbf{F} E[(omega_t mathbf{x}_{t-1} - mathbf{hat x}_{t|t-1})^T] mathbf{F}^T \ & + E[omega_t omega_t^T] end{align}$

考慮到狀態向量和雜訊是不相關的，則 $E[(omega_t mathbf{x}_{t-1} - mathbf{hat x}_{t|t-1})^T] = 0$ ?，上式就可以簡化為

$egin{align} mathbf{P}_{t|t-1} & = mathbf{F} E[(mathbf{x}_{t-1} - mathbf{hat x}_{t|t-1}) cdot (mathbf{x}_{t-1} - mathbf{hat x}_{t|t-1})^T]mathbf{F}^T + E[omega_t omega_t^T] \ & = mathbf{F}_t mathbf{P}_{t-1} mathbf{F}^T_t + mathbf{Q}_t end{align}$

推導完畢。

可以看到，經過預測更新，協方差矩陣P 變大了。這是因為狀態轉換並不完美，而且運動測量值含有雜訊，具有較大的不確定性。

預測更新實際上相當於「加法」：將當前狀態轉換到下一時刻（並增加一定不確定性），再把外界的干擾（運動測量值）疊加上去（又增加了一點不確定性）。

上面即為卡爾曼濾波中預測這一步。這一步相對比較直觀，推導也較測量更新簡單，就只在這裡詳細給出了。

如果得到了測量值，那麼我們就可以對狀態向量進行測量更新了，對應的公式為

$egin{align} mathbf{hat x}_t & = mathbf{hat x}_{t|t-1} + mathbf{K}_t (mathbf{z}_t - mathbf{H}_t mathbf{hat x}_{t|t-1}) \ mathbf{P}_t & = mathbf{P}_{t|t-1} - mathbf{K}_t mathbf{H}_t mathbf{P}_{t|t-1} end{align}$

其中，

$mathbf{K}_t = mathbf{P}_{t|t-1} mathbf{H}^T_t (mathbf{H}_t mathbf{P}_{t|t-1} mathbf{H}^T_t + mathbf{R}_t)^{-1}$

為卡爾曼增益。

從這裡就可以看到，測量更新顯然比預測更新複雜，難點也集中在這裡。下面就給出測量更性的詳細推導。

推導

一維case

從t-1 時刻起，小車運動後，經過前面所述的預測更新後，我們就得到了t 時刻的小車位置的估計，由於在卡爾曼濾波中，我們使用高斯概率分布來表示小車的位置，因此這個預測的位置可以寫為：

$y_1(r; mu_1, sigma_1) = frac{1}{sqrt{2 pi sigma_1^2}} e ^{-frac{(r - mu_1)^2}{2 sigma_1^2}}$

為了與前面的通用的推導區別開來，在這個一維的例子中我們使用了新的符號。不過熟悉高斯概率分布的話應該可以馬上看出來， $mu_1$ ? 為這個高斯分布的均值，? $sigma_1$ 為方差，而r 為小車的可能位置， $y_1$ ? 為某個可能位置 (r) 的概率分布。

假設在t 時刻，我們通過某測距儀測得小車距離原點的距離r，由於測量包含雜訊（且在面前我們假設了其為高斯雜訊），因此該測量值也可以利用高斯概率分布來表示：

$y_2(r; mu_2, sigma_2) = frac{1}{sqrt{2 pi sigma_2^2}} e ^{-frac{(r - u_2)^2}{2 sigma_2^2}}$

除了下標外，其餘的字母的含義都和上面的式子一樣。

如上圖瑣事，現在在t 時刻，我們有了兩個關於小車位置的估計。而我們所能得到的關於小測位置的最佳估計就是將預測更新和測量更新所得的數據融合起來，得到一個新的估計。而這個融合，就是一個簡單的「乘法」，並利用了一個性質：兩個高斯分布的乘積仍然是高斯分布。

$y_{fused}(r; mu_1, sigma_1, mu_2, sigma_2) = frac{1}{sqrt{2 pi sigma_1^2}} e ^{-frac{(r - u_1)^2}{2 sigma_1^2}} cdot frac{1}{sqrt{2 pi sigma_2^2}} e ^{-frac{(r - u_2)^2}{2 sigma_2^2}} = frac{1}{sqrt{2 pi sigma_1^2 sigma_2^2}} e ^{-(frac{(r - u_1)^2}{2 sigma_1^2} + frac{(r - u_2)^2}{2 sigma_2^2})}$

將上式化簡一下：

$y_{fused}(r;mu_{fused}, sigma_{fused}) = frac{1}{sqrt{2 pi sigma_{fused}^2}} e ^{-frac{(r - u_{fused})^2}{2 sigma_{fused}^2}}$

其中，? $mu_{fused}$ 為 $mu_1$ ? 和 $mu_2$ ? 的加權平均， $sigma_{fused}$ ? 則是 $sigma_1$ ? 和 $sigma_2$ ? 的調和平均的二分一：

$mu_{fused} = frac{mu_1 sigma_2^2 + mu_2 sigma_1^2}{sigma_1^2 + sigma_2^2} = mu_1 + frac{sigma_1(mu_2 - mu_1)}{sigma_1^2 + sigma_2^2}$

$sigma_{fused} = frac{1}{frac{1}{sigma_1} + frac{1}{sigma_2}} = frac{sigma_1 sigma_2}{sigma_1^2 + sigma_2^2} = sigma_1^2 - frac{sigma_1^4}{sigma_1^2 + sigma_2^2}$

最右邊的式子是為了後面的計算而準備的。

本質上，這（高斯分布相乘）就是卡爾曼濾波中測量更新的全部了。

那麼，怎麼由上面兩個簡單的一維的式子得到前一節? $mathbf{hat x}_t$ 和? $mathbf{P}_t$ 呢？一步一步來。

轉換矩陣H 的引入

在剛剛的一維情況的小例子中，我們其實做了一個隱式的假設，即有預測更新得到的位置的概率分布和測距儀所得的測量值具有相同的單位 (unit)，如米 (m)。

但實際情況往往不是這樣的，比如，測距儀給出的可能不是距離，而是信號的飛行時間（由儀器至小車的光的傳播時間），單位為秒 (s)。這樣的話，我們就無法直接如上面一般直接將兩個高斯分布相乘了。

此時，就該轉換矩陣 $mathbf{H}_t$ ? 閃亮登場了。由於? $r = c cdot t$ ，c 為光速。所以此時 $H_t = frac{1}{c}$ ? （測量方程為 $t = frac{r}{c}$ ?，可以回去參考一下式(4)）。

預測值就要寫為：

$y_1 (s; mu_1, sigma_1, c) = frac{1}{sqrt{2 pi (frac{sigma_1}{c})^2}} e ^{-frac{(s - frac{mu_1}{c})^2}{2 (frac{sigma_1}{c})^2}}$

而測量值保持不變：

$y_2(s; mu_2, sigma_2) = frac{1}{sqrt{2 pi sigma_2^2}} e ^{-frac{(s - u_2)^2}{2 sigma_2^2}}$

這樣，兩個高斯概率分布在轉換矩陣H 的作用下又在同一個空間下了。根據前面 $mu_{fused}$ ? 和 $sigma_{fused}$ ? 的公式（式(27) 和式(28)），可得：

$frac{mu_{fused}}{c} = frac{mu_1}{c} + frac{(frac{sigma_1}{c})^2(mu_2 - frac{mu_1}{c})}{(frac{sigma_1}{c})^2 + sigma_2^2}$

將上式兩端都乘以c 則可得：

$mu_{fused} = mu_1 + frac{frac{sigma_1^2}{c}}{(frac{sigma_1}{c})^2 + sigma_2^2} cdot (mu_2 - frac{mu_1}{c})$

由於 $H = frac{1}{c}$ ?（這裡轉換矩陣H 不隨時間變化而變化，所以把下標t 略去），並記? $K = frac{H sigma_1^2}{H^2 sigma_1^2 + sigma^2_2}$ ，則上式可以寫為：

$mu_{fused} = mu_1 + K (mu_2 - H mu_1)$

類似的有，

$frac{sigma^2_{fused}}{c^2} = (frac{sigma_1}{c})^2 - frac{(frac{sigma_1}{c})^4}{(frac{sigma_1}{c})^2 + sigma_2^2}$

兩邊乘以? $c^2$ 有：

$sigma^2_{fused} = sigma^2_1 -frac{frac{sigma_1^2}{c}}{(frac{sigma_1}{c})^2 + sigma_2^2} cdot frac{sigma_1^2}{c} = sigma^2_1 - K H sigma_1$

推廣至高維

到了這一步，這個一維情況下卡爾曼濾波的測量更新步驟就已經徹底講完了。剩下的就是將這個一維例子推廣至高維空間中。其實大家仔細觀察一下就會得到答案。

? $mu_{fused}$ 就是 $mathbf{hat x}_t$ ?，是測量更新後所得的狀態向量；
? $mu_1$ 就是? $mathbf{hat x}_{t|t-1}$ ，是t-1 時刻到t 時刻的小車的預測更新（或叫運動更新）後的狀態向量；
? $mu_2$ 是? $mathbf{z}_t$ ，是測量值；
? $sigma_{fused}^2$ 在高維空間中為 $mathbf{P}_t$ ?，為測量更新後，狀態向量的協方差矩陣；
? $sigma_1^2$ 在高維空間中就成了協方差矩陣 $mathbf{P}_{t|t-1}$ ?，是預測更新後狀態向量的協方差矩陣；
? $sigma_2^2$ 是 $mathbf{R}_t$ ?，是測量值的協方差矩陣；
H 就是? $mathbf{H}_t$ 了，高維空間中的轉換矩陣。
而最重要的，卡爾曼增益 $K = frac{H sigma_1^2}{H^2 sigma_1^2 + sigma^2_2}$ ?，在高維空間中就可以寫為 $mathbf{K}_t = mathbf{H}_t mathbf{P}_{t|t-1} cdot (mathbf{H}_t mathbf{P}_{t|t-1} mathbf{H}^T_t + mathbf{R}_t)^{-1}$ ?。看著很複雜，但仔細對照的話就是把前面相迎的數據替換帶入即可。

最後，根據式(33) $mu_{fused} = mu_1 + K (mu_2 - H mu_1)$ ? 就可得：

$mathbf{hat x}_t = mathbf{hat x}_{t|t-1} + mathbf{K}_t (mathbf{z}_t - mathbf{H}_t mathbf{hat x}_{t|t-1})$

根據? $sigma^2_{fused} = sigma^2_1 - K H sigma_1$ 就可得：

$mathbf{P}_t = mathbf{P}_{t|t-1} - mathbf{K}_t mathbf{H}_t mathbf{P}_{t|t-1}$

小結一下

通過這個一維情況的推導，希望能說明卡爾曼濾波就是在給定初始值的情況下，由預測和測量不斷迭代、更新狀態向量。而預測就是一個「加法」：狀態轉換和運動預測疊加；測量則是簡單的高斯分布相乘，中間引入了一個轉換矩陣將測量值和狀態向量映射在同一個代數空間中。

討論

至此，相信你已經明白了卡爾曼濾波的推導過程。而具體的問題就取決於你的建模了。如在上面的小車的例子中，各個參數如下：

$egin{align} mathbf{F}_t & =egin{bmatrix} 1 & Delta t \ 0 & 1 end{bmatrix},\ mathbf{B}_t & = egin{bmatrix} frac{Delta t^2}{2} \ Delta t end{bmatrix} \ mathbf{H}_t & = egin{bmatrix} frac{1}{c} & 0 end{bmatrix} end{align}$

假設該時刻下運動測量值為加速度 $a_t = frac{f_t}{m}$ ，為均值為0標準差為 $sigma_a$ 的高斯分布（標準差可以為經驗值或儀器精度。 $omega_t sim N(0, mathbf{Q}_t)$ ， $mathbf{Q}_t = mathbf{B}_t mathbf{B}^T_t sigma_a^2 = egin{bmatrix} frac{1}{4} Delta t^4 & frac{1}{2} Delta t^3 \ frac{1}{2} Delta t^3 & Delta t^2 end{bmatrix} cdot sigma_a^2$

對於測量值，同樣可以假設 $mathbf{v}_t sim N(0, sigma_z^2)$ ，那麼 $mathbf{R}_t = mathbf{H}_t mathbf{H}^T_t sigma_z^2 = [frac{1}{c^2} cdot sigma_z^2]$

多問個為什麼

如果只關心卡爾曼濾波的推導和結果，到這裡就可以停止啦。

但推完卡爾曼濾波，我還有幾個個為什麼。知其然更要知其所以然。下面是我對於自己的疑惑學習、思考得到的解答，而且礙於表達能力，不敢說百分百正確。

首先是對於預測更新。前面也說到了，預測更新相當於「加法」。這相對好理解一些。在t-1 時刻我們有了對於小車位置的一個估計，根據對小車速度（狀態向量之一）、小車的加速度（運動測量值）的建模，在輔以時間間隔，自然可以計算出小車在該時間間隔內的位移和速度增量，再將之疊加到原有的狀態向量上即可。由於建模和測量的過程帶有雜訊，所以此時小車的位置估計的精度是下降的（方差增大）。

那麼為什麼測量更新就是乘法而非加法呢？

因為測量更新的依據是貝葉斯法則。在有了測量值之後，我們求小車位置的概率分布其實就是在求? $P(mathbf{x} | mathbf{z})$ 。根據貝葉斯法則有：

$P(mathbf{x} | mathbf{z}) = frac{P(mathbf{z} | mathbf{x}) P(mathbf{x})}{P(mathbf{z})}$

? $P(mathbf{x} | mathbf{z})$ 是後驗概率。直接求後驗概率比較困難（為什麼？）。假設就在這個一維的小車例子中，當我們得到一個距離測量值z，那麼小車的位置可能是距離原點的-z 或z 的兩個點上。對於二維就可能是一個圓，三維則是一個球。此時要精確地知道小車的位置（消除歧義點），一則我們可以繼續測量，二則需要額外的信息。這就使得求後驗概率比較費時費力。

反觀貝葉斯法則的右側，此時我們已經有了先驗概率 $P(mathbf{x})$ ?，這是上一時刻的狀態向量的概率分布，並且我們也有了 $P(mathbf{z} | mathbf{x})$ ?，因為所得的測量值 $mathbf{z}_t = mathbf{H}_t mathbf{x}_t + mathbf{v}_t$ ? 表達的就是在當前位置下，我們能得到的測量值，亦即貝葉斯中的似然。在 $P(mathbf{z})$ ? 為一個常數的情況下，最大化 $P(mathbf{z} | mathbf{x}) P(mathbf{x})$ ? 就得到了最優的 $P(mathbf{x} | mathbf{z})$ ?。

既然測量更新是以貝葉斯公式為基礎，那麼反觀預測更新，除了前面那個直觀的「加法」解釋之外，是不是也有一個概率上的解釋呢？

連續的高斯分布所表示的小車位置的預測更新我沒找到（不好意思），但就離散情況的話還是有的，就是全概率公式。以下圖為例，假設t-1 時刻，小車的位置分布概率如圖所示，到了t 時刻，預測小車向前運動了3米（3個格子），但由於模型的不確定性和噪音，我們不能保證小車精確地向前走了3米，根據概率分布，我們可以假設小車有80%的概率往前走了3米，10%的概率往前走了兩米，而另有10%的概率往前走了4米。

那麼，在t 時刻，若小車真的運動到了這個位置，其概率分布是怎樣的呢？它既有可能是在距離該位置3米遠的地方以0.8的概率運動到現在這個位置的，也有可能是以0.1 的概率從2或4米遠的地方為初始位置運動到這的，根據全概率公式，可以表達為

$P(z) = 0.8 cdot 0.6 + 0.1 cdot 0.2 + 0.1 cdot 0.2 = 0.52$

類似地，t時刻下，小車運動後在其他位置上的概率分布也可以用全概率公式表達出來。當然，最後的計算結果還需要進行歸一化處理。

如果我們不斷地減小每個方格的解析度，並按照高斯分布給予每個方格一個概率值，並對小車運動也做如此的離散化處理，應該也是可以不斷逼近連續的情況（個人猜想）。

至此，關於卡爾曼濾波的個人淺見就到此為止了。精通還需要不斷地實踐，但希望讀完本文，能讓你對卡爾曼濾波有全面的了解。

有幫助的話，幫忙點個讚唄。

一些參考文獻：

Ramsey Faragher 的 understanding the basis of the kalman filter.

維基百科 Kalman filter

高翔的《視覺SLAM十四講》