原文: https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

1 概述

你可以對某個動態系統有不確定信息的任何地方使用卡爾曼濾波器，并且對系統下一步的狀態做出有根據的猜測。即使出現混亂的現實狀態，卡爾曼濾波器都會給出一個合理的結果。

卡爾曼濾波器非常適合連續變化的系統。它具有占用內存少的優點，而且速度非常快，這使得它非常適合處理實時問題和應用在嵌入式系統中。

卡爾曼濾波器實際上非常簡單，如果你以正確的方式看待它，非常容易理解。我將嘗試用大量圖片來闡明它。先決條件很簡單：你只需要對概率和矩陣有基本的了解。

我將以卡爾曼濾波器可以解決的問題的簡單示例開始。

2 我們可以用卡爾曼濾波器做什么?

讓我們舉一個玩具示例：你建造了一個可以在樹林中漫步的小型機器人，并且機器人需要準確地知道它在哪里，以便可以導航。

我們會說機器人有一個狀態 $\overrightarrow{x_k}$，狀態包含了位移 (position) 和速度 (velocity):

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

需要注意，狀態只是關于系統底層配置的數字列表，它可以是任何變量。在我們的示例中，他是位移和速度，但它可以是油箱中的液體量、汽車發動機的溫度、用戶手指在觸摸板上的位置數據，或者是你需要跟蹤的任何數量的事物。

我們的機器人配備了 GPS 傳感器，其精度約為 10 米，這很不錯，但它需要比 10 米更精確地知道自己的位置。在樹林里有很多溝壑和懸崖，如果機器人的誤差超過幾英尺它就可能會掉下懸崖，因此僅靠 GPS 是不夠的。

我們可能還知道一些有關機器人如何移動的信息：它知道發送給車輪馬達的命令，并且知道如果它朝一個方向前進并且沒有任何東西干擾，那么在下一刻它很可能會沿著同一個方向走得更遠。但是它當然并不了解其運動得全部：它可能會受到風得沖擊，車輪可能會稍微打滑，或者在顛簸得地形上滾動，因此車輪轉動的量可能并不準確代表機器人實際行駛了多遠，并且預測不會是完美的。

GPS 傳感器會告訴我們一些狀態信息，但這些信息只是間接的，而且帶有一些不確定性和不準確性。我們的預測會告訴我們一些機器人如何移動的信息，但這些信息只是間接的，而且帶有一些不確定性和不準確性。

但是，如果我們利用所有可用的信息，我們能否得到比單獨估計更好的答案呢？答案當然是肯定的，這就是卡爾曼濾波器的用途。

3 卡爾曼濾波器分析問題的視角

讓我們看看我們試圖描述的場景。我們繼續使用只有位置和速度的簡單狀態。

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

我們不知道實際的位移和速度是多少；位移和速度的可能組合范圍可能很廣，但其中一些組合比其他組合更有可能：

卡爾曼濾波器假設兩個變量（在我們的例子中是位移和速度）都是隨機的，并且服從高斯分布 (Gaussian distributed)。每個變量都有一個平均值 $\mu$，即隨機分布的中心（也是最可能的狀態），以及方差 ${\sigma }^2$，即不確定性：

在上圖中，位移和速度是不相關的 (uncorrelated)，這意味這一個變量的狀態并不能告訴你另一個變量的狀態。

下面的例子顯示了更有趣的東西：位移和速度是相關的。觀察到特定位移的可能性取決于你的速度：

例如，如果我們根據舊位置估計新位置，則可能會出現這種情況。如果我們的速度很高，我們可能會移動得更遠，因此位移會更遠；如果我們移動得很慢，就不會走那么遠。

跟蹤這種關系確實很重要，因為它能給我們提供更多信息：一次測量可以告訴我們其他測量可能是什么。這就是卡爾曼濾波器得目標，我們希望從不確定得測量中盡可能多地獲得信息。

這種相關性可以用協方差矩陣 (covariance matrix) 來表示。簡而言之，矩陣的每個元素 ${\Sigma }_{ij}$ 是第 i 個變量和第 j 個變量之間的相關程度。可以猜到，協方差矩陣是對稱的，這意味著交換 i 和 j 無關緊要。協方差矩陣通常標記為 “$\Sigma$”，所以我們稱他們的元素為 “${\Sigma }_{ij}$”。

4 用矩陣描述問題

我們將關于狀態的知識建模為高斯斑點 (Gaussian blob)，因此我們同時需要兩條信息，我們將給出最佳估計值 ${\hat{\mathbf{x}}}_k$，(在其他地方成為 $\mu$ ) 及其協方差矩陣 ${\mathbf{P}}_k$ 。

$$\begin{array}{c}\begin{array}{r}{\hat{\mathbf{x}}}_k=\left[\begin{array}{c}\text{position}\\ \text{velocity}\end{array}\right]\\ {\mathbf{P}}_k=\left[\begin{array}{c}{\Sigma }_{pp}{\Sigma }_{pv}\\ {\Sigma }_{vp}{\Sigma }_{vv}\end{array}\right]\end{array}\end{array}\text{\hspace{1em}?}$$

當然，我們這里只使用位移和速度，但狀態可以包含任意數量的變量并表示你想要的任何內容。

接下來，我們需要某種方法來查看當前狀態(時間 k-1)，并預測在時間 k 的下一個狀態。其實我們不知道哪個狀態是真實的狀態，我們的預測函數也并不關心，它只對所有的狀態起作用，并為我們提供一個新的分布：

我們可以用矩陣 ${\mathbf{F}}_k$ 來表示這個預測步驟：

它會將我們原始估計中的每個點映射到新的預測位置，如果原始估計是正確的，系統會移動到該位置。

讓我們應用一下，如何使用矩陣來預測未來一刻的位移和速度，我們將使用一個非常基本的運動學公式：

$$\begin{array}{rlr}& 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}{c}1\Delta t\\ 01\end{array}\right]{\hat{\mathbf{x}}}_{k?1}\\ & \\ & ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k?1}\end{array}\text{\hspace{1em}?}$$

我們現在有一個預測矩陣，它將當前狀態映射到下一個狀態，但我們仍然不知道如何更新協方差矩陣。

這時我們需要另一個公式。如果我們將分布中的每個點乘以一個矩陣，那么協方差矩陣會發生什么？

這個很簡單，我們直接寫公式：

$$\begin{array}{rl}Cov(x)& =\Sigma \\ Cov(\mathbf{A}x)& =\mathbf{A}\Sigma {\mathbf{A}}^T\end{array}\text{\hspace{1em}?}$$

將方程 (4) 與 方程 (3) 結合：

$$\begin{array}{c}\begin{array}{r}{\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}\end{array}\text{\hspace{1em}?}$$

4.1 外部影響

不過，我們還沒有捕捉到任何東西。可能有一些與狀態本身無關的變化，外部世界可能正在影響系統。

例如，如果用狀態模擬火車的運動，火車操作員可能會按下油門導致火車加速，同樣，在我們的機器人示例中，導航軟件可能會發出車輪轉動或者停止的命令。如果我們知道關于外界正在發生事情的額外信息，我們可以在向量 $\overrightarrow{u_k}$ 里加入一個成員，用它做點什么，并把它添加到我們的預測中作為更正。

比方說，我們知道預期的加速度 $a$ 是由油門或者控制指令控制。從基本的運動學中，我們得到：

$$\begin{array}{rlr}{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]a\\ & ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k?1}+{\mathbf{B}}_k\overrightarrow{{\mathbf{u}}_k}\end{array}\text{\hspace{1em}?}$$

${\mathbf{B}}_k$ 被稱為控制矩陣, $\overrightarrow{u_k}$稱為控制向量。（對于沒有外部影響的非常簡單的系統，你可以省略這些）。

讓我們再添加一個細節。如果我們的預測不是 100% 準確的模型會發生什么。

4.2 外部不確定性

如果狀態根據自身屬性演變一切都還好。如果狀態是根據外力演變的，只要我們知道這些外力是什么，一切也都會還好。

但是我們不知道的力量呢？例如，如果我們在追蹤一架四軸飛行器，它可能會被風吹來吹去。如果我們跟蹤一個輪式機器人，輪子可能會打滑，或者地面上的顛簸可能會讓它變慢。我們無法跟蹤這些事情，如果發生任何這種情況，我們的預測可能會出錯，因為我們沒有考慮到這些額外的力量。

我們可以通過在每個預測步驟后添加一些新的不確定性來模擬與“世界”（即我們沒有跟蹤的事情）相關的不確定性：

在我們最初的估計中，每個狀態都可能轉移到一系列狀態。因為我們非常喜歡高斯斑點，所以我們會說每個點${\hat{x}}_{K?1}$被移動到具有協方差 ${\mathbf{Q}}_k$ 的高斯斑點中的某個點。另一種說法是，我們將未跟蹤的影響視為具有協方差的噪聲 ${\mathbf{Q}}_k$.

這產生了一個新的高斯斑點，具有不同的協方差（但平均值相同）：

我們通過簡單地添加來獲得擴展的協方差 ${\mathbf{Q}}_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}?}$$

換句話說，新的最佳估計是從以前的最佳估計中得出的預測，并加上對已知外部影響的修正。

新的不確定性是從舊的不確定性中預測的，還有一些來自環境的額外不確定性。

好吧，那就夠了。我們對這個系統可能在哪里有一個模糊的估計，給出了 ${\hat{\mathbf{x}}}_k$ 和 ${\mathbf{P}}_k$ 。那么當我們從傳感器獲取一些數據時又會發生什么？

5 通過測量完善估算

我們可能有幾個傳感器，可以給我們提供有關我們系統狀態的信息。就目前而言，他們測量什么并不重要；也許一個讀取位移，另一個讀取速度。每個傳感器都間接地告訴我們一些關于狀態的信息 —— 換句話說，傳感器在一個狀態上運行并產生一組讀數。

請注意，讀數的單位和尺度可能與我們正在跟蹤的狀態的單位和尺度不同。你也許能猜到接下來要做什么：我們將用矩陣 ${\mathbf{H}}_k$ 對傳感器進行建模。

我們可以找出我們期望以通常的方式看到的傳感器讀數的分布：

$$\begin{array}{rl}{\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}?}$$

卡爾曼濾波器有一個很大的優點是處理傳感器噪音。換句話說，我們的傳感器至少有些不可靠，我們原始估計中的每個狀態都可能導致一系列傳感器讀數。

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

我們將把這種不確定性的協方差（即傳感器噪聲）稱為 ${\mathbf{P}}_k$ 。分布的平均值等于我們觀察到的讀數，我們將稱之為 $\overrightarrow{{\mathbf{z}}_k}$ 。

所以現在我們有兩個高斯點：一個圍繞我們變換預測的平均值，一個圍繞我們得到的實際傳感器讀數。

我們必須嘗試將我們對基于預測狀態（粉紅色）的讀數的猜測與我們實際觀察到的傳感器讀數（綠色）的不同猜測相調和 (reconcile)。

那么，我們最有可能的新狀態是什么？對于任何可能的閱讀 $(z_1,z_2)$，我們有兩個相關的概率：

• (1) 我們的傳感器讀取的概率 $\overrightarrow{{\mathbf{z}}_k}$ 是一個（錯誤的）測量 $(z_1,z_2)$；
• (2) 我們之前的估計認為的概率 $(z_1,z_2)$ 是我們應該看到的讀數。

如果我們有兩個概率，并且我們想知道兩個概率都是真的，我們只需將它們相乘即可。因此，我們取兩個高斯斑點并乘以它們：

剩下的是重疊，即兩個斑點都明亮（可能）的區域。它比我們之前的任何一個估計都要精確得多。這個分布的平均值是兩種估計最有可能的配置，因此，鑒于我們掌握的所有信息，是對真實配置的最佳猜測。

嗯。這看起來像另一個高斯斑點。

事實證明，當你用單獨的均值和協方差矩陣乘以兩個高斯點時，你會得到一個新的高斯斑點，它有自己的平均值和協方差矩陣。也許你可以看到這會去哪里：必須有一個公式來從舊參數中獲取那些新參數。

6 組合高斯

讓我們找到那個公式。首先從一個維度看這個是最簡單的。一個協方差為 ${\sigma }^2$，均值為 $\mu$ 的高斯曲線定義 為：

$$\begin{array}{c}\mathcal{N}(x,\mu ,\sigma )=\frac{1}{\sigma \sqrt{2\pi }}{e}^{?\frac{(x?\mu {)}^2}{2{\sigma }^2}}\end{array}\text{\hspace{1em}?}$$

我們想知道當你把兩條高斯曲線相乘時會發生什么。下面的藍色曲線代表了兩個高斯曲線（未歸一化）的交集：

$$\begin{array}{c}\mathcal{N}(x,{\mu }_0,{\sigma }_0)?\mathcal{N}(x,{\mu }_1,{\sigma }_1)\stackrel{?}{=}\mathcal{N}(x,{\mu }^{\prime },{\sigma }^{\prime })\end{array}\text{\hspace{1em}?}$$

你可以將方程 (9) 代入方程 (10) 并做一些代數變換（小心重新歸一化，使總概率為1）以獲得：

$$\begin{array}{rl}{\mu }^{\prime }& ={\mu }_0+\frac{{\sigma }_0^2({\mu }_1?{\mu }_0)}{{\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}?}$$

我們可以提取出系數 $\mathbf{k}$：

$$\begin{array}{c}\mathbf{k}=\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}\end{array}\text{\hspace{1em}?}$$

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

記下如何進行之前的估算，并添加一些東西來進行新的估算。看看那個公式有多簡單！

但是矩陣版本呢？好吧，我們把 (12) 和 (13) 寫成矩陣形式。如果 $\Sigma$ 是高斯斑點的協方差矩陣，$\vec{\mu }$ 是它沿著每個軸線的平均數，然后：

$$\begin{array}{c}\mathbf{K}={\Sigma }_0({\Sigma }_0+{\Sigma }_1{)}^{?1}\end{array}\text{\hspace{1em}?}$$

$$\begin{array}{rl}\overrightarrow{{\mu }^{\prime }}& =\overrightarrow{{\mu }_0}+\mathbf{K}(\overrightarrow{{\mu }_1}?\overrightarrow{{\mu }_0})\\ {\Sigma }^{\prime }& ={\Sigma }_0?\mathbf{K}{\Sigma }_0\end{array}\text{\hspace{1em}?}$$

$\mathbf{K}$ 是一個叫做卡爾曼增益的矩陣，我們一會兒就會使用它。

放松，我們快完成了！

7 綜合

我們有兩種分布：

• The predicted measurement: $({\mu }_0,{\Sigma }_0)=({\mathbf{H}}_k{\hat{\mathbf{x}}}_k,{\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T)$ ;
• The observed measurement: $({\mu }_1,{\Sigma }_1)=(\overrightarrow{{\mathbf{z}}_k},{\mathbf{R}}_k)$ ;

我們可以把這些插入到方程中(15) 找到他們的重疊：

$$\begin{array}{rlr}{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{\prime }& ={\mathbf{H}}_k{\hat{\mathbf{x}}}_k& +\mathbf{K}(\overrightarrow{{\mathbf{z}}_k}+{\mathbf{H}}_k{\hat{\mathbf{x}}}_k)\\ {\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}?}$$

并且從 (14) 得到，卡爾曼增益是：

$$\begin{array}{c}\mathbf{K}={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k{)}^{?1}\end{array}\text{\hspace{1em}?}$$

我們可以把 (16) 和 (17) 中的 ${\mathbf{H}}_k$ 替換掉（注意夾在中間的 $\mathbf{K}$），以及 ${\mathbf{P}}_k$ 后面的 ${\mathbf{H}}_k^T$ .

$$\begin{array}{rl}{\hat{\mathbf{x}}}_k^{\prime }& ={\hat{\mathbf{x}}}_k+\mathbf{K}(\overrightarrow{{\mathbf{z}}_k}?{\mathbf{H}}_k{\hat{\mathbf{x}}}_k)\\ {\mathbf{P}}_k^{\prime }& ={\mathbf{P}}_k?\mathbf{K}{\mathbf{H}}_k{\mathbf{P}}_k\end{array}\text{\hspace{1em}?}$$

$$\begin{array}{c}{\mathbf{K}}^{\prime }={\mathbf{P}}_k{\mathbf{H}}_k^T({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k{)}^{?1}\end{array}\text{\hspace{1em}?}$$

上面給出了完整方程。

就是這樣！${\hat{\mathbf{x}}}_k$ 是我們新的最佳估計，我們可以通過 ${\mathbf{P}}_k^{\prime }$ 進行反饋，來到另一輪預測或更新，想多少次就多少次。

8 總結

在上述所有數學推導中，你只需要實現方程 (7)，(18) 和 (19).（或者如果你忘記了那些，你可以從方程中重新推導一切 (4) 和 (15) ）

這將允許您準確地對任何線性系統進行建模。對于非線性系統，我們使用擴展卡爾曼濾波器，它通過簡單地將有關其平均值的預測和測量線性化來工作。（我將來可能會做第二份關于 EKF 的文章）。

如果我的工作做得很好，希望其他人會意識到這些事情有多酷，并想出一個意想不到的新地方把它們付諸行動。