Wiki - Kalman Filter
中文介紹
似乎對於object的位置與軌跡偵測有幫助
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
X(k-1|k-1) 前一狀態最佳結果
X(k|k-1) 利用上一狀態的預測
U(k) 狀態控制量
A、B 參數
P(k|k-1)=A P(k-1|k-1) A'+Q ……… (2)
P 對應X的Covarience
Q 系統的Covarience
前兩個是對系統的預測
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
Kg (Kalman gain)
Z(k) 測量到的新數值
H 參數
Kg(k)= P(k|k-1) H' / (H P(k|k-1) H' + R) ……… (4)
P(k|k)=(I-Kg(k) H)P(k|k-1) ……… (5)
I 單位矩陣
假如我們要估算k時刻的是實際溫度值。首先你要根據k-1時刻的溫度值,來預測k時刻的溫度。因為你相信溫度是恆定的,所以你會得到k時刻的溫度預測值是跟 k-1時刻一樣的,假設是23度,同時該值的高斯噪聲的偏差是5度(5是這樣得到的:如果k-1時刻估算出的最優溫度值的偏差是3,你對自己預測的不確定度是4度,他們平方相加再開方,就是5)(Step 1 and 2, state and covariance prediction)。然後,你從溫度計那裡得到了k時刻的溫度值,假設是25度,同時該值的偏差是4度。
由於我們用於估算k時刻的實際溫度有兩個溫度值,分別是23度和25度。究竟實際溫度是多少呢?相信自己還是相信溫度計呢?究竟相信誰多一點,我們可以用他們的 covariance來判斷。因為Kg^2=5^2/(5^2+4^2),所以Kg=0.78 (Step 4, Kalman gain correction),我們可以估算出k時刻的實際溫度值是:23+0.78* (25-23)=24.56度 (Step 3, state correction)。可以看出,因為溫度計的covariance比較小(比較相信溫度計),所以估算出的最優溫度值偏向溫度計的值。
現在我們已經得到k時刻的最優溫度值了,下一步就是要進入k+1時刻,進行新的最優估算。到現在為止,好像還沒看到什麼自回歸的東西出現。對了,在進入 k+1時刻之前,我們還要算出k時刻那個最優值(24.56度)的偏差。算法如下:((1-Kg)*5^2)^0.5=2.35 (Step 5, covariance correction)。這裡的5就是上面的k時刻你預測的那個23度溫度值的偏差,得出的2.35就是進入k+1時刻以後k時刻估算出的最優溫度值的偏差(對應於上面的3)。
(1)
X(2|1) = X(1|1) + 0
23 = 23 + 0
(2)
P(2|1) = P(1|1) + Q
5 = (3^2 + 4^2)^0.5
Z(2) = 25
R = 4
(4)
Kg(2) = P(2|1) H' / (H P(2|1) H' + R)
0.78 = (5^2 / (5^2 + 4^2))^0.5
(3)
X(2|2) = X(2|1) + Kg(2) (Z(2) - H X(2|1))
24.56 = 23 + 0.78 * (25 - 23)
(5)
P(2|2) = (I - Kg(2) H) P(2|1)
2.35 = ((1-0.78)*5^2)^0.5