Intro

最近在學習偉大的Kalman Filter，這篇筆記主要是對國外的兩篇介紹Kalman Filter的博客的翻譯和一些個人理解，博客鏈接：Kalman Filter For Dummies，?How a Kalman filter works。兩篇博客的側重的層次面不太一樣，第一篇講的層次比較淺顯但是缺失了一些細節，第二篇講的很深入但是有些抽象，這篇筆記將兩篇博客結合起來，希望可以由淺入深的理解Kalman Filter。

Kalman Filter的簡介和應用

首先先介紹一下Kalman Filter（以后簡稱kf）。kf是一種線性二次估計算法，由Rudolf Kalman于1960年提出，在隨后的阿波羅計劃中的導航系統就主要應用了kf算法。kf用對系統在時域的測量來預測一些未知的變量在下一個時間點的狀態，測量的對象包括噪音干擾和其他的不確定因素。kf相比于其它的預測算法有更高的精準度。

kf可以用于任何包含未知信息的動態系統，然后自適地預測系統下一步的狀態。哪怕環境的干擾很多很復雜，kf一般也可以得出很準確的結果。kf在連續變化的系統有很好的效果。它的優點是在預測時并不需要記錄很多系統過去的狀態，只需要知道上一狀態的預測結果的誤差協方差矩陣就可以了，所以kf可以算得很快，可以用于實時的動態系統。

一個kf的小例子

首先我們創造一個場景來引入kf算法。假設我們有一個機器人可以在叢林中穿梭，機器人的控制算法需要知道機器人的位置和速度然后才可以導航機器人的運動。我們用一個狀態Xk向量來表示：

這個例子為了簡化所以只考慮兩個狀態變量，在現實中可能會有更多的變量，更多的變量就意味著更多的信息，這更有利于kf的計算精準度。

同時，這個機器人還有一個GPS傳感器來測量距離和速度。這個GPS的精度在10米左右，但這是遠遠不夠的，因為如果在叢林里有一個懸崖的話10米的精度并不能保證機器人可以及時的避免。所以單單依靠GPS的數據來控制是不夠精準的。而且傳感器的也會被干擾，所以傳感器的數據并不是完全可依靠的。

機器人的運動可以分為主動因素和外界干擾兩個方面。主動因素是控制程序給電機發送的運動指令，它可以是向前向后，但這并不能保證機器人一定會沿著指定的方向運動，因為還有外界干擾的存在。比如說機器人遇到障礙了，被風吹了等等。所以外界干擾也是不可被忽視的一部分。

在這里就可以引入kf算法中的兩個概念，主觀預測和客觀測量。我們的主觀預測就是基于我們對機器人發出的命令，如果是向前5米，那我們對下一個時間點的的主觀預測就是向前5米。這個預測當然是不準的，因為我們沒有考慮到外界的干擾。客觀測量就是我們通過GPS得到的數據，首先這個數據是間接的，因為傳感器的得到的數據只是一些電平信號，我們還需要將它們對應轉換到現實世界的數據。其次這個數據也是不可靠的，理由見上。

我們對機器人下一個狀態的最優預測只能基于主觀預測和客觀測量這兩個信息，但是哪一個更可靠一點呢？我們加入權重來讓這個預測結果更公平一點，這里就引入了我們最優預測的權重公式：

其中Zk是我們得到的客觀測量值，X（k－1）就是我們上一次做出的主觀的預測，Xk就是我們最終的預測值，而K就是我們要通過kf找到的權重值，又稱Kalman Gain。這樣得到的估計相比于獨立的主觀估計和客觀估計都要精確的多。

但是注意，這個最優預測并不是用于實際的使用，實際中還是使用主觀預測為估計，但是當到下一個時間點得到客觀測量后，我們可以修正我們的估計，得到最優估計，這個估計是作為參數傳遞給下一次估計，這樣一次次修正，我們的估計會更加精確。

分布函數與協方差矩陣

在我們的問題情景里，我們只有兩個需要追蹤的狀態，位置和速度。我們可以假設位置和速度都服從于高斯分布，kf算法中，都假設要追蹤的變量服從于高斯分布。一維高斯分布的分布函數為：

中間的μ是這個高斯分布的期望，σ是標準差，σ^2是方差，在這個分布里只有一個變量，這個函數的積分為1。現在我們把高斯分布擴展到二維，則分布函數為：

在這個圖像中，X軸Y軸分別代表一個變量，Z軸是概率，所以中間最高點對應的X，Y為這兩個變量的期望，整個凸起部分的體積積分為1。用2維的圖來表示則為：

顏色越白的地方概率越大，所以期望在中心點，周圍黑色的部分概率為0。注意上圖中所標注的方差的大小不是所標注的范圍，此處有誤解。

在以上的分布的例子里，位置和速度的相關性不是很強，但是在現實中，位置一般是和速度有很大相關性的，比如以下的例子：

在這個例子中，速度與位置成正相關，當速度大的時候，往往位置也會大，速度小的時候，位置也會小。這樣的相關性信息也是kf算法所需要的。多維變量的相關性的信息是用協方差矩陣進行儲存的。協方差矩陣可以儲存一個一維列向量的變量之間相互的相關性信息。在這個例子中一維的變量向量和協方差矩陣為：

自己和自己的相關性是1，所以Σpp和Σvv都是1。相關性與順序無關，所以Σpv等于Σvp。所以協方差矩陣Pk是一個對稱矩陣。

主觀預測與狀態空間方程的構建

首先要構建的是狀態空間方程，所以變量為位置和速度。k是時間變量，我們的主觀預測需要從（k－1）時的狀態來預測k時的狀態：

藍色的區域是（k－1）時的速度與位置服從的分布，紫色的是k時速度與位置服從的分布。這個圖假設不同的時間點速度與位置服從的分布不同，然而在大部分時候，速度與位置的分布是不隨時間變化而變化的。

在這里，我們做的是主觀預測，我們的預測是給予一個經驗公式，這個公式可以來自于控制的命令，或者其它的一些我們已知的信息。我們的預測需要將（k－1）的狀態的分布轉移到k的狀態的分布，每一個（k－1）的點都可對應到K時的一個點。假設我們狀態轉移矩陣是Fk則：

注意，一般情況下，這個Fk就是1，因為分布的關系不會隨時間變化而變化。

我們可以把這個state space 方程寫下來：

Δt是k－1到k的時間變化，在這里假設機器人勻速運動。這樣我們就得到了我們的初步的主觀預測方程。我們可以從（k－1）的狀態預測下一個時間點時位置和速度的值。同時根據協方差矩陣的性質：

我們可以得到一個完整的預測信息：

但是這個狀態方程只是一個很基礎的，我們還需要考慮其他因素對系統的影響，比如說電機對機器人的控制，外界干擾等等。

我們首先考慮電機對機器人的控制，如果加上電機的加速度，則空間狀態方程為：

Bk就是控制矩陣，uk就是加速度向量。

此外還要考慮噪聲的影響，如果加上噪聲，則每一個（k－1）時的點不會對應到K時的一個點，而是對應到一片區域，所以對應關系如下：

因為每一個點都對應到一片區域，所有的對應疊加在一起，得到的新的K的分布范圍應該比以前要大，如下圖所示：

面積明顯比上圖要大，同時我們假設噪聲是高斯白噪聲，所以期望為0，所以加上噪聲后K的期望不變。假設噪聲所造成的那個小綠圈的速度與位置的協方差矩陣為Qk，則新的主觀預測和協方差矩陣為：

通過這個方程我們可以在X（k－1）時主觀的預測出Xk時的狀態。下一步就是得到客觀測量的結果。

客觀測量

我們的機器人上有GPS傳感器可以得到位置和速度的信息，假設sensor1測的是速度，sensor2測的是位置：

但是注意，因為傳感器得到的只是電平值，所以得到的分布函數的范圍與現實中的速度和位置的范圍不一樣。我們需要用一個轉移矩陣把現實世界的速度位置數據的map到傳感器的電平數據單位，每一個點對應傳感器坐標的一個點，然后在相同的單位下進一步計算權重Kalman gain。這里我們引入轉移矩陣Hk：

對應的期望與協方差被轉換為：

這里的μ expected就是我們的主觀估計在傳感器的單位下的值，Σ expected則是我們的主觀估計的協方差矩陣在傳感器單位的轉換后的矩陣。

當然，我們也不能忽略傳感器的噪聲影響，噪聲會造成我們在把現實世界的值轉換為傳感器單位時一個點對應傳感器坐標系的一片區域：

所以當傳感器坐標系中每一個點都被這樣的一個綠色的小橢圓代替，新的傳感器坐標系下的數據分布將變為：

形狀變成這樣是因為我們假設的傳感器噪音是這樣的一個橢圓形，原來的紫色的小圓形被擴張成了這樣的綠色橢圓。

則綠色橢圓分布的期望（中心點）就是我們傳感器讀到的客觀測量數據，但是包含誤差。

得出最優修正預測

得到我們的主觀預測在傳感器坐標系的分布和客觀測量得到的數據的分布后，就可以計算權重了。首先我們先將兩個分布重疊在一起，得到如下分布圖：

紫色的是主觀估計，綠色的是客觀測量。所以我們就有了兩個概率：

1，我們的主觀估計是我們最優估計的概率

2，客觀測量是我們最優估計的概率

在這里我們直接將兩個概率分布函數相乘，得到一個新的分布。因為黑的地方概率為0，所以相乘后紫色和綠色的非重疊面積都變為0了。我們只得到中心重疊的區域為我們最優的預測的分布：

這個分布的期望就是我們對下一個時間點K的最優預測。這個預測的特點是，在這個點上，主觀估計和客觀測量的概率最大。

一個一維更直觀的圖示為：

那個小藍色分布的期望就是紅色分布和綠色分布同時預測時的最佳預測值。

計算權重Kalman Gain

我們首先考慮只有一個變量時的情況。這時分布的波動情況用方差表示。

因為我們是直接將兩個分布函數相乘得到一個新的分布，所以用公式表達為：

如果兩個分布都是服從高斯分布：

則相乘以后得到的新的分布的期望和方差為：

我們可以在期望的計算公式中提出一個K，作為我們的權重Kalman Gain：

下面我們考慮多個變量的情況，這時數據的波動情況就不能用方差表示了，應該用協方差矩陣來表示，所以直接替換得到的新的分布和新的K：

這樣，我們就得到多變量時的Kalman Gain。這個分布期望就是我們最終的預測。協方差矩陣就是我們要傳給下一次預測的信息。

推導公式

我們的主觀預測的分布是：

客觀測量的分布是：

所以代入剛才得到的最佳預測的分布公式得到：

K‘就是我們的最佳預測的權重Kalman Gain，Xk’就是我們的最佳預測，Pk‘就是我們要傳給下一次預測的協方差矩陣。至此一次預測已經完成。

迭代流程圖

我們首先給以個起始估計，然后在（k－1）時先做主觀估計，用主觀估計值作為我們的估計，然后等到K時，用得到客觀測量進行矯正我們的估計，再把矯正后的新的值作為這次的最佳估計和新的協方差矩陣傳遞給下一次估計。

Time Update是在（k－1）階段，Measurement Update是在K階段。等到K時我們才可以得到客觀測量值，所以是用客觀測量進行矯正之后的最佳估計值再用來下一次估計。但這個最佳估計并沒有被用在實際中，之前實際用到的估計還是Time Update的估計值。

簡單的計算例子

參見A Simple Example

作者：凌晨2點的北京 鏈接：https://www.jianshu.com/p/2768642e3abf 來源：簡書 著作權歸作者所有。商業轉載請聯系作者獲得授權，非商業轉載請注明出處。

總結

以上是生活随笔為你收集整理的Kalman Filter 学习笔记的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。