卡曼濾波器

卡曼濾波器（英文：Kalman filter）， 又叫卡曼濾波，亦都叫做線性二次估計（英文：linear quadratic estimation，LQE），係一種統計同控制理論算法，針對啲線性、理應係高斯誤差嘅系統嘅；而非線性系統就需要用到擴展卡曼濾波器嚟線性化過佢。卡曼濾波器跟住時間推移，攞到啲一系列嘅觀測值同埋好多時仲有系統斷估值、啲值包含有統計噪音同其他嘸准確性嘅，並根據每橛時間範圍內啲變數嘅聯合概率分佈嚟改善啲對未知變數嘅估計；好多時噉樣互相更新嘅估計值係準確過基於單個測量做嘅估計。個濾波器得名自Rudolf E. Kálmán，佢係個理論嘅主要開發者之一。

卡曼濾波器跟踪系統嘅估計狀態，以及估計方差或者話嘸確定性。更新估計使用到狀態轉換模型同測量。${\hat {x}}_{k\mid k-1}$表示個齋斷估值畀時間𨂾 k處個系統狀態，喺未考慮第 k個測量值y_k之前嘅；$P_{k\mid k-1}$表示佢對應嘅嘸確定性。

概述

卡曼濾波算法分兩重工作。喺預測步驟裏便，卡曼濾波器產生啲對當前狀態變數及佢個嘸確定性嘅估計。一旦觀察到下一次測量嘅嗰個結果、個值梗會因一定量嘅誤差包括隨機噪音而有啲走趲嘅，就使用加權平均值（「卡曼增益」，英文：Kalman gain）嚟更新過呢啲估計值，並為具有確定性高啲嘅估計值畀多啲權重。呢個算法係遞歸嘅，佢可以實時運行得，僅使用到當前嘅輸入測量值同先前計算有嘅狀態，以及佢個嘸確定性矩陣，而嘸需要額外嘅過去信息。

卡曼濾波器嘅最佳性係假設個誤差係高斯類型。用 Rudolf E. Kálmán嘅話嚟講：「總而言之，以下假設係作畀啲隨機過程嘅：物理隨機現象可能可以認為係由於啲主要隨機源（primary random sources）激發咗啲動態系統。假設啲主要隨機源係有零平均值嘅獨立高斯隨機過程；噉啲動態系統就會係線性嘅。」 即管戥高斯無關，若果個過程同埋啲測量協方差係已知嘅，噉個卡曼濾波器係喺最細均方誤差意義上最佳嘅線性估計器。

過程

系統狀態方程

内文：狀態空間 § 線性系統

參睇線性系統嘅狀態空間方程，有：

$\mathbf {x} _{t+1}=\mathbf {A} _{t}\mathbf {x} _{t}+\mathbf {B} _{t}\mathbf {u} _{t}+\mathbf {w} _{t}$

$\mathbf {z} _{t+1}=\mathbf {C} _{t}\mathbf {x} _{t}+\mathbf {v} _{t}$

其中啲關鍵嘅：

$\mathbf {w} _{t},\mathbf {v} _{t}$分別係過程噪音同埋測量噪音，服從到啲平均值零嘅高斯分佈，即${\textstyle \mathbf {w} _{t}\sim N(0,\mathbf {Q_{t}} ),\mathbf {v} _{t}\sim N(0,\mathbf {R_{t}} )}$；

$\mathbf {z} _{t+1}$ 係輸出值，呢度係觀測模型個測量值。

$\mathbf {A} _{t}$係系統矩陣，亦即係狀態矩陣；$\mathbf {B} _{t}$係輸入矩陣；$\mathbf {C} _{t}$ 係觀測矩陣。

喺以下啲式當中，設卡曼濾波係一𨂾𨂾噉做，所以用 ${k}$表示啲時間步；而卡曼濾波器當中個狀態轉移矩陣同埋觀測矩陣就分別爲$\mathbf {F} _{t}$同$\mathbf {H} _{t}$，而且係隔開個噪音，所以有：

${\widehat {\mathbf {x} }}_{k+1|k}^{-}=\mathbf {F} _{k}{\widehat {\mathbf {x} }}_{k|k}+\mathbf {B} _{k}\mathbf {u} _{k}$ （預測方程）

${\widehat {\mathbf {z} }}_{k+1}=\mathbf {H} _{k+1}{\widehat {\mathbf {x} }}_{k+1|k}^{-}$ （觀測方程）

預測

設喺 ${k}$嗰陣攞到最佳值即送去做預測，得到到一隻先驗斷估值同埋佢個協方差：

${\widehat {\mathbf {x} }}_{k+1|k}^{-}=\mathbf {F} _{k}{\widehat {\mathbf {x} }}_{k|k}+\mathbf {B} _{k}\mathbf {u} _{k}$

$\mathbf {P} _{k+1|k}^{-}=\mathbf {F} _{k}\mathbf {P} _{k|k}\mathbf {F} _{k}^{\mathrm {T} }+\mathbf {Q} _{k}$

其中協方差表示個斷估值喺噪音影響下有幾大嘅範圍。

測量同更新

過唨一𨂾時間𨂾之後，卡曼濾波器攞到新嘅測量資訊$\mathbf {z} _{k}$，戥個「擬測量值」$\mathbf {H} _{k+1}{\widehat {\mathbf {x} }}_{k+1|k}^{-}$求差、個由斷估值轉成嘅，可以求得到個新息值（英文：innovation，即測量值多出擬測量值嘅部分）同埋佢個協方差：

${\widehat {\mathbf {y} }}_{k+1}=\mathbf {z} _{k+1}-\mathbf {H} _{k+1}{\widehat {\mathbf {x} }}_{k+1|k}^{-}$

$\mathbf {S} _{k+1}=\mathbf {H} _{k+1}\mathbf {P} _{k+1|k}^{-}\mathbf {H} _{k+1}^{\mathrm {T} }+\mathbf {R} _{k+1}$

注意到由於卡曼濾波器嘸帶噪音，所以個觀測模型$\mathbf {H} _{k+1}$夠勁嘅話 ${\widehat {\mathbf {y} }}_{k+1}$理應畀到似$\mathbf {v} _{t}$ 噉嘅噪音。

係噉攞得到所謂嘅卡曼增益：

$\mathbf {K} _{k+1}=\mathbf {P} _{k+1|k}^{-}\mathbf {H} _{k+1}\mathbf {S} _{k+1}^{-1}$

藉由呢個卡曼增益即更新得原本個斷估值成好啲嘅估計值，同埋更新埋個協方差：

${\widehat {\mathbf {x} }}_{k+1|k+1}={\widehat {\mathbf {x} }}_{k+1|k}^{-}+\mathbf {K} _{k+1}{\widehat {\mathbf {y} }}_{k+1}$

$\mathbf {P} _{k+1|k+1}=(\mathbf {I} -\mathbf {K} _{k+1}\mathbf {H} _{k+1})\mathbf {P} _{k+1|k}^{-}$，或者$\mathbf {P} _{k+1|k+1}=\mathbf {P} _{k+1|k}^{-}-\mathbf {K} _{k+1}\mathbf {S} _{k+1}\mathbf {K} _{k+1}^{\mathrm {T} }$

係噉，又可以即刻返到預測嗰匹繼續做落去。

示意圖

下低係分𨂾總覽示意圖，橢圓啲係多變數高斯分佈，所有啲符號喺上高都得到解釋過。

簡化場景

直接輸出預測值嘅話，相當於喺$\mathbf {H} =1$場景下，係有返啲簡啲嘅式：

${\widehat {\mathbf {y} }}_{k+1}=\mathbf {z} _{k+1}-{\widehat {\mathbf {x} }}_{k+1|k}^{-}$

$\mathbf {S} _{k+1}=\mathbf {P} _{k+1|k}^{-}+\mathbf {R} _{k+1}$

$\mathbf {K} _{k+1}=\mathbf {P} _{k+1|k}^{-}\mathbf {S} _{k+1}^{-1}={\dfrac {\mathbf {P} _{k+1|k}^{-}}{\mathbf {P} _{k+1|k}^{-}+\mathbf {R} _{k+1}}}$

$\mathbf {P} _{k+1|k+1}=\mathbf {P} _{k+1|k}^{-}-\mathbf {K} _{k+1}\mathbf {P} _{k+1|k}^{-}$

校參

參睇卡曼增益同埋估計更新：

${\widehat {\mathbf {x} }}_{k+1|k+1}={\widehat {\mathbf {x} }}_{k+1|k}^{-}+\mathbf {K} _{k+1}{\widehat {\mathbf {y} }}_{k+1}$

通過校個卡曼增益大細可以校得到個比例（加權重），想倚賴模型嘅斷估值多啲定倚賴帶觀測嘅新息值多啲。

校卡曼增益大細

考慮卡曼增益嘅式：

$\mathbf {K} _{k+1}=\mathbf {P} _{k+1|k}^{-}\mathbf {H} _{k+1}\mathbf {S} _{k+1}^{-1}$

捉啲$\mathbf {P} _{k+1|k}^{-}$、$\mathbf {S} _{k+1}$換齊成$\mathbf {P} _{k|k}$，可以有一樸樚式：

$\mathbf {K} _{k+1}={\dfrac {\mathbf {F} _{k}\mathbf {P} _{k|k}\mathbf {F} _{k}^{\mathrm {T} }+\mathbf {Q} _{k}}{\mathbf {H} _{k+1}(\mathbf {F} _{k}\mathbf {P} _{k|k}\mathbf {F} _{k}^{\mathrm {T} }+\mathbf {Q} _{k})\mathbf {H} _{k+1}^{\mathrm {T} }+\mathbf {R} _{k+1}}}$

注意到裏便有$\mathbf {Q} _{k},\mathbf {R} _{k+1}$，可以考慮模型嘅過程噪音、傳感器嘅觀測噪音嚟設置。喺簡易場景下（$\mathbf {F} =1$同$\mathbf {H} =1$），可以有下式：

$\mathbf {K} _{k+1}={\dfrac {\mathbf {P} _{k|k}+\mathbf {Q} _{k}}{\mathbf {P} _{k|k}\mathbf {+} \mathbf {Q} _{k}+\mathbf {R} _{k+1}}}$

可以直觀噉睇得到$\mathbf {Q} _{k},\mathbf {R} _{k+1}$嘅設置係點影響返卡曼增益嘅大細。

拓展

卡曼濾波方法嘅擴展同廣義化都得到咗開發，譬如非線性系統適用嘅擴展卡曼濾波器同無跡卡曼濾波器。底層模型係一個隱馬可夫模型，其中啲潛在變數個狀態空間係連續嘅，而且所有啲潛在同觀察到嘅變數都具有高斯分佈。另外，卡曼濾波器都已成功噉用喺多傳感器融合同分佈式傳感器網絡，嚟開發分佈式或者共識卡曼濾波器。

應用

卡曼濾波器喺技術上有好多應用。一個常見嘅應用係攞嚟引導、導航同控制啲載具，特別係飛機、航天器同動態定位嘅船舶。另外，卡曼濾波器係喺時間序列分析裏便廣泛應用開嘅概念，用喺訊號處理同計量經濟學等啲領域。卡曼濾波器亦都係喺機械人運動規劃同控制個領域嘅主要課題之一，用喺軌跡優化都得。卡曼濾波器仲有用喺模擬中樞神經系統對運動嘅控制。由於發出運動命令同接收感官回輸之間存在有時間延遲，使用卡曼濾波器嘅話即支持返一種現實模型，可以估計得運動系統嘅當前狀態而且發出啲更新命令。