Kalman Filter 通俗講解

Kalman Filter 通俗講解

引言

Kalman Filter,很多人剛聽到這個名詞時,總是會下意識認為這就是個濾波器。我這裡想要重點宣告的是,Kalman Filter不是濾波,它是一種資訊融合的過程。
那麼Kalman Filter到底是什麼?它在那些方面有著應用,它的基本原理又是什麼。如果你參考國內的論壇或者教材上,你一定會發現,大部分都是一些複雜公式與枯燥抽象的說明。這裡我們將通過儘量通俗易懂的講解來給大家解釋下什麼叫Kalman Fiter。

先睹為快

正如我前面提到的,從定義及複雜公式去理解卡爾曼濾波,對咱們凡人來說基本是不可能的。我們將公式簡化,先從下面的公式來開始講解:

這裡寫圖片描述
其中下標k” role=”presentation”>kkk是指狀態,這裡我們可以把它看成間隔,比如k=1″ role=”presentation”>k=1k=1k=1指1ms,k=2″ role=”presentation”>k=2k=2k=2指2ms。我們的目的就是要找到在狀態k” role=”presentation”>kkk時X” role=”presentation”>XXX的估計。在公式中Zk” role=”presentation”>ZkZkZ_k就是真實測量值。我們要牢記,這些測量值並不可靠。(如果這些測量值是可靠的,那麼我們就沒必要學kalman filter了:))。Kk” role=”presentation”>KkKkK_k指卡爾曼增益(這是整個公式中的核心點)。Xk−1^” role=”presentation”>Xk−1^Xk−1^\hat{X_{k-1}}就是上一狀態時的估計。
在上述公式中,我們唯一未知的量就是卡爾曼增益Kk” role=”presentation”>KkKkK_k。像測量值及上一狀態估計,我們都已經知道了。當然了,求出卡爾曼增益並不簡單,但是我們有方法來解決它。
我們從另一角度來思考,讓我們假設卡爾曼增益為0.5,我們會得到什麼呢?它就是個簡單的平均!換句話說,我們應該找到對應於每個狀態的更加智慧化的卡爾曼增益係數。歸根結底就是:

卡爾曼濾波器為每個結果狀態找到最優的平均因子。 不知怎的,還記得一些關於過去的狀態。

如何,Kalman Filter神奇吧!

分步指南

下面是分佈指南,可以快速開始卡爾曼濾波。

Step1 構建模型

這是最重要的步驟。首先,你必須確保卡爾曼濾波的條件符合你的問題。
我們還記得卡爾曼濾波的兩個典型方程(wiki):
xk=Axk−1 Buk Wk−1″ role=”presentation”>xk=Axk−1 Buk Wk−1xk=Axk−1 Buk Wk−1x_k = Ax_{k-1} Bu_k W_{k-1}
zk=Hxk vk” role=”presentation”>zk=Hxk vkzk=Hxk vkz_k = Hx_k v_k

第一個公式意味著每個xk” role=”presentation”>xkxkx_k可以通過一個線性隨機方程來表示。任何一個xk” role=”presentation”>xkxkx_k都是它前一狀態的的只加上控制訊號uk” role=”presentation”>ukuku_k及處理噪聲Wk−1″ role=”presentation”>Wk−1Wk−1W_{k-1}的線性組合。大多數的情況下,並不需要控制訊號uk” role=”presentation”>ukuku_k。

第二個公式告訴我們,任何測量值(這裡我們並不確定它是否準確)是當前狀態訊號值及測量噪聲的線性組合,我們預設該值服從高斯分佈。

這兩個公式中的處理噪聲及測量噪聲,我們都認為是統計獨立的。

方程式中的係數A,B” role=”presentation”>A,BA,BA,B及H” role=”presentation”>HHH是一般形式的矩陣。但是在大多數的訊號處理問題中,我們使用的模型中,這些係數都僅僅是數值。而且當這些值在狀態間發生變化時,大多數的情況下,我們可以假設它們就是常數。

如果我們確定我們的系統符合這個模型(大多數的系統都是符合的),那麼接下來,就只剩下估計噪聲函式Wk−1″ role=”presentation”>Wk−1Wk−1W_{k-1}及vk” role=”presentation”>vkvkv_k的均值和標準差。我們知道在現實生活中,沒有哪個訊號是純高斯分佈的,但是我們可以通過近似來進行假設。

這不是一個大問題,因為我們將看到卡爾曼濾波演算法試圖收斂到正確的估計,即使高斯噪聲引數估計很差。

這裡我們要始終牢記於心的是:“你估計噪聲引數越準確,你就能獲得更好的估計”

Step2 開始處理

如果你成功的將你的模型擬合到卡爾曼濾波器中,那麼接下來的步驟就是決定必要的引數及你的初始值。

我們有兩個不同的方程集合:時間更新(預測)及測量更新(校準)。兩個方程集合都是應用在第k” role=”presentation”>kkk個狀態

這裡寫圖片描述
我們在步驟一中建立模型,因此我們知道矩陣A,B和H。很可能,他們都是數值常量。並且他們極可能都是數值1.

我建議你們重新寫下這些公式,並看看如何去簡化這些公式。
.
上述公式中最痛苦的事兒就是確定R和Q了。R還相對比較容易找出,因為,通常來講,我們對環境中的噪聲還是比較確定的。但是尋找Q就並不明顯了。在這個階段,我們並不能給你一個指定的方法。

為了開始這個處理,我麼需要知道x0″ role=”presentation”>x0x0x_0和P0″ role=”presentation”>P0P0P_0的估計

Step3 迭代

在我們收集了我們需要的所有資訊後,我們開始處理流程,現在我們可迭代估計。我們要牢記上一狀態的估計將成為當前狀態的估計的輸入。
這裡寫圖片描述
這裡,先驗估計指測量更新校準前的粗略估計。這裡我們使用在測量更新公式中的先驗值

在測量更新公式中,我們找到了在狀態k時x的估計。並且找到了為了狀態k 1估計所需要的值。

我們計算的卡爾曼增益,在下一個迭代步驟中並不需要。 這是一組方程中隱藏的,神祕的,最重要的部分。

我們在測量更新階段評估的值也稱為後驗值。這也是說得通的。

一個簡單的例子

如果小夥伴們在看到這裡還有點稀裡糊塗的,那麼接下來我們找個例子來加深下理解。

現在,讓我們努力去估計一個標量隨機常數,例如來自某個訊號源的“電壓讀數”。假設它有一個恆定的a V電壓,但是我們讀數會不準確,可能會讀高了,也有可能讀低了。我們假設測量噪聲的的標準差為0.1 V。

現在我們來構建我們的模型:
這裡寫圖片描述

就像我之前說的,我們可以把方程簡化成非常簡單的形式。

  • 首先,我們有一個一維的訊號的問題,因此我們模型中的每個係數都是數值,而不是矩陣。
  • 我們並沒有控制訊號uk” role=”presentation”>ukuku_k
  • 因為訊號是一個常數,那麼常數A就是1,因為我們已經知道下一個值等於先前的值。我們很幸運,在這個例子中我們有一個恆定的值,但是即使它具有其他任何線性性質,我們都可以將它假設成1.
  • 值H=1,因為我們知道測量值是由狀態值和一些噪聲組成的。你很少會遇到現實生活中,H與1不相同的情況

最後,我們假設我們得到了下面的測量值

TIME (ms)12345678910
VALUE (V)0.390.500.480.290.250.320.340.480.410.45

我們應該從某處開始,比如說k=0。我們應該找到或者假設某個初始狀態。這裡我們給出了一些初始值。我們假設X0=0″ role=”presentation”>X0=0X0=0X_0=0並且P0=1″ role=”presentation”>P0=1P0=1P_0=1,那麼為什麼我們不選擇P0=0″ role=”presentation”>P0=0P0=0P_0=0呢?這很簡單。如果我們這樣選擇,那麼就意味著環境中不存在噪聲,而這個假設將導致在k狀態下X的所有結果估計值為0(保留初始狀態),所以我們一般不取P0=0″ role=”presentation”>P0=0P0=0P_0=0
我們來寫時間更新和測量更新方程
這裡寫圖片描述

每一輪計算的步驟:
這裡寫圖片描述