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

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. 這一個狀態的預測值和真實值之間的偏差

過程包括兩個方面：

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

2、由上一個更新值和真實值之間的偏差 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-協方差矩陣