1. 程式人生 > >[Math]理解卡爾曼濾波器 (Understanding Kalman Filter)

[Math]理解卡爾曼濾波器 (Understanding Kalman Filter)

想要 文章 開始 and 結果 update tla 多少 play

1. 卡爾曼濾波器介紹

卡爾曼濾波器的介紹, 見 Wiki

這篇文章主要是翻譯了 Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation

感謝原作者。

如果敘述有誤,歡迎指正!


2. 基本模型

2.1 系統模型

卡爾曼濾波模型假設k時刻的真實狀態是從(k − 1)時刻的狀態演化而來,符合下式:

技術分享 (1)

  • Fk 是作用在 Xk−1 上的狀態變換模型(/矩陣/矢量)。
  • Bk 是作用在控制器向量uk上的輸入-控制模型。
  • Wk 是過程噪聲,並假定其符合均值為零,協方差矩陣為Qk的多元正態分布。

技術分享 (2)

2.2 測量模型

時刻k,對真實狀態 xk的一個測量zk滿足下式:

技術分享 (3)

其中Hk是觀測模型,它把真實狀態空間映射成觀測空間,vk 是觀測噪聲,其均值為零,協方差矩陣為Rk,且服從正態分布。

技術分享 (4)

初始狀態以及每一時刻的噪聲{x0, w1, ..., wk, v1 ... vk} 都認為是互相獨立的.

卡爾曼濾波要做的是:

已知:

  1. 系統的初始狀態 x0

  2. 每個時間的測量 Z

  3. 系統模型和測量模型

求解:

狀態x隨著時間變化而產生的值


3. 預測與更新

3.1 預測方程

預測是這樣一個問題:

已知:

  1. 上一個狀態的更新值
  2. 上一個狀態的更新值和真實值之間的誤差

求解:

  1. 這一個狀態的預測值
  2. 這一個狀態的預測值和真實值之間的誤差

過程包括兩個方面:

一、由上一個更新值 Xk-1|k-1 預測這一個預測值 Xk|k-1

二、由上一個更新值和真實值之間的誤差 Pk-1|k-1 預測下一個預測值和真實值之間的誤差 Pk|k-1

具體來說,就是以下兩個方程。

技術分享 (預測狀態) (5)

技術分享 (預測估計協方差矩陣) (6)

這裏:

Xk-1|k-1 這種記法代表的是上一次的更新值,後面一個 k-1可以看做 Zk-1, 也就是上一次經過對比Zk-1(實際就是更新)之後所估計出的狀態Xk-1。

Xk|k-1 這種記法代表這一次的預測值, 同理於剛才的介紹, 經過上一次Zk-1之後所估計出的狀態Xk。

預測公式-預測狀態
也就是公式(5), 可以直接由系統模型導出。

預測公式-協方差矩陣:

P代表著估計誤差的協方差,代表著一種 confidence ,比如先驗估計誤差(預測值與真實值之間誤差)的協方差

技術分享


註:

方差有兩種形式

技術分享

技術分享

協方差的定義:

技術分享

如果說方差是用來衡量一個樣本中,樣本值的偏離程度的話。協方差就是用來衡量兩個樣本之間的相關性有多少,也就是一個樣本的值的偏離程度,會對另外一個樣本的值偏離產生多大的影響,協方差是可以用來計算相關系數的,相關系數P=Cov(a.b)/Sa*Sb, Cov(a.b)是協方差, Sa Sb 分別是樣本標準差。【可以參考另一篇博客《理解協方差》】

cov(x,y)
= E( (x-u)(y-v)‘ )
= E(xy‘ - xv‘ - uy‘ + uv‘)
= E(xy‘) - E(xv‘) - E(uy‘) + E(uv‘)
= E(xy‘) - uv‘ - uv‘ + uv‘
= E(xy‘) - uv‘


比較(5)和(1), 相減:

技術分享

由於 狀態估計誤差 和 系統噪聲 是不相關的

技術分享

註:

如果隨機變量 x和y是不相關的, 那麽

Cov(x,y) = 0
=> E( (x-ex)(y-ey)‘ ) = 0
=> E(xy‘) - ex*ey‘ = 0

如果 ex 和ey為0 => E(x,y‘) = 0 就像上面的情況, 誤差和噪聲都服從正態分布,所以期望都是0 .

獨立的充要條件:P(xy) = P(x)P(y)

3.2 更新方程

更新過程實際上就是一下問題:

已知:

  1. 由上一個更新值得到的當前的預測值。
  2. 當前的觀測值
  3. 觀測模型

求解:

  1. 融合了預測值和觀測的更新值
  2. 由預測值的估計誤差得到更新值的估計誤差

更新方程如下:

技術分享

技術分享

其中K稱為kalman增益, 就像一個補償,決定著預測值應該變化多少幅度,才能變成更新值。
技術分享

先看一個簡單的例子,從這個例子中來推導出這三個方程。

3.3 簡單的例子

3.3.1 舉例

技術分享

有一個直線軌道, 軌道上有一個火車,從火車站出發, 在t時刻,火車想要知道自己距離火車站的位置,可以有兩個信息來源:

  1. 根據 t-1時刻的狀態信息,以及一些控制信息來推斷, 狀態包括 t-1時刻的位置、速度等, 控制信息包括司機剎車、加速等等。
  2. 根據 t時刻的測量數據來推斷, 這裏假設車上有一個聲波發射器,可以探測到發射到火車站需要多少時間,進而得到離車站的距離。

要想得到一個比較好的結果,顯然不能只依靠某一種方法來推斷,而 Kalman Filter的方法是:

  • 首先, 利用t-1時刻進行推斷, 這一步叫預測

  • 然後, 利用t時刻的measurement 也可以推斷, 使用這個推斷對預測進行校正, 這一步叫更新

技術分享

3.3.2 火車位置 - 預測

t=0的時候, 火車狀態如 Figure.2 ,這時候, 火車的位置是比較準確的。

t=1的時候, 火車的預測狀態如 Figure.3 可以看到, 位置的方差變大了。

技術分享

火車的預測主要遵循 式(1),而預測的方差在不斷變大,也就是說預測的準確度在下降, 這是由累積誤差 w 導致的。

3.3.3 火車位置 - 測量

t=1 的時候,我們還有 measurement ,同樣可以推斷火車的位置,見下圖中的藍色的pdf

技術分享

3.3.4 火車位置 - 更新

將兩個pdf相乘,得到下圖中的綠色pdf, 綠色pdf中較高的位置, 意味著預測和測量對這個位置都比較支持。

技術分享

這裏有一個高斯分布的一個重要性質就是,兩個高斯分布的乘積還是高斯分布。

This is critical as it permits an endless number of Gaussian pdfs to be multiplied over time, but the resulting
function does not increase in complexity or number of terms; after each time epoch the new pdf is fully represented by a Gaussian function. This is the key to the elegant recursive properties of the Kalman filter。

3.3.5 推導更新方程

紅色的pdf是預測的火車位置, 方程如下:

技術分享

藍色的pdf是測量的火車位置, 方程如下:

技術分享

綠色的pdf二者融合的或者位置, 方程如下:

技術分享

寫成如下形式:

技術分享

這裏:

技術分享
技術分享

這兩個式子,就是kalman濾波的更新方程

但是,這只是一個很特殊的例子,因為這裏假設預測和測量都是采用同樣的坐標系

更現實的情況是二者需要統一到一個 domain 中

比如上面所舉的例子中:

預測的時候, 預測值是用米作為單位的。
但是當測量的時候, 測量得到的值是用聲波經過的秒數作為單位的。

必須先要把兩個量統一到同一個domain才能進行融合。

比如上式子中, y2 (measurement)實際是聲波傳遞時間的一個正態分布,也就是說單位是秒。

一般做法是把
預測值 => 測量值

y1就變成:

技術分享

y2不變:

技術分享

這樣兩個坐標系都在 mesaurement domain 了。

兩個pdf所在坐標系的橫軸都是表示時間,而且以秒為單位了。

`
統一domain之後,更新方程就有了如下形式

3.3.5.1 期望更新方程:

技術分享

技術分享

H = 1/c

K = 技術分享 代入,得到

技術分享

這裏的H就相當於觀測方程中的H, K就是卡爾曼增益。

3.3.5.2 方差更新方程:

類似地, 融合之後的方差更新變成了

技術分享

4. 總結

各個變量對應的情況如下:

技術分享

技術分享

最終的更新方程

技術分享

5. 實現

Talk is cheap, show me the code.

%本例子從百度文庫中得到, 稍加註釋
clear
N=200;%取200個數

%% 生成噪聲數據 計算噪聲方差
w=randn(1,N); %產生一個1×N的行向量,第一個數為0,w為過程噪聲(其和後邊的v在卡爾曼理論裏均為高斯白噪聲)
w(1)=0;
Q=var(w); % R、Q分別為過程噪聲和測量噪聲的協方差(此方程的狀態只有一維,方差與協方差相同) 

v=randn(1,N);%測量噪聲
R=var(v);

%% 計算真實狀態
x_true(1)=0;%狀態x_true初始值
A=1;%a為狀態轉移陣,此程序簡單起見取1
for k=2:N
    x_true(k)=A*x_true(k-1)+w(k-1);  %系統狀態方程,k時刻的狀態等於k-1時刻狀態乘以狀態轉移陣加噪聲(此處忽略了系統的控制量)
end


%% 由真實狀態得到測量數據, 測量數據才是能被用來計算的數據, 其他都是不可見的
H=0.2;
z=H*x_true+v;%量測方差,c為量測矩陣,同a簡化取為一個數


%% 開始 預測-更新過程

% x_predict: 預測過程得到的x
% x_update:更新過程得到的x
% P_predict:預測過程得到的P
% P_update:更新過程得到的P

%初始化誤差 和 初始位置
x_update(1)=x_true(1);%s(1)表示為初始最優化估計
P_update(1)=0;%初始最優化估計協方差

for t=2:N
    %-----1. 預測-----
    %-----1.1 預測狀態-----
    x_predict(t) = A*x_update(t-1); %沒有控制變量
    %-----1.2 預測誤差協方差-----
    P_predict(t)=A*P_update(t-1)*A‘+Q;%p1為一步估計的協方差,此式從t-1時刻最優化估計s的協方差得到t-1時刻到t時刻一步估計的協方差

    %-----2. 更新-----
    %-----2.1 計算卡爾曼增益-----
    K(t)=H*P_predict(t) / (H*P_predict(t)*H‘+R);%b為卡爾曼增益,其意義表示為狀態誤差的協方差與量測誤差的協方差之比(個人見解)
    %-----2.2 更新狀態-----
    x_update(t)=x_predict(t)  +  K(t) * (z(t)-H*x_predict(t));%Y(t)-a*c*s(t-1)稱之為新息,是觀測值與一步估計得到的觀測值之差,此式由上一時刻狀態的最優化估計s(t-1)得到當前時刻的最優化估計s(t)
    %-----2.3 更新誤差協方差-----
    P_update(t)=P_predict(t) - H*K(t)*P_predict(t);%此式由一步估計的協方差得到此時刻最優化估計的協方差
end

%% plot
%作圖,紅色為卡爾曼濾波,綠色為量測,藍色為狀態
%kalman濾波的作用就是 由綠色的波形得到紅色的波形, 使之盡量接近藍色的真實狀態。
t=1:N;
plot(t,x_update,‘r‘,t,z,‘g‘,t,x_true,‘b‘);

6. Reference

Wiki-卡爾曼濾波器

Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation

Wiki-協方差矩陣

[Math]理解卡爾曼濾波器 (Understanding Kalman Filter)