卡爾曼濾波原理

以最簡單的,單輸入,單輸出系統爲例

這個視頻《理解卡爾曼濾波器 - Understanding Kalman Filters》講的比較好,

視頻鏈接https://www.bilibili.com/video/av24098897/?p=3


問題分析:很顯然,我們可以通過對速度v的積分來獲取當前位置,可惜這個值s1還有累計誤差,而且當小車打滑、過溝時,我們手裏的v的值也就並不是那麼準確,這樣得出的預測值s1,就不是很可靠。這一過程我們稱爲:用系統的數學模型來預估。

還有一個獲得路程s的方法就是讀取GPS的值,減掉小車的起始位置,爲了敘述簡便,我們直接把這兩者的差值叫做:GPS的輸出值,由於GPS的輸出值是有噪聲的,GPS讀出的測量值S2,也不是很可靠。這一過程我們稱爲:用測量值來預估。

控制器的計算過程只能是離散的,無法使用積分,只能使用累加:假設我們每20ms計算一次,那麼預測值:

s1(k)=s1(k-1) + Bv + w

式中,B爲採樣時間0.02s,v爲小車的速度m/s,w爲誤差,一般假設該誤差符合正態分佈N(0, Q),0爲均值,Q爲方差

另一個預測值我們直接從GPS讀出:

s2(k)=sg + z

式中,sg爲GPS讀出的值,z爲GPS的噪聲引起的誤差,一般假設該誤差符合正態分佈N(0, R),0爲均值,R爲方差

注意:由於s1的累計誤差問題,實際上,我們遞推s1(k)時,並不是用的直接用的s1(k-1),而是用卡爾曼最優的估計值。


卡爾曼濾波器公式:

sk(k)= sk(k-1)+Bv + k*(sg - sk(k-1))

式中,sk(k)爲第k次卡爾曼的估計值,sg爲GPS讀出的值,k爲係數,顯然這裏k=1

前兩項稱爲:預估器(在讀GPS之前的預估值),第三相稱爲:更新值



在瞭解卡爾曼濾波器之前,先得來了解一下“狀態觀測器”。場景是這樣的:在t時刻給火箭提供的燃料量爲w(t),火箭噴嘴的溫度爲Tin,爲了瞭解Tin(t)與w(t)的關係,我們必須要獲取很多組樣本(w1,Tin1)、(w2,Tin2)······,可惜噴嘴的溫度太高,我們無法測量噴嘴的溫度,取而代之,我們只能測量噴嘴外部的外殼的溫度Text

  

上圖(左)中,上面爲真正的火箭,下面爲它的數學模型,因爲沒有直接饋通,該模型不含D矩陣,當然,這個數學模型我們無法使它精確的描述火箭,暫且用一個一階系統來描述,即右圖。我們的目的是,通過輸入量u,實時預測噴嘴真正的溫度x,我們能拿到的數字量有:輸入量u,實測的外殼溫度y,根據這兩個值,我們把u代入數學模型表達式(上面右圖的下面的框圖),可以得到用數學模型預測的外殼溫度值和噴嘴溫度,那麼這個預測值能不能當做噴嘴的真實溫度x呢?要回答這個問題,我們必須先研究一下這兩者的差值:ex


K=Bk,兩式相減得:

解這個一階線性微分方程,得:,其中ex0爲ex在0時刻的初值

顯然,當A-KC<0時,預測的噴嘴溫度與真實的噴嘴溫度x的差值,會隨着時間的延長,逐漸變爲0,這時,我們就可以用來當做x的最優預測值了。顯然,增大K值,可以使e更快的變爲0。


下面進入卡爾曼濾波器:

情景假設:我們有一輛小車,我們可以用遙控器任意調節小車的速度v,小車上安裝了一個GPS,任務要求:讓小車從一個位置沿直線出發,然後實時返回已走過的路徑長度s。


小車實際的狀態空間模型如上圖所示,這個表達式是用離散狀態空間表達式。w和v分別代表了系統誤差和測量誤差,一般假設兩者都滿足正態分佈:w~N(0,Q),v~N(0, R)。Q代表了用數學模型來預測真實系統的偏離的程度。


上圖中,上半部爲實際的小車,下半部爲我們提出的數學預測模型,這樣就跟前文的火箭類似了,這裏我們能通過模型給出一個預測的位置值,也能通過GPS獲得一個測量值y。

真實的小車模型中帶有系統誤差,例如車輪打滑,或者車輪進坑等,使得路程不完全等於速度的積分。這裏相比火箭問題更糟糕的情況是,測量值也有很大誤差,火箭問題中,我們就把測量的外殼溫度當做外殼的真實溫度了,但在這裏,GPS的測量路程,我們還不能完全當做真實的路程,它也有誤差.

數學模型給出的預測值爲:


式中,爲預測值,u爲輸入值,yk爲GPS的測量值,k是反饋係數。這個式子就是我們的狀態觀測器。

上式的前兩項稱爲預估器,第三項稱爲更新值,把上式簡化一下,變爲:


實際上,我們在計算預估值,和更新值時,還得順便計算一下新的系統誤差的協方差和測量誤差的協方差,也即:




發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章