前言：
最近在研究鼎鼎大名的卡爾曼濾波器 Kalman Filter。這個濾波器已經發展很久囉，因為太威了，各種資料的處理，數據的融合都少不了它。所以被廣泛地使用在工業上的各行各業，航天，導航，汽車，無人車控制，...甚至是電腦裡滑鼠位置速度的讀取，連最近最夯的BMS電池管理系統都可以見到它的身影。它其實是個數學統計理論加上馬可夫鏈的一個結果，核心的五個公式，可以透過嚴密的統計跟控制理論完整的推導出來。不過對初學者而言，直接透過反直覺的這五個矩陣運算的複雜公式來學習卡爾曼濾波器，除了直接被嚇跑，恐怕也得不到任何新的洞見跟有價值的想法。
所以最好的學習方式，還是先從簡單的一維公式開始，比較可以容易操作跟熟悉所有卡爾曼濾波器的想法。等非常熟悉後，再回頭去看這五個卡爾曼濾波器的數學推導跟所有動態模型的條件跟假設，這樣在學習上會比較有幫助些。
所以這篇來用一維的卡爾曼濾波器公式先來熟悉，跟練功。比較嚴密跟太數學的推導之後再說吧！(留到下一篇)
然後為了推廣 FORTH 語言的立場，這篇裡面的程式碼還是採用 FORTH 來撰寫。 gFORTH 有非常棒雙精確度的浮點運算，拿來當作數值運算的環境是非常方便的。
當然這篇也算是一種個人的學習筆記，方便我自己以後忘記囉可以回來查詢囉。
卡爾曼濾波器 Kalman Filter
很細的介紹 Wiki 百科都有，就不贅述了，直接跳入比較務實的計算操作
卡爾曼濾波器 Kalman Filter 計算步驟
主要分兩個步驟： (1) 預測，(2) 更新 (資料融合)
(1) 預測:
利用物體狀態改變的線性模型，利用上一次得到的最佳值，對它的下一個狀態跟誤差值進行預測，得到一個新的狀態跟誤差值的預測值。
這裡，狀態值用 X 來標注。狀態的誤差值就是變異數，用 p 來標注
這裡的預測，要完全看你的模型的。
例如，飛機在天上以 Xv 的速度飛行，經過 T 時間後，假如我們的狀態 Xp 是位置，。這時候新的位置的預測會是 Xp' = Xp + Xv*T
我們位置狀態誤差值的變異數是 Pp，速度狀態誤差值的變異數Pv
又變異數 Var(X) = E(X^2) - E(X)^2
所以 Var(a*X) = E((a*X)^2) - E(a*X)^2 = a^2*E(X) - a^2*E(X)^2 = a^2*Var(X)
所以
Pp' = Var(Xp') = Var(Xp+Xv*T) = Var(Xp) + Var(Xv*T) = Var(Xp) + T^2*Var(Xv) = Pp + T^2*Pv
Pv' = Pv
每次，我們都可以根據上次經過量測，資料融合後的最佳估計值，根據物體狀態改變的線性模型，來預測系統下一次最新的狀態跟誤差變異數。這樣的預測值，會提供給下次的量測值，來進行資料融合，得到最新最準確的狀態預估值跟誤差。
(2) 更新 (資料融合):
對物體進行量測，得到新的量測值跟誤差值的變異數。 將新的量測值跟預測值進行資料融合。得到最準確的狀態預估值跟誤差。
這裡的資料融合的思維，是卡爾曼濾波器的核心。 (請參考這個 YouTube 教學連結)
假設今天有兩筆資料，一筆誤差比較大，另外一筆誤差比較小。卡爾曼認為不需要丟棄誤差比較大的資料，因為即使是誤差比較大的資料，裏頭仍然有有用的資訊。只要適當的利用統計公式將兩筆資料適當的「融合」成更準確的資料。融合過的資料當會比原來的兩筆都更為準確。
那麼，如何做這個融合的動作呢？答案來自於兩個常態分佈的相乘！(兩常態分佈相乘，會依舊是一個常態分佈)
假定資料一來自於特定平均值，標準差下的常態分佈的母群體。資料二來自於另一個特定平均值，標準差下的常態分佈的母群體。這時候如果將此兩個常態分佈相乘，就可以得到融合這兩個母群體特性的第三個常態分佈。
根據這樣的想法，就可以推得卡爾曼濾波器的核心資料融合公式。
假定：
物體的狀態遵守平均值 X, 標準差 Sigma-p (或變異數 p) 的常態分佈
量測遵守平均值Z, 標準差 Sigma-r (或變異數 r) 的常態分佈
兩個分佈相乘，融合成新的分佈
整理一下
新的，融合過後的常態分佈，新的平均值標註為 X', 新的標準差標註為 Sigma-p', 新的變異數標註為 p'
所以，得到新的平均值 x'，新的變異數 p' 融合的公式囉！
發現可以用一個權重係數 Kalman Gain 來簡化上面的公式。卡爾曼權重 Kalman Gain，簡寫為 KG，定義為 p/(p+r)
所以，
得到我們的預測值跟量測值的資料融合卡爾曼濾波器公式
又
得到我們的預測值跟量測值的變異數資料融合卡爾曼濾波器公式
範例：台北101高度量測
今天我們手上有個靠氣壓計來量測高度的高度計，這個高度計還蠻不錯的，每次量測的時候除了顯示出當時的絕對高度外，還會估出此次量測的標準差出來。我們試著用這個氣壓高度計，用無人機飛上台北101的尖塔上，量測一下 Taipei 101 的高度吧！假設10次量測後的高度跟誤差 1sigma 標準差如下，來用卡爾曼濾波器來預估下真正台北101的高度吧！
來示範一下怎麼計算，
(1) 第一次 量測值 Z = 470.8, 變異數 r = 30^2 = 900
預測：
因為尚未有前一次狀態預估值X，跟狀態誤差變異數 p，所以直接拿量測值來當起始值
X=470.8, p = r = 900
這裡我們還有一個叫做 q 的，這個稱之為系統的雜訊變異數。原來 Taipei 101 畢盡不是數學上的東西，而是真實世界上的建築。既然是真實世界上的東西，那就不可能是定值。 Taipei 101 的真實高度，其實隨時受到風的吹襲，或是突然來了個地震，會稍微地受到一點影響。這樣的誤差被稱之為系統的「雜訊」，假設這個雜訊也遵守高斯分佈，所以可以用q 的變異數來描述它。
這裡為了簡化計算，我們假設它是零。
融合：
第一次，量測值被拿來當起始值囉，所以沒資料可以融合。
(2) 第二次 量測值 Z = 542.0, 變異數 r = 30^2 = 900
預測：
上一次的 X = 470.8, p = 900
因為 Taipei 101 是不會動的靜態建築，所以新的預測值跟上一次的最佳預估值一樣， 所以新的預測值 X=470.8, p=900
融合：
Kalman Gain
KG = p / (p +r) = 900 / (900 + 900) = 0.5
X' = X + KG(Z-X) = 470.8 + 0.5*(542.0-470.8) = 506.4
p' = (1-KG)*p = (1-0.5)*900 = 450
融合後的最佳預估值為 X' = 506.4, p' = 450
(3) 第三次 量測值 Z = 404.5, 變異數 r = 50^2 = 2500
預測：
上一次的 X = 506.4, p = 450
因為 Taipei 101 是不會動的靜態建築，所以新的預測值跟上一次的最佳預估值一樣， 所以新的預測值 X=506.4, p=450
融合：
Kalman Gain
KG = p / (p +r) = 450 / (450 + 2500) = 0.152542372
X' = X + KG(Z-X) = 506.4 + 0.152542372*(404.5-506.4) = 490.8559323
p' = (1-KG)*p = (1-0.152542372)*450 = 381.3559326
融合後的最佳預估值為 X' = 490.8559323, p' = 381.3559326
...
十次量測計算結果如下
製成圖表，每次量測後的最佳估計值及對應的標準偏差 1sigma
這裡可以看得到卡爾曼濾波器的運作邏輯囉，卡爾曼增益 KG 是一種變異數的加權係數。 KG=1 表示預測值的變異程度太大，所以完全採用量測值來預估。 KG=0 表示量測值的變異太大，完全採用預測值來預估。每次量測的變異數決定了此次資料融合入正確資料的程度。
例如 Taipei 101 這10筆資料，第五筆跟第六筆，量測標準差為 10，是10筆中最精確的兩筆資料。所以最佳估計值立即向這兩筆資料收斂。
第7, 8, 9, 10 這四筆資料，因為量測表準差是 70, 70, 100, 100 遠比當時經過第五第六筆數據收斂過後的最佳標準差 6.59 還差太多囉，所以 KG 的權重自然會變小，這四筆新量測的資料融合進來的影響力就大幅下修。
最後經過這10次的量測，卡爾曼濾波器告訴我們， Taipei 101 的高度是 505.3 公尺，一個標準差的誤差是 6.51 公尺。
也就是說，我們有 95.45% 的信賴程度， Taipei 101 的高度會是 503.3 公尺 +/- 13.02 公尺。
(Taipei 101 的真實高度 508 公尺)
這個數據比單純使用平均值490.62 公尺還準確些，且卡爾曼濾波器可以給出最終的標準差出來，告訴你最佳估計值可以被信賴的程度。這是只單純使用平均值所得不到的資訊。
FORTH 程式執行結果
將這些計算步驟，轉化成 FORTH 程式碼後， Taipei 101 高度計算結果如下
FORTH 程式碼簡介
正統的 FORTH 是不用浮點運算的。不過這裡，因為我們只是要做卡爾曼濾波器的研究，應用的對象未明，這時候還是用浮點運算比較方便些。當然啦，只要對象確定，捨棄浮點，改用固定整數比例運算也不會太難。所以這裡還是把 gFORTH 當數值分析軟體使用，採用浮點運算啦！
定義三個變數，
變數 xx 是系統狀態估計值x
變數 pp 是系統狀態估計的誤差值p (共變數)
因為系統狀態的雜訊所造成的誤差值q (共變數)是固定的，所以定義成變數 qq
KFinit 用來起始這三個變數
: KFinit ( F: x p q --)
fdup qq f!
f+ pp f! \ 一開始的 p = p + q
xx f!
;
給定 p, r 計算 Kalman Gain
公式 KG = p / ( p + r )
: KG ( F: p r -- KG) \ calculate Kalman Gain
fover f+ f/ \ KG = p / (p + r)
;
predict 用來定義依據狀態改變模型，每次 x, p 的狀態改變。在這裏，我們的台北101的狀態是靜態的，不會變，所以無狀態改變。
: predict ( --) \ based on your state transition model
;
然後是重頭戲 update，這裡因為這是資料融合的步驟，所以改用 merge 的名字。
資料融合的過程，將卡爾曼濾波器內部所預估出來的最佳資料，跟最新的量測值跟其量測誤差，以卡爾曼濾波器公式進行資料融合，將新量測值所得到的最新資訊，改善既有的舊資訊並「融合」成最新更準確的資料。
: merge ( F: z r --)
\ 計算 Kalman Gain
pp f@ fswap ( F: z p r )
KG ( F: z KG)
\ 融合新的估計值 x' = x + KG * (z - x)
fswap xx f@ f- \ F: KG z-x
fover f* \ F: KG KG*(z-x)
xx f@ f+ \ F: KG KG*(z-x)+x
xx f!
\ 融合新的估計誤差值 p' = p * (1 - KG) + q
1E0 fswap f- \ F: 1-KG
pp f@ f* qq f@ f+ \ F: (1-KG)*p+q
pp f!
;
整個卡爾曼濾波器運作兩步驟： (1) 預測，(2)量測值融合 (更新)
給定 z 新量測值，跟 r 此次量測誤差值 (共變數)。然後執行卡爾曼濾波器運作，預測跟量測數據融合成最新的數據。
: estimate ( F: z r --)
predict
merge
;
列印結果
: .est
cr
." Estimate:" xx f? cr
." Variance:" pp f? cr
;
最後，
完整FORTH程式碼列表
\
\ Kalman Filter in 1 dimension
\ Frank Lin 2021.4.29
\
fvariable xx ( estimate state)
fvariable pp ( estimate variance)
fvariable qq ( state process noise variance)
: KFinit ( F: x p q --)
fdup qq f!
f+ pp f! \ p' = p + q
xx f!
;
: KG ( F: p r -- KG) \ calculate Kalman Gain
fover f+ f/ \ KG = p / (p + r)
;
: predict ( --) \ based on your state transition model
;
: merge ( F: z r --)
\ calculate Kalman Gain
pp f@ fswap ( F: z p r )
KG ( F: z KG)
\ update: x' = x + KG * (z - x)
fswap xx f@ f- \ F: KG z-x
fover f* \ F: KG KG*(z-x)
xx f@ f+ \ F: KG KG*(z-x)+x
xx f!
\ update: p' = p * (1 - KG) + q
1E0 fswap f- \ F: 1-KG
pp f@ f* qq f@ f+ \ F: (1-KG)*p+q
pp f!
;
: estimate ( F: z r --)
predict
merge
;
: f? ( addr --) f@ f. ;
: .est
cr
." Estimate:" xx f? cr
." Variance:" pp f? cr
;
XXX
留言列表
