什麼是卡爾曼濾波?

你可以在任何含有不確定信息的動態系統中使用卡爾曼濾波器來對系統的下一步做出有根據的預測。即使有噪聲信息的干擾，卡爾曼濾波器也能弄清楚真實發生的事情。

卡爾曼濾波器非常適合持續變化的系統。它運行速度快，佔用內存小(只需記錄前一個狀態)，對於實時性問題和嵌入式系統而言是理想選擇。

你從google上搜索到的關於卡爾曼濾波器的數學實現往往相當複雜，但事實上卡爾曼濾波器很簡單，只要你以一個正確的方式看它，理解起來就很容易。

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

讓我們舉一個有意思的例子：你製造了一個可以在樹林裏面行駛的小機器人，爲了實現導航，它需要知道當前所處的位置。

我們假定以 $\underset{X_{k}}{\rightarrow}$ 來描述機器人的狀態，包含了位置和速度這兩個信息。

$\underset{X_{k}}{\rightarrow} =(\underset{p}{\rightarrow},\underset{v}{\rightarrow})$

注意，所謂的狀態實際上就是關於系統的一系列參數，它可以是任何東西。在這個例子中狀態是位置和速度，換到其他例子它可以是引擎的溫度，觸摸板上指尖的位置等等。

小機器裝有GPS傳感器，精度爲10m。對於應付一般場景這已經很好了，但不要忘了我們是在小樹林裏面行駛，那裏有許多的溝渠和懸崖，小機器上哪怕偏差個幾米都有可能掉到山溝溝裏，所以單靠GPS還不夠。

實際上拋開GPS，我們也能預測機器人的移動，我們將指令發送給小機器上的馬達，如果它保持一個固定的方向運動而不受障礙物的干擾，那麼下一刻它會在訪方向上走得更遠。顯然，機器人對自己的狀態是全然不知的：它可能在逆風行駛，輪子有可能打滑，在崎嶇不平的地形上翻滾。因此，車輪轉動的次數並不能準確地代表實際行駛的距離，預測也變得不可能完美。

綜上所述，GPS爲我們提供了一些關於狀態的信息，但那是間接的、不準確的；我們的預測也提供了關於機器人軌跡的信息，但那同樣也是間接的、不準確的。

但這已經是我們能夠獲得的全部信息，基於這些信息，我們是否能給出一個比單獨基於某一項預測更好的結果?答案是肯定的，因爲有卡爾曼濾波器。

卡爾曼濾波器是怎麼看待這個問題的?

繼續上面的話題，我們有一個狀態，包含速度和位置。

$\underset{x}{\rightarrow}=\begin{bmatrix} p\\ v \end{bmatrix}$

我們不知道實際的位置和速度是多少，只是有一系列位置和速度的可能的值的組合，其中有一些組合比其它組合可能性更大。

譯者注：顏色越亮表示可能性越大，反之則可能性越小。小白點表示實際值，這也是我們迫切想知道，努力去追求，去接近的東東。

卡爾曼濾波器假定所有的變量(在我們這個例子中就是速度和位置)都是隨機的，而且符合高斯分佈(正太分佈)。每個變量(隨機變量)都有一個均值 $\mu$ ，它是隨機分佈的中心。以及一個方差 $\sigma ^{2}$ ，衡量不確定性的大小。

在上圖中，位置和速度是不相關的，意味着已知其中一個量的狀態並不能推測出另一個量可能的狀態。

下面這幅圖展示了一些有意思的東西，速度和位置變得相關，小機器人在特定位置的可能性依賴於速度的大小。

這其實也很好理解，基於一箇舊位置來估計新位置，如果速度很快，小機器人就可能移動得更遠(位置更遠)；如果速度很慢，機器人當然就走不了那麼遠。

這種聯繫對於目標跟蹤來說非常的重要，因爲它提供給我們更多信息：測量一個量告訴了我們關於其它量的可能的情況。這也是卡爾曼濾波器的目標，要從不確定的測量值中儘可能地挖掘出更多的信息。

使用協方差矩陣來捕捉這種相關性，簡而言之，協方差矩陣中的一個值 $\Sigma _{ij}$ 就是表示第i個狀態變量和第j個狀態變量之間的相關程度。協方差矩陣是對稱陣，交換i和j的位置值是一樣大小。

譯者注：協方差矩陣在統計學和機器學習中隨處可見，一般而言，可視作由方差和協方差兩部分組成，即方差構成了對角線上的元素，協方差構成了非對角線上的元素。

用矩陣來描述問題

現在使用高斯分佈來建模我們已知的關於狀態方面的知識，我們需要時刻兩方面的信息：最佳估計 $^{\hat{x}_{k}}$ (均值，一般使用來表示)以及協方差矩陣 $P_{k}$ 。

接下來，我們需要着眼於當前狀態(k-1時刻)來預測下一個狀態(k時刻)。記住，我們並不知道哪一個狀態是"實際"的狀態，但是預測函數並不在意這一點，它會作用於所有這些狀態，並給出一個新的狀態分佈。

我們用矩陣Fk來表示預測步驟：

它將原始估算中的每一個點移動到新的預測位置，如果原始估算是正確的，這就是系統將要移動到的位置。

我們是怎樣使用矩陣來預測下一個時刻的位置(position)和速度(velocity)的呢?這裏有一個簡單的公式：

換成矩陣形式：

現在我們就有了一個預測矩陣，它可以告訴我們下一刻系統的狀態，但目前爲止我們還不知道怎麼去更新協方差矩陣。現在將引出另一個公式。如果使用矩陣A來乘以分佈中的每一個點，那麼對於訪分佈的協方差矩陣會有什麼變化呢？我就直說了吧，會有下面這個等式：

把(4)和(3)結合起來可知：

譯者注：關於(4)式，這裏有個通俗易懂的證明：

外部影響

與狀態自身無關的外部因素同樣也會對系統有影響，例如建模列車的運動，列車操作員就可能手動對列車進行調速。同樣地，在我們小機器這個例子中，導航軟件也可能發佈一個指令讓機器人停下來。對於這些信息我們用一個向量 $\underset{u_{k}}{\rightarrow}$ 來表示，並將其納入預測系統作爲修正。