詳解卡爾曼濾波（Kalman Filter）原理

前言

看過很多關於卡爾曼濾波的資料，發現很多資料寫的都很晦澀，初學者很難看懂。在網上找了很多資料之後，發現了這篇博文講的非常清晰易懂，特此翻譯記錄，以備後用。為保證翻譯的原滋原味，以下均用第一人稱表述。

背景

我不得不說一說卡爾曼濾波，因為它能做到的事情簡直令人驚歎。

很可惜的是，很少有軟體工程師和科學家對此有深入瞭解。這讓我感到很沮喪，因為卡爾曼濾波是如此通用且強大的工具，它能在不確定情況下組合資訊。有時，它提取準確資訊的能力似乎幾乎是不可思議的。如果聽起來我講的太多了，那麼請看一下之前釋出的視訊，其中演示了一個利用卡爾曼濾波觀察自由浮動物體的速度來確定它的方向。能力真強大！

卡爾曼濾波是什麼？

你可以在任何含有不確定資訊的動態系統中的使用卡爾曼濾波，對系統的下一步動作做出有根據的猜測。即使伴隨著各種干擾，卡爾曼濾波總是能指出真實世界發生的情況。它可以利用怪異現象之間的關聯，你可能不會想到利用這些關聯！

卡爾曼濾波對於持續變化的系統是理想的選擇。由於卡爾曼濾波除了記憶前一個狀態而不需要保留其他的歷史記憶資訊，因此卡爾曼濾波具有輕量化的特點，執行速度非常快，非常適合處理實時的問題和嵌入式系統。

你在Google上找到的大部分關於卡爾曼濾波的數學描述是晦澀難懂的。那是非常糟糕的狀況！因為卡爾曼濾波能被簡單和容易的方式所理解的。因此，本文是一個非常不錯的文章主題，本文將嘗試用許多清晰、美觀的圖片來闡明它。 本文的前提很簡單，你僅僅需要對概率和矩陣有基本的瞭解。

我們能用卡爾曼濾波做什麼？

本文將從一個簡單的例子開始，說明卡爾曼濾波可以解決的問題。但如果你想直接接觸精美的圖片和數學，請隨時跳轉。

舉一個簡單的小例子：你已經做了一個能在叢林中游走的小機器人，為確保其能導航，機器人需要知道它所在的位置。

將機器人的運動狀態表示為$\overrightarrow{x_k}$，其僅僅包含了位置和速度：

$$\overrightarrow{x_k}=(\vec{p},\vec{v})$$

請注意，狀態只是關於系統底層配置的數字列表，它可以是任何東西。在我們的例子中，它是位置和速度，但它可以是油箱中液體量的資料、汽車引擎的溫度、使用者手指在觸控板上的位置或者任何你需要跟蹤的東西。

我們的機器人也有GPS感測器，精確大約10米，但它需要更精確地知道自己的位置。在樹林中有很多溝壑和懸崖，如果機器人的誤差超過幾英尺，它可能會從懸崖上掉下去。所以僅依賴GPS進行定位是遠遠不夠的。

我們可能還知道機器人是如何移動的：機器人知道傳送給車輪馬達的指令，如果它朝一個方向前進，沒有任何干擾，下一刻它可能會繼續朝同一方向前進。當然，它對自己的運動並不完全瞭解：它可能會受到風的衝擊，車輪可能會打滑，或者在崎嶇不平的地形上滾動；所以輪子轉動的數量可能不能準確地代表機器人實際行走了多遠，這個預測也不會完全準確。

GPS感測器告訴我們一些關於狀態的資訊，但只是間接的，帶有一些不確定性而且並不精準。我們的預測告訴了機器人是如何移動的，但只是間接的，並且也是不確定和不精準的。

但是，如果我們利用所有可用的資訊，我們能得到一個比這兩個估計本身更好的答案嗎?當然，答案是肯定的，這就是卡爾曼濾波器的作用。

如何從卡爾曼濾波的角度看待問題？

讓我們來看看我們想要詮釋的一個場景。我們繼續上一個例子，機器人僅僅包含一個位置和速度的簡單狀態。

$$\vec{x}=\left[\begin{array}{c}p\\ v\end{array}\right]$$

我們不知道實際的位置和速度是什麼；有一系列可能的位置和速度組合可能是正確的，但其中一些狀態比其他的狀態更可能：

卡爾曼濾波器假設兩個變數（例子中為位置和速度）都是隨機變數且服從高斯分佈的。隨機變數的均值為$\mu$，它表示隨機分佈的中心位置，即機器人最可能的狀態，不確定性用方差${\sigma }^2$表示：

在上圖中，位置和速度是不相關的，這意味著一個變數的狀態不能推測出其他變數的狀態。

更有趣的是下面的例子：位置和速度是呈相關性的。觀察特定位置的可能性取決於你的速度：

這種情況可能會出現。例如：根據一箇舊的位置估計一個新的位置，如果速度很快，可能會走得更遠，所以位置會更遠。如果走得很慢，那麼就不會走的很遠。

這種關係非常重要，因為它給我們提供了更多的資訊：一個測量值告訴我們其他測量值可能是什麼。我們要儘可能多地從不確定的目標中壓縮卡爾曼濾波器的資訊！

這種相關性被稱為協方差矩陣。簡而言之，矩陣的每個元素${\Sigma }_{ij}$是第$i$個狀態變數和$j$個狀態變數之間的相關程度。（由於協方差矩陣是對稱的，也就是交換$i$和$j$不會影響最終的結果)。協方差矩陣往往用“$\Sigma$”來表示，其中的元素則表示為“${\Sigma }_{ij}$”。

利用矩陣描述問題

我們基於高斯分佈來建立狀態變數，所以在時間$k$需要兩條資訊：將最佳估計稱為${\hat{\mathbf{x}}}_{\mathbf{k}}$（即均值$\mu$）和它的協方差矩陣${\mathrm{P}}_{\mathrm{k}}$。
$$\begin{array}{l}{\hat{\mathbf{x}}}_{\mathbf{k}}=\left[\begin{array}{c}\text{position}\\ \text{velocity}\end{array}\right]\\ {\mathrm{P}}_{\mathrm{k}}=\left[\begin{array}{cc}{\Sigma }_{pp}& {\Sigma }_{pv}\\ {\Sigma }_{vp}& {\Sigma }_{vv}\end{array}\right]\end{array}\text{\hspace{1em}(1)}$$
(在這裡本文只使用了位置和速度，但是該狀態可以包含任意數量的變數，並表示任何需要表示的東西，這對於處理其他問題是非常有益的)。

接下來，我們需要某種方式來知道目前狀態（時刻$k-1$）並預測下一個時刻$k$的狀態。記住，我們不知道哪個狀態是“真實的”狀態，但我們的預測函式並不關心。它適用於所有的情況，並給出了一個新的分佈：

用矩陣${\mathbf{F}}_{\mathbf{k}}$表示這個估計步驟：

它將我們原始估計中的每個點都移動到了一個新的預測位置，如果原始估計是正確的話，這個新的預測位置就是系統下一步會移動到的位置。

那我們又如何用矩陣來預測下一個時刻的位置和速度呢？下面用一個基本的運動學公式來表示：
$$\begin{array}{l}{p}_k=p_{k-1}+\Delta t{v}_{k-1}\\ v_k=v_{k-1}\end{array}$$

用另一種表達方式就是：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& =\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]{\hat{\mathbf{x}}}_{k-1}\\ & ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}\end{array}\text{\hspace{1em}(2, 3)}$$

現在，我們有了一個預測矩陣，該矩陣提供了下一個狀態，但是我們仍然不知道如何更新協方差矩陣。

這也就是為什麼我們需要另一個公式的原因。如果我們將分佈中的每個點乘以矩陣$\mathrm{A}$，那麼其協方差矩陣$\Sigma$會變成什麼？

對，很簡單。下面就來確認這個：
$$\begin{array}{rl}\mathrm{Cov}(x)& =\Sigma \\ \mathrm{Cov}(\mathbf{A}x)& =\mathbf{A}\Sigma {\mathbf{A}}^T\end{array}\text{\hspace{1em}(4)}$$

下一步聯合公式(4)和(3)：
$$\begin{array}{l}{\hat{\mathbf{x}}}_k={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}\\ {\mathbf{P}}_k={\mathbf{F}}_{\mathbf{k}}{\mathbf{P}}_{k-1}{\mathbf{F}}_k^T\end{array}\text{\hspace{1em}(5)}$$

外部控制變數

不過，我們並沒有捕捉到所有的資訊，可能會有一些與狀態本身無關的變化（外部世界的改變可能會影響到這個系統）。

例如，當模擬一列火車的狀態模型時，列車駕駛員可能會踩下油門，導致列車加速。類似地，在上述的機器人例子中，導航軟體可能會發出一個命令讓車輪轉動或停止。如果我們知道這個世界上正在發生什麼的其他資訊，我們可以把它填充到一個叫做$\overrightarrow{u_k}$的向量中，用它做一些事情，然後把它加入我們的預測中進行修正。

假設我們知道由於油門設定或控制命令而產生的預期加速度$a￥。從基本運動學我們得到：
$$\begin{array}{l}{p}_k=p_{k-1}+\Delta t{v}_{k-1}+\frac{1}{2}a\Delta t^2\\ v_k=v_{k-1}+a\Delta t\end{array}$$

其矩陣形式為：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]\\ & ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k\overrightarrow{{\mathbf{u}}_k}\end{array}\text{\hspace{1em}(6)}$$
其中${\mathrm{B}}_{\mathrm{k}}$稱為控制矩陣（control matrix），$\overrightarrow{u_k}$是控制向量（control vector）（對於沒有外部控制變數的簡單系統而言，可以忽略掉這些）。

外部干擾

讓我們再增加一個細節。如果我們的預測不是一個100%準確的模型，那會發生什麼？

如果狀態是基於自身的屬性發展的，那麼一切都是好的。如果狀態是以外力為基礎發展起來的，只要我們知道這些外在因素是什麼，一切還是好的。

但是對於我們不知道的外在因素呢?例如：如果我們在追蹤一個四軸飛行器，它可能會受到風的衝擊；如果我們追蹤一個帶輪子的機器人，輪子可能會打滑，或者地面上的顛簸會讓它減速。我們無法跟蹤這些東西，如果這些發生了，我們的預測可能會出錯，因為我們沒有考慮這些額外的因素。

我們可以通過在每個預測步驟之後新增一些新的不確定性，來建立與“世界”（包括我們沒有跟蹤的事物）相關的不確定性模型：

最初估計的每個狀態都有可能移動的範圍。高斯分佈非常適合這個場景，${\hat{x}}_{k-1}$中的每一點點都被移動到一個高斯分佈內的某個地方協方差為${\mathrm{Q}}_{\mathrm{k}}$。另一種說法是，我們用協方差${\mathrm{Q}}_{\mathrm{k}}$來處理未跟蹤的噪聲的影響。

這會產生一個新的高斯分佈，因此也有了一個不同的協方差，但是均值是相同的。

只需新增${\mathrm{Q}}_{\mathrm{k}}$即可得到擴充套件的協方差，以下為預測步驟完整的表示式：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k\overrightarrow{{\mathbf{u}}_k}\\ {\mathbf{P}}_k& ={\mathbf{F}}_{\mathbf{k}}{\mathbf{P}}_{k-1}{\mathbf{F}}_k^T+{\mathbf{Q}}_k\end{array}\text{\hspace{1em}(7)}$$

換句話說，新的最優估計的預測來自兩個方面，即先前的最優估計加上已知外部控制量的修正。

新的不確定性是根據舊的不確定性，加上一些來自環境的附加不確定性來預測的。

好，這很簡單。我們通過${\hat{\mathrm{x}}}_{\mathrm{k}}$和$\mathrm{P}$${}_k$對系統的位置有一個模糊的估計，當我們從感測器得到一些資料時會發生什麼?

利用外部觀測值修正估計量

我們可能有幾個感測器，這些感測器可以向我們提供有關係統狀態的資訊。 就目前而言，衡量什麼無關緊要； 也許一個讀取位置，另一個讀取速度。 每個感測器都告訴我們有關狀態的一些間接資訊，換句話說，感測器在狀態下執行併產生一組資料。

注意，感測器資料的單位和比例可能與我們所追蹤的狀態的單位和比例不一樣。你也許能猜到這是怎麼回事：我們將用矩陣對感測器進行建模，$\mathrm{H}$${}_k$。

我們可以用通常的方式找出我們希望看到的感測器讀數的分佈：
$$\begin{array}{l}{\vec{\mu }}_{\text{expected }}={\mathbf{H}}_k{\hat{\mathbf{x}}}_k\\ {\mathbf{\Sigma }}_{\text{expected }}={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T\end{array}\text{\hspace{1em}(8)}$$

卡爾曼濾波器最適合做的一件事是處理感測器噪聲。 換句話說，我們的感測器至少有些不可靠，並且我們原始估計中的每個狀態都可能導致感測器讀數在一定範圍內。

從我們觀察到的每一個讀數，我們可以猜測系統處於一個特定的狀態。但由於存在不確定性，一些狀態比其他狀態更有可能產生我們看到的讀數：

我們將這種不確定性（即感測器噪聲）的協方差稱為$\mathrm{R}$${}_k$。這個分佈的平均值等於我們觀察到的讀數，我們稱之為$\overrightarrow{z_k}$。

因此，現在我們有兩個高斯分佈：一個是經過轉換得預測平均值，另一個是得到的實際感測器讀數。

我們必須嘗試將基於預測狀態（粉紅色）的讀數猜測與基於實際觀察到的感測器讀數（綠色）的猜測保持一致。

那麼新的最可能的狀態是什麼？對於任何可能的讀數$(z1,z2)$，有兩個相關的概率：（1）感測器測量值$\overrightarrow{z_k}$是$(z1,z2)$測量誤差的概率；（2）前一個狀態得估計值$(z1,z2)$。

如果這兩個概率都知道，只需要把它們相乘就能夠得到最後得結果：

剩下的重疊部分，即兩個斑點都亮、可能相同的區域，比我們之前的任何一個估算值都精確得多。 這種分佈的平均值就是兩個估計最有可能的值，是給定所有資訊的最佳估計。

瞧！這個重疊的區域看起來像另一個高斯分佈。

　　
  如你所見，把兩個具有不同均值和方差的高斯分佈相乘，你會得到一個新的具有獨立均值和方差的高斯分佈！下面用公式講解。

聯合高斯分佈

  先以一維高斯分佈來分析比較簡單點，具有方差${\sigma }^2$和$\mu$的高斯曲線可以用下式表示：
$$\mathcal{N}(x,\mu ,\sigma )=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}\text{\hspace{1em}(9)}$$

  如果把兩個服從高斯分佈的函式相乘會得到什麼呢？

$$\mathcal{N}\left(x,{\mu }_0,{\sigma }_0\right)\cdot \mathcal{N}\left(x,{\mu }_1,{\sigma }_1\right)\stackrel{?}{=}\mathcal{N}\left(x,{\mu }^{\prime },{\sigma }^{\prime }\right)\text{\hspace{1em}(10)}$$

將式（9）代入到式（10）中（注意重新歸一化，使總概率為1）可以得到：
$$\begin{array}{rl}{\mu }^{\prime }& ={\mu }_0+\frac{{\sigma }_0^2\left({\mu }_1-{\mu }_0\right)}{{\sigma }_0^2+{\sigma }_1^2}\\ {\sigma }^{\prime 2}& ={\sigma }_0^2-\frac{{\sigma }_0^4}{{\sigma }_0^2+{\sigma }_1^2}\end{array}\text{\hspace{1em}(11)}$$

將式（11）中的兩個式子相同的部分用 $\mathbf{k}$表示：
$$\mathbf{k}=\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}\text{\hspace{1em}(12)}$$

$$\begin{array}{rl}{\mu }^{\prime }& ={\mu }_0+\mathbf{k}\left({\mu }_1-{\mu }_0\right)\\ {\sigma }^{\prime 2}& ={\sigma }_0^2-\mathbf{k}{\sigma }_0^2\end{array}\text{\hspace{1em}(13)}$$

下面進一步將式（12）和（13）寫成矩陣的形式，如果$\Sigma$表示高斯分佈的協方差，$\vec{\mu }$表示每個維度的均值，則：

$$\mathbf{K}={\Sigma }_0{\left({\Sigma }_0+{\Sigma }_1\right)}^{-1}\text{\hspace{1em}(14)}$$

$$\begin{array}{l}{\vec{\mu }}^{\prime }=\overrightarrow{{\mu }_0}+\mathbf{K}\left(\overrightarrow{{\mu }_1}-\overrightarrow{{\mu }_0}\right)\\ {\Sigma }^{\prime }={\Sigma }_0-\mathbf{K}{\Sigma }_0\end{array}\text{\hspace{1em}(15)}$$
  矩陣$\mathbf{K}$稱為卡爾曼增益，下面將會用到。放鬆！馬上就要結束了！

整合上述公式

我們有兩個高斯分佈，預測部分$\left({\mu }_0,{\Sigma }_0\right)=\left({\mathbf{H}}_k{\hat{\mathbf{x}}}_k,{\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T\right)$和測量部分$\left({\mu }_1,{\Sigma }_1\right)=\left(\overrightarrow{{\mathbf{z}}_k},{\mathbf{R}}_k\right)$，將他們放到式(15)中算出它們之間的重疊部分：

$$\begin{array}{rlr}{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{\prime }& ={\mathbf{H}}_k{\hat{\mathbf{x}}}_k& +\mathbf{K}\left(\overrightarrow{{\mathrm{z}}_k}-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k\right)\\ {\mathbf{H}}_k{\mathbf{P}}_k^{\prime }{\mathbf{H}}_k^T& ={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T& -\mathbf{K}{\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T\end{array}\text{\hspace{1em}(16)}$$

由式（14）可得卡爾曼增益為：
$$\mathbf{K}={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k\right)}^{-1}$$

將式（16）和式（17）的兩邊同時左乘矩陣的逆（注意$\mathbf{K}$裡面包含了${\mathbf{H}}_k$）將其約掉，再將式（16）的第二個等式兩邊同時右乘矩陣${\mathbf{H}}_k^T$的逆得到以下等式：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k^{\prime }& ={\hat{\mathbf{x}}}_k+{\mathbf{K}}^{\prime }\left(\overrightarrow{{\mathrm{z}}_k}-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k\right)\\ {\mathbf{P}}_k^{\prime }& ={\mathbf{P}}_{k^{-}}{\mathbf{K}}^{\prime }{\mathbf{H}}_k{\mathbf{P}}_k\end{array}\text{\hspace{1em}(18)}$$

$${\mathbf{K}}^{\prime }={\mathbf{P}}_k{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k\right)}^{-1}\text{\hspace{1em}(19)}$$

上式給出了完整的更新步驟方程。${\hat{\mathbf{x}}}_k$就是新的最優估計，我們可以將它和${\mathbf{P}}_k$放到下一個預測和更新方程中不斷迭代。

總結

對於上述所有的數學公式，你僅僅需要實現公式（7）、（18）和（19）。（如果你忘記了上述公式，你也能從公式（4）和（5）重新推導。）

總結

對於上述所有的數學公式，你僅僅需要實現公式（7）、（18）和（19）。（如果你忘記了上述公式，你也能從公式（4）和（5）重新推導。）

這將允許你精確地建模任何線性系統。對於非線性系統，需要用到擴充套件卡爾曼濾波，區別在於EKF多了一個把預測和測量部分進行線性化的過程。