卡爾曼濾波算法--核心公式推導導論 - ZZ

卡爾曼濾波算法--核心公式推導導論

 

10 個月前

寫在最前面：這是我第一篇專欄文章，感謝知乎提供這麼一個平臺，讓本身能和你們分享知識。本人會不按期的開始更新文章，文章的內容應該集中在汽車動力學控制，整車軟件架構，控制器等方面。做爲一名在校碩士，不少理解均可能不全面，不正確，你們有不一樣意見歡迎討論。 謝謝！
---------------------------------------------------------------------------------------------------------------------------

卡爾曼濾波算法的應用很普遍，省略不少字。

卡爾曼濾波算法的優勢不少，省略不少字。

卡爾曼濾波算法很牛逼，由於有一堆公式，有一堆符號，看起來就很牛逼啊，乍一看不懂的都很牛逼啊！

本文針對卡爾曼濾波算法的核心公式進行推導，不讓你們被它華麗的外表嚇到。（以後計劃寫關於針對非線性狀況的EKF和UKF，對卡爾曼濾波算法作一個全面一點的應用介紹。感興趣的能夠關注專欄。）

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

Okay，進入正題。這篇文章假設讀者已經對卡爾曼濾波算法有初步的瞭解，知道它能作什麼，知道它的優勢，知道它很牛逼，而且你已經對它產生興趣，但不知道如何下手。

首先給出一個控制理論中公式，別急着翻控制理論的書，沒那麼複雜：

兩個基本問題：

1.卡爾曼濾波算法要作什麼？

對狀態進行估計。

2. 卡爾曼濾波算法怎麼對狀態進行估計？

利用狀態過程噪聲和測量噪聲對狀態進行估計。

一個狀態在一個時刻點k的狀態進入下一個時刻點k+1狀態，會有不少外界因素的干擾，咱們把干擾就叫作過程噪聲，（這個詞一看就是硬翻譯過來的，別在乎爲何叫噪聲）用w表示。任何一個測量儀器，都會有偏差，咱們把這個偏差叫作量測噪聲，用v表示。

回到上面那個公式，狀態方程表示狀態在不斷的更新，從一個時刻點進入下一個時刻點，這個很好理解。關鍵是量測方程，它表示，咱們不斷更新的狀態有幾個能用測量儀器測出來，好比，汽車運動狀態參數有不少，好比速度，輪速，滑移率等，可是咱們只能測量出輪速，所以量測方程要作的就是把狀態參數中能量測的狀態拿出來。

咱們始終要記得咱們要作的事：咱們要獲得的是優化的狀態量Xk。

理解了上面以後就能夠開始推導公式了。

1.首先不考慮過程噪聲對狀態進行更新，很簡單：

舉個例子，v(k)=v(k-1)+at，勻加速運動咯。

2.不考慮測量噪聲取出能測量的狀態，也很簡單：

3.用測量儀器測量出來的狀態值（你們能夠考慮到：測量的值就是被各類噪聲干擾後的真實值）減去上面不考慮噪聲獲得的測量值：

這個值在數學上是一個定義值，叫作新息，有不少有趣的性質，感興趣的能夠本身谷歌。

咱們對步驟暫且停一停。這個叫新息的值有什麼用？由上面的過程咱們能夠明顯看到，它反映了過程噪聲和測量噪聲綜合對測量狀態值的影響，也就是它包含了w和v的狀況。

回到數學層面，（不要懼怕，很簡單的數學應用和思考啦！）一個數值c由兩部份內容a和b組成，那麼怎樣用數學表達式來表達？

通常有兩種作法：

I.直接相加：c=a+b;

II. 用比例的方法：a=n*c,b=(1-n)*c

卡爾曼採用了方法II，用比例的方法來作（其實這也是爲何叫作濾波的緣由，由於濾波就是給權值之類的操做）。也就是說，過程噪聲w=新息*一個比例。這樣獲得的過程噪聲加上原來（第一步）不考慮過程噪聲的狀態值不就是優化值了嗎？ 也就是：

Okay，都寫到這裏了，有必要作一下前提假設：

a. 什麼高斯噪聲，均值爲零一堆；

b.Ak,Ck,wk的協方差Q,vk的協方差R，系統協方差初始值P0,狀態初始值X0，都已知。爲何已知，你實際作項目就知道了。不過不懂的能夠留言或者私信。

那麼到目前爲止咱們的思路就是清楚了，找到一個合適的Hk值（卡爾曼增益），那麼咱們就能獲得狀態的最優值。（卡爾曼說的，不是我說的，因此你問爲何，你要問他，這麼深層次的理論留給博士和學者們去作就好，咱們就現學現用就行，哈哈哈，站在巨人的肩膀！）

問題來了：怎麼獲得合適的Hk？彷佛不是隨便一個參數。

這是偏差協方差矩陣。

思路：使得偏差協方差矩陣Pk最小的Hk。

爲何？這裏我從感觀的角度說明本身的理解，歡迎討論。

協方差表示什麼，協方差表示二者之間的聯繫或者關係，關係越大，協方差越大。偏差協方差越小說明過程噪聲和量測噪聲的關係越小。關係越小能作什麼，這要回到咱們第3步討論的咱們用比例的方法分開了w和v。用比例分開，到底多少屬於w，多少是v，若是關係越小，分開的越精確，好比一堆白砂糖和鹽，若是兩種混合的很均勻，咱們說它關係很大，也就越難用比例的方法將其分開。

4.求的偏差協方差矩陣Pk

天然是把裏面的Xk先獲得，而後公式運算，經過上面的步驟咱們也容易獲得：

而後複雜的數學計算，和以前假設的高斯噪聲，新息的性質之類（至於過程，我的以爲你若是隻作應用，不研究算法，就不必深刻去看了），就能獲得下面的卡爾曼濾波遞推公式：

經過上面的解釋，咱們也就不難知道這些公式都在幹嗎，知道幹嗎就能夠了。在知道A,C,P0,Q,R的狀況下，整個公式的運算流程也都很清晰了。

————————————————————————————————————————

歡迎評論！轉載請註明出處！謝謝你們！