卡爾曼濾波詳解

阿新 • • 發佈：2018-11-20

　　在網上看了不少與卡爾曼濾波相關的部落格、論文，要麼是隻談理論、缺乏感性，或者有感性認識，缺乏理論推導。能兼顧二者的少之又少，直到我看到了國外的一篇博文，真的驚豔到我了，不得不佩服作者這種細緻入微的精神，翻譯過來跟大家分享一下，原文連結：http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
　　我不得不說說卡爾曼濾波，因為它能做到的事情簡直讓人驚歎！意外的是很少有軟體工程師和科學家對對它有所瞭解，這讓我感到沮喪，因為卡爾曼濾波是一個如此強大的工具，能夠在不確定性中融合資訊，與此同時，它提取精確資訊的能力看起來不可思議。

什麼是卡爾曼濾波？

　　你可以在任何含有不確定資訊的動態系統中使用卡爾曼濾波，對系統下一步的走向做出有根據的預測，即使伴隨著各種干擾，卡爾曼濾波總是能指出真實發生的情況。
　　在連續變化的系統中使用卡爾曼濾波是非常理想的，它具有佔用記憶體小的優點（除了前一個狀態量外，不需要保留其它歷史資料），並且速度很快，很適合應用於實時問題和嵌入式系統。
　　在Google上找到的大多數關於實現卡爾曼濾波的數學公式看起來有點晦澀難懂，這個狀況有點糟糕。實際上，如果以正確的方式看待它，卡爾曼濾波是非常簡單和容易理解的，下面我將用漂亮的圖片和色彩清晰的闡述它，你只需要懂一些基本的概率和矩陣的知識就可以了。

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

　　用玩具舉例：你開發了一個可以在樹林裡到處跑的小機器人，這個機器人需要知道它所在的確切位置才能導航。

　　我們可以說機器人有一個狀態 $這裡寫圖片描述$ ，表示位置和速度：

$這裡寫圖片描述$

　　注意這個狀態只是關於這個系統基本屬性的一堆數字，它可以是任何其它的東西。在這個例子中是位置和速度，它也可以是一個容器中液體的總量，汽車發動機的溫度，使用者手指在觸控板上的位置座標，或者任何你需要跟蹤的訊號。
　　這個機器人帶有GPS，精度大約為10米，還算不錯，但是，它需要將自己的位置精確到10米以內。樹林裡有很多溝壑和懸崖，如果機器人走錯了一步，就有可能掉下懸崖，所以只有GPS是不夠的。

　　或許我們知道一些機器人如何運動的資訊：例如，機器人知道傳送給電機的指令，知道自己是否在朝一個方向移動並且沒有人干預，在下一個狀態，機器人很可能朝著相同的方向移動。當然，機器人對自己的運動是一無所知的：它可能受到風吹的影響，輪子方向偏了一點，或者遇到不平的地面而翻倒。所以，輪子轉過的長度並不能精確表示機器人實際行走的距離，預測也不是很完美。
　　GPS 感測器告訴了我們一些狀態資訊，我們的預測告訴了我們機器人會怎樣運動，但都只是間接的，並且伴隨著一些不確定和不準確性。但是，如果使用所有對我們可用的資訊，我們能得到一個比任何依據自身估計更好的結果嗎？回答當然是YES，這就是卡爾曼濾波的用處。

卡爾曼濾波是如何看到你的問題的

　　下面我們繼續以只有位置和速度這兩個狀態的簡單例子做解釋。

$這裡寫圖片描述$

　　我們並不知道實際的位置和速度，它們之間有很多種可能正確的組合，但其中一些的可能性要大於其它部分：

　　卡爾曼濾波假設兩個變數（位置和速度，在這個例子中）都是隨機的，並且服從高斯分佈。每個變數都有一個均值 μ，表示隨機分佈的中心（最可能的狀態），以及方差 $這裡寫圖片描述$ ，表示不確定性。

　　在上圖中，位置和速度是不相關的，這意味著由其中一個變數的狀態無法推測出另一個變數可能的值。下面的例子更有趣：位置和速度是相關的，觀測特定位置的可能性取決於當前的速度：

　　這種情況是有可能發生的，例如，我們基於舊的位置來估計新位置。如果速度過高，我們可能已經移動很遠了。如果緩慢移動，則距離不會很遠。跟蹤這種關係是非常重要的，因為它帶給我們更多的資訊：其中一個測量值告訴了我們其它變數可能的值，這就是卡爾曼濾波的目的，儘可能地在包含不確定性的測量資料中提取更多資訊！
　　這種相關性用協方差矩陣來表示，簡而言之，矩陣中的每個元素 $這裡寫圖片描述$ 表示第 i 個和第 j 個狀態變數之間的相關度。（你可能已經猜到協方差矩陣是一個對稱矩陣，這意味著可以任意交換 i 和 j）。協方差矩陣通常用“ $這裡寫圖片描述$ ”來表示，其中的元素則表示為“ $這裡寫圖片描述$ ”。

使用矩陣來描述問題

　　我們基於高斯分佈來建立狀態變數，所以在時刻 k 需要兩個資訊：最佳估計 $這裡寫圖片描述$ （即均值，其它地方常用 μ 表示），以及協方差矩陣 $這裡寫圖片描述$ 。

$這裡寫圖片描述$ 　　　　　　　　(1)

　　（當然，在這裡我們只用到了位置和速度，實際上這個狀態可以包含多個變數，代表任何你想表示的資訊）。接下來，我們需要根據當前狀態（k-1 時刻）來預測下一狀態（k 時刻）。記住，我們並不知道對下一狀態的所有預測中哪個是“真實”的，但我們的預測函式並不在乎。它對所有的可能性進行預測，並給出新的高斯分佈。

　　我們可以用矩陣 $這裡寫圖片描述$ 來表示這個預測過程：

　　它將我們原始估計中的每個點都移動到了一個新的預測位置，如果原始估計是正確的話，這個新的預測位置就是系統下一步會移動到的位置。那我們又如何用矩陣來預測下一個時刻的位置和速度呢？下面用一個基本的運動學公式來表示：

　　現在，我們有了一個預測矩陣來表示下一時刻的狀態，但是，我們仍然不知道怎麼更新協方差矩陣。此時，我們需要引入另一個公式，如果我們將分佈中的每個點都乘以矩陣 A，那麼它的協方差矩陣 $這裡寫圖片描述$ 會怎樣變化呢？很簡單，下面給出公式：

　　結合方程（4）和（3）得到：

外部控制量

　　我們並沒有捕捉到一切資訊，可能存在外部因素會對系統進行控制，帶來一些與系統自身狀態沒有相關性的改變。
　　以火車的運動狀態模型為例，火車司機可能會操縱油門，讓火車加速。相同地，在我們機器人這個例子中，導航軟體可能會發出一個指令讓輪子轉向或者停止。如果知道這些額外的資訊，我們可以用一個向量這裡寫圖片描述來表示，將它加到我們的預測方程中做修正。
　　假設由於油門的設定或控制命令，我們知道了期望的加速度，根據基本的運動學方程可以得到：

　　以矩陣的形式表示就是：

　　 $這裡寫圖片描述$ 稱為控制矩陣，稱為控制向量（對於沒有外部控制的簡單系統來說，這部分可以忽略）。讓我們再思考一下，如果我們的預測並不是100%準確的，該怎麼辦呢？

外部干擾

　　如果這些狀態量是基於系統自身的屬性或者已知的外部控制作用來變化的，則不會出現什麼問題。
　　但是，如果存在未知的干擾呢？例如，假設我們跟蹤一個四旋翼飛行器，它可能會受到風的干擾，如果我們跟蹤一個輪式機器人，輪子可能會打滑，或者路面上的小坡會讓它減速。這樣的話我們就不能繼續對這些狀態進行跟蹤，如果沒有把這些外部干擾考慮在內，我們的預測就會出現偏差。
　　在每次預測之後，我們可以新增一些新的不確定性來建立這種與“外界”（即我們沒有跟蹤的干擾）之間的不確定性模型：

　　原始估計中的每個狀態變數更新到新的狀態後，仍然服從高斯分佈。我們可以說這裡寫圖片描述的每個狀態變數移動到了一個新的服從高斯分佈的區域，協方差為。換句話說就是，我們將這些沒有被跟蹤的干擾當作協方差為的噪聲來處理。

　　這產生了具有不同協方差（但是具有相同的均值）的新的高斯分佈。

　　我們通過簡單地新增這裡寫圖片描述得到擴充套件的協方差，下面給出預測步驟的完整表示式：

　　由上式可知，新的最優估計是根據上一最優估計預測得到的，並加上已知外部控制量的修正。
　　而新的不確定性由上一不確定性預測得到，並加上外部環境的干擾。
　　好了，我們對系統可能的動向有了一個模糊的估計，用這裡寫圖片描述和來表示。如果再結合感測器的資料會怎樣呢？

用測量值來修正估計值

　　我們可能會有多個感測器來測量系統當前的狀態，哪個感測器具體測量的是哪個狀態變數並不重要，也許一個是測量位置，一個是測量速度，每個感測器間接地告訴了我們一些狀態資訊。

　　注意，感測器讀取的資料的單位和尺度有可能與我們要跟蹤的狀態的單位和尺度不一樣，我們用矩陣 $這裡寫圖片描述$ 來表示感測器的資料。

　　我們可以計算出感測器讀數的分佈，用之前的表示方法如下式所示：

　　卡爾曼濾波的一大優點就是能處理感測器噪聲，換句話說，我們的感測器或多或少都有點不可靠，並且原始估計中的每個狀態可以和一定範圍內的感測器讀數對應起來。

　　從測量到的感測器資料中，我們大致能猜到系統當前處於什麼狀態。但是由於存在不確定性，某些狀態可能比我們得到的讀數更接近真實狀態。

　　我們將這種不確定性（例如：感測器噪聲）用協方差 這裡寫圖片描述表示，該分佈的均值就是我們讀取到的感測器資料，稱之為。
現在我們有了兩個高斯分佈，一個是在預測值附近，一個是在感測器讀數附近。

　　我們必須在預測值（粉紅色）和感測器測量值（綠色）之間找到最優解。
　　那麼，我們最有可能的狀態是什麼呢？對於任何可能的讀數這裡寫圖片描述，有兩種情況：（1）感測器的測量值；（2）由前一狀態得到的預測值。如果我們想知道這兩種情況都可能發生的概率，將這兩個高斯分佈相乘就可以了。