卡爾曼濾波器推導與解析 - 案例與圖片

      在SLAM的後端優化中有存在這兩大類優化方法:濾波器優化和非線性優化.目前大多數基於視覺的SLAM算法都是採用的非線性優化的相關方法(如應用較多的G2O圖優化框架).但是濾波器的方法仍然在某些情況下有應用,且以前不少論文都是基於濾波器優化的方法設計的SLAM算法.因此學習濾波器優化的方法對於做SLAM算法相關的研究還是有很大的意義的.

      卡爾曼濾波(KF)和擴展卡爾曼濾波(EKF)是一類常用的濾波器優化方法.本文主要翻譯一片國外的博客,以較爲易懂的方式介紹卡爾曼濾波器的結構以及背後的數學原理.如果各位英文不錯還是比較推薦直接看英文原文,博客的原地址爲:http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/#mathybits

      除了這篇文章以外個人認爲以下這幾篇文章也寫的非常好,推薦閱讀:

      1. The Extended Kalman Filter: An Interactive Tutorial for Non-Experts

          這篇文章通俗易懂,還配有大量的交互式圖形小程序幫助大家理解各個參數對整個系統的影響,非常適合入門.同時該文章還配有matlab代碼和C++代碼.

      2. Tutorial: The Kalman Filter - MIT

          這是一篇MIT課程上的tutorial.比較深入的講解了KF濾波器背後的一些原理.

      3. KF, EKF and SLAM course in MATLAB 

         該課程授課教師爲MATLAB SLAM TOOLBOX的作者。在該課程上詳細講解了KF和EKF的相關理論以及EKF-SLAM的實現方法，對於大家深入瞭解EKF以及EKFSLAM相關理論（稀疏矩陣，反向深度，反向模型）都有很大幫助（附帶KF、EKF以及EKFSLAM Matlab代碼以及對應視頻）。

      4. Probabilistic Robotics - Chpter 3

         最後強烈推薦一下概率機器人,裏面的卡爾曼濾波這一章講的非常的好,如果各位有時間強烈建議讀一下.

 

以下爲翻譯內容(略有更改) - How a Kalman filter works, in picture

------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

1. 使用卡爾曼濾波器的目的

      我們假設你建造了一個可以在樹林裏行走的小機器人,爲了精準的進行導航,機器人需要每時每刻都知道它自己的位置

      我們用符號來表示機器人的狀態變量,在此處我們假設狀態變量只包含機器人的位置和速度:

      需要注意的是,狀態只是一列和你係統有關的變量,它可以是任何的變量,不僅限於位置和速度(例如可以將引擎的溫度,操作人員的手指在觸摸板上的位置等變量作爲系統的狀態變量,只要這些變量的狀態是可跟蹤的).

      在製造這個機器人的時候,我們在它上面也安裝了一些傳感器,其中就包括GPS.機器人上的GPSD定位精度爲10m,但是由於樹林中存在一些障礙物以及窪地,懸崖我們需求更高精度的定位能力,否則機器人就有可能撞到障礙物或者是跌落.

      同時,我們也知道一些與機器人運動相關的信息(變量):知道控制端口發送至機器人驅動輪上的命令,知道機器人的正方向朝向以及前方有無障礙物等.但是很明顯,我們無法獲取到有關機器人運動的所有信息(變量):機器人有可能收到強風影響,驅動輪可能打滑等.因此僅僅是通過計量驅動輪轉動圈數無法準確的計算出機器人走了有多遠.

      同理,GPS能夠非直接的告訴我們一些機器人的狀態,但是GPS所提供的信息也是帶有誤差和不確定行的.因此如果只是依據GPS做出的預測也是不夠精確的.

      雖然單一的依靠某一種信息無法給我們提供一個具有足夠置信度的結果,但是如果我們將所有有用的信息結合在一起,我們能否在不依賴機器人自身以外的傳感器得到一個比較的預測結果呢?答案當然是肯定的,這就是卡爾曼濾波器的設計目的.

 2. 卡爾曼濾波器

      同上,我們將繼續用位置和速度來代表機器人的狀態:

      由於不確定性的存在,機器人當前時刻的位置和速度值存在很多可能的可能性,我們無法知道準確的值.但是某中可能性的概率相較於其他會更大,如下圖所示:

 

      卡爾曼濾波器假設所有變量(在此例中爲位置和速度)都滿足隨機的高斯分佈.每一個變量都有均值u來代表隨機分佈的中心點,符號代表了不確定性.關於這兩個變量的解釋如下圖所示:

      在該示意圖中,位置和速度是不具有關聯性的,也就是說我們無法通過位置推斷得到和與速度有關的信息,反之亦然.

      當位置和速度變量具有關聯性時,其示意圖如下所示.如圖所示,觀察到某一個特定位置的可能性(概率)受當前機器人的速度影響.

      上圖所述這種情形可能在這種情況下發生.例如,當前我們依據上一時刻的位置去預測機器人下一時刻的位置.如果機器人的運行速度較高,其移動距離也可能變的更大,從而導致計算的位置也發生較大的距離變化.反之亦然.

      這種關係是十分重要的,因爲它會告訴我們更多的有關信息:一個變量的狀態將會反應出其他變量量的一些狀態.這也是卡爾曼濾波器的目的,即我們希望儘量的從具有不確定性的信息中獲取儘可能多的有用信息.

      在之後計算過程中我們用協方差矩陣(covariance matrix)來代表和描述這種關聯關係.即Σij描述了第i個狀態變量與第j個狀態變量之間的關聯性.協方差矩陣通常用符號Σ表示,其中的元素表示爲Σij.

 3. 用矩陣方式表述問題

      現在我們運用高斯來表述狀態量(位置與速度)的情況,因此對於當前時刻k機器人的狀態我們需要通過兩個變量進行定義:

            1. 估計值 - 各狀態量的均值,也可以用符號u進行表示;

            2. 協方差矩陣Pk;

      在此處我們僅僅只使用了機器人的位置和速度來表示狀態量,但是在實際操作和運算中,狀態矩陣的定義可以包括其他任何有用的變量.

      現在,我們需要了解一下當前狀態(t = k-1)和下一時刻的預測狀態(t = k)的情況.我們需要注意的是,雖然我們無法明確指出當前分佈中的哪一種情況是當前真實的狀態,但是我們仍然可以通過預測方程得到新時刻的分佈情況.因爲預測方程並非只作用於某一個具體的狀態量,而是作用與當前分佈中所包含的所有情況.

      在下圖中我們用矩陣Fk來表示預測步驟:

      現在思考一下,我們如何運用一個矩陣來預測下一時刻機器人的位置和速度呢?在此,我們將會運用如下所示的運動學方程進行預測:

      轉化爲矩陣形式有:

      很明顯,這是一個勻速運動模型,新時刻的位置P_new = P_previous + time*v .現在我們已經求得了預測矩陣(或稱爲狀態轉移矩陣),但是我們仍然不知道如何去更新協方差矩陣.

      爲了更新協方差矩陣,我們需要一個新的公式即:

      該公式表示,將當前分佈中所包含的所有情形(點)乘以預測矩陣就能夠得到更新或的協方差矩陣了.

      以上所屬內容,可以總結爲以下兩個公式:

4. 外部影響

      即使計算到了現在,我們也仍未囊括所有信息.因爲在系統中存在這一些不隨狀態量變化而發生變化的變量 - 外部世界的一些因素也能對系統產生影響.

      以火車運行爲例,火車的操作員可能會控制運行開關使得火車加速.類似的,在此機器人的案例中,機器人導航系統可能會發出指令使得機器人驅動輪轉向或停止.因此,在計算中我們需要考慮到這類變量對系統的影響.通常來說我們將這類變量稱作爲系統的控制變量,用符號進行表示.

      現在我們假設對於機器人這個案例,已知其外部控制量爲加速度a(由運動開關或控制命令進行控制),則之前所述的運行方程可更新爲:

      矩陣形式爲:

      其中Bk爲控制矩陣,而爲控制變量所組成的矩陣(n x 1).系統中的控制量只有在其已知或可獲取時才加入到方程中,並不是必不可少的.

      現在在讓我們思考另外一個問題,如果我們預測並不是一個100%精準的模型,那麼將會發生什麼?

5. 外部不確定性

      假如我們的變量是通過系統本身的屬性以及已知的外部影響而計算得到的,那麼狀態計算將不會有太大問題.但假如影響系統的量我們無法明確獲取到他們的值呢?

      仍然以我們的機器人爲例,在運動過程中其驅動輪可能打滑,機器人也有可能撞到地面上的隆起物(地面凹凸不平)使速度變慢.我們很難表示或追蹤這些因素所產生的影響.一旦這些情況發生,我們的預測結果就很有可能與實際結果產生很大的偏差,因爲我們並未在數學模型中考慮到這些因素.

      但是不用擔心,在數學上對於這種情況是有解決辦法的.我們在每一次的預測步驟中加入新的不確定性來表示這些世界中存在但是我們無法明確表示的變量所帶來的影響.

      原始估計中的每個狀態都會通過變換方程運動到一個新的狀態範圍中.

      如圖下所示,上一時刻狀態中的每一個狀態點(藍色區域表示)都會移動到一個新的區域範圍中(該範圍新加入了不確定性,用綠色圓表示).也就是說,我們將世界中無法表示且會對狀態變量帶來不確定性的影響視作噪聲(KF中通常爲白噪聲,協方差用表示).

      由於加入了額外的噪聲協方差,因此很明顯相較與之前推導中所產生的分佈區域,在在這種情況下的分佈區域會明顯不一樣.      

      現在,讓我們將新的噪聲協方差加入到之前所推導出來的公式中:

      這兩個公式代表了卡爾曼濾波器中的預測部分,是卡爾曼濾波器五個基礎公式中的前兩個.其中估計值爲根據上一時刻系統狀態和當前時刻系統控制量所得到的系統估計值,該估計值又叫做先驗估計值,爲各變量高斯分佈的均值.而爲協方差矩陣,代表了不確定性,它是由上一時刻的協方差矩陣和外部噪聲的協方差一起計算得到的.

      好了,現在我們已經得到了系統的預測(估計)值了.但是在真是的系統中,我們往往還能通過傳感器得到一些能反映系統狀態的測量值.

6. 測量值(觀測值)

      通常我們會在機器人上安裝一些傳感器,這些傳感器的返回值(測量量)能夠讓我們瞭解更多與機器人當前狀態有關的信息.例如,我們現在有兩個傳感器,一個返回位置信息另一個返回速度信息.這兩個傳感器都能夠間接的提供一些機器人運動狀態的信息(sensor operate on a state and produce a set of readings).

      需要注意的是,傳感器返回的信息其單位和尺度可能與我們在預測步驟中所跟蹤的狀態量的單位和尺度有所不同.因此通常我們使用矩陣Hk來表示狀態量與傳感器所觀測的測量量之間的關係.

      通常,我們能夠計算出傳感器返回值的分佈情況(通過傳感器出廠說明中的參數等信息):

      需要注意的是卡爾曼濾波器之所以好用的一個原因就是它可以很好的處理傳感器的噪聲.也就是說,傳感器的觀測值(或測量值)是具有一定程度的不可靠性的(由噪聲引起),因此傳感器的讀數是並非是一個精確值,而是一個帶有不確定區域的範圍.

      通過傳感器的觀測,我們可以猜測我們的機器人當前所處的狀態.但是因爲不確定性的存在,因此某些狀態比其他狀態有更大的可能行被視作傳感器的讀數.

      在卡爾曼濾波器中我們用符號來表示傳感器因爲噪聲所帶來的不確定性的協方差.而傳感器觀測值的均值就等於傳感器讀數值,通常用來進行表示.

      因此,現在我們有了兩個不同的高斯分佈:一個代表預測步驟,一個代表傳感器的觀測.

      我們現在需要根據預測步驟(粉色)和傳感器讀數(綠色)來調整傳感器讀數的分佈(此處是在解釋後文將會提到的卡爾曼增益).

      那麼現在哪一種纔是當前最有可能的狀態情況呢?對於某一組傳感器讀數(Z1,Z2),我們現在有兩種可能:

            1. 當前的傳感器測量爲錯誤的,不可以很好的代表機器人的狀態(當前機器人的狀態應該完全與預測步驟的分佈相同);

            2. 當前的傳感器測量爲正確的,可以很好的代表當前機器人的狀態(當前機器人的狀態應該完全與傳感器觀測分佈相同);

      如果現在我們假設這兩種可能行都是正確的,我們將其分佈相乘,就能得到一個新的如下所示的分佈:

        相乘後剩下的區域爲高量部分,在此區域中兩種假設情況都是高量.這就意味這這個新生成的分佈區域比之前任一一種假設都要精確(完全相信預測步驟和完全相信傳感器觀測),因此該區域爲對機器人當前狀態的最好估計(可以用下圖表示).

      以上的表述證明了,當你講兩個具有獨立的均值和協方差矩陣(seperate means and covariance matrics)的高斯分佈相乘時,就能夠得到一個具有新的均值和協方差矩陣的高斯分佈.也許你能夠發現:一定存在一個數學表達式能夠通過舊的參數值計算出來這個新的參數值.

7. 結合高斯

      現在我們一起來找到這個數學公公式.爲了易於理解,我們首先從一維情況開始推導.一維的高斯分佈可以表示爲:

      現在我們想要知道,當我們講兩個高斯曲線(分佈)相乘時會發生什麼.下圖中的藍色部分表示相乘後得到的新的高斯分佈的結果.

      現在我們將一階高斯分佈的公式帶入到上式中有:

      現在讓我們將上式改寫爲矩陣形式的表達式,並令Σ爲高斯分佈的協方差矩陣:

      K被成爲卡爾曼增益,我們將會在後面用到它.

8. 整合

      我們有兩個分佈:預測步驟的分佈爲,傳感器觀測值的分佈爲.我們將這兩個分佈帶入到公式:

      能夠求解得到重疊區域:

      因此卡爾曼增益爲:

      在以上的推導過程中我們可以發現每一個變量都乘上了變量,因此可以約掉,又發現在計算協方差矩陣時可以約掉.因此簡化後的公式爲:

      在卡爾曼濾波器中我們稱這三個公式爲更新步驟,這也是卡爾曼濾波器五個基本公式中的最後三個.

      此處的估計值纔是當前時刻機器人最後的最優估計值,而爲當前時刻機器人最有估計的協方差矩陣.這兩個變量將會當成是當前時刻最後的輸出值進入到下一次的卡爾曼濾波器計算過程中.

      卡爾曼濾波器工作流程示意圖大致如下:

9. 總結

      卡爾曼濾波器預算分爲預測和更新兩步,切具有五個基本方程:

            1. 預測步驟:

            2. 更新步驟     

      需要注意的是,卡爾曼濾波器只適用於線性系統,在真實世界中我們所面對的系統通常爲非線性系統,因此工程更常用的是擴展卡爾曼濾波.擴展卡爾曼濾波與卡爾曼濾波十分相似,唯一不同點在與擴展卡爾曼濾波需要對系統進行線性化(雅克比矩陣).