擴展卡曼濾波器

擴展卡曼濾波器（英文：Extended Kalman filter, EKF）係擴展原本針對線性系統嘅卡曼濾波器嘅，令到非線性系統啲平均值同埋協方差得到線性化並攞嚟做預測同埋修正。喺啲過渡模型有明確定義到嘅情況下，EKF 得認為^[1]係個 de facto 標準喺非線性狀態估計理論、導航系統同埋GPS當中。^[2]

公式

喺擴展卡曼濾波器中，狀態轉移同觀察模型嘸需要係線性函數畀狀態，而係可以係可微得嘅函數^[3]^:407：

{\boldsymbol {x}}_{k}=f_{k-1}({\boldsymbol {x}}_{k-1},{\boldsymbol {u}}_{k-1},{\boldsymbol {w}}_{k-1})

{\boldsymbol {z}}_{k}=h_{k}({\boldsymbol {x}}_{k},{\boldsymbol {v}}_{k})

呢度 ${\boldsymbol {u}}_{k}$ 係枚控制向量， ${\boldsymbol {w}}_{k}$ 同 ${\boldsymbol {v}}_{k}$ 係過程噪音同觀測噪音，都着假定係零均值嘅多元噪音、分別有協方差 ${\boldsymbol {Q}}_{k}$ 同 ${\boldsymbol {R}}_{k}$ 嘅，即有 ${\textstyle {\boldsymbol {w}}_{t}\sim (0,{\boldsymbol {Q_{t}}}),{\boldsymbol {v}}_{t}\sim (0,{\boldsymbol {R_{t}}})}$ 。

函數 $f$ 可以攞嚟根據先前個估計值預測下一狀態；相類似，函數h 可以攞嚟根據預測狀態計算預測嘅測量值。之但係， $f$ 同 $h$ 嘸直接噉應用得喺協方差上，而係要計返個偏導數矩陣（Jacobi矩陣）。

喺每個時間𨂾，Jacobi 矩陣都藉由噉時嘅預測狀態評估到。啲矩陣可以用喺卡曼濾波器方程入便。呢個過程本質上係線性化個非線性函數畀噉時嘅估計。

離散時間預測同更新方程

符號 ${\hat {\mathbf {x} }}_{n\mid m}$ 表示對 $\mathbf {x}$ 喺時間 $n$ 嘅估計，由啲觀察結果一路到並包括埋時間 $m\leq n$ 嘅畀出。具體理解個過程可以參睇卡曼濾波器#過程。^[3]^:409

預測

預測狀態估計	${\hat {\boldsymbol {x}}}_{k\|k-1}=f({\hat {\boldsymbol {x}}}_{k-1\|k-1},{\boldsymbol {u}}_{k-1},0)$
預測協方差估計	${\boldsymbol {P}}_{k\|k-1}={{\boldsymbol {F}}_{k}}{\boldsymbol {P}}_{k-1\|k-1}{{\boldsymbol {F}}_{k}^{\top }}+{{\boldsymbol {L}}_{k}}{\boldsymbol {Q}}_{k}{{\boldsymbol {L}}_{k}^{\top }}$

更新

新息或者測量殘差	${\tilde {\boldsymbol {y}}}_{k}={\boldsymbol {z}}_{k}-h({\hat {\boldsymbol {x}}}_{k\|k-1},0)$
新息（或者殘差）協方差	${\boldsymbol {S}}_{k}={{\boldsymbol {H}}_{k}}{\boldsymbol {P}}_{k\|k-1}{{\boldsymbol {H}}_{k}^{\top }}+{{\boldsymbol {M}}_{k}}{\boldsymbol {R}}_{k}{{\boldsymbol {M}}_{k}^{\top }}$
近最優嘅卡曼增益	${\boldsymbol {K}}_{k}={\boldsymbol {P}}_{k\|k-1}{{\boldsymbol {H}}_{k}^{\top }}{\boldsymbol {S}}_{k}^{-1}$
更新唨嘅狀態估計	${\hat {\boldsymbol {x}}}_{k\|k}={\hat {\boldsymbol {x}}}_{k\|k-1}+{\boldsymbol {K}}_{k}{\tilde {\boldsymbol {y}}}_{k}$
更新唨嘅協方差估計	${\boldsymbol {P}}_{k\|k}=({\boldsymbol {I}}-{\boldsymbol {K}}_{k}{{\boldsymbol {H}}_{k}}){\boldsymbol {P}}_{k\|k-1}$

其中狀態轉移矩陣同觀測矩陣定義為以下啲 Jacobi 矩陣； ${\boldsymbol {F}}_{k},{\boldsymbol {H}}_{k}$ 係相對於狀態值嘅， ${\boldsymbol {L}}_{k},{\boldsymbol {M}}_{k}$ 係相對於噪音值嘅：

{{\boldsymbol {F}}_{k}}=\left.{\frac {\partial f}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k-1}}

，

{{\boldsymbol {L}}_{k}}=\left.{\frac {\partial f}{\partial {\boldsymbol {w}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k-1}}

{{\boldsymbol {H}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1}}

，

{{\boldsymbol {M}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {v}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1}}

加性過程（additive process）同測量噪音嘅情況下可以睇作：

{\boldsymbol {x}}_{k}=f({\boldsymbol {x}}_{k-1},{\boldsymbol {u}}_{k-1})+{\boldsymbol {w}}_{k-1}

{\boldsymbol {z}}_{k}=h({\boldsymbol {x}}_{k})+{\boldsymbol {v}}_{k}

而 ${\boldsymbol {L}}_{k},{\boldsymbol {M}}_{k}$ 就等於 1。

缺點

戥線性嘅傳統卡曼濾波器嘸同，擴展卡曼濾波器通常嘸係最佳嘅估計器（若果測量同狀態轉換模型都係線性嘅就係最佳，因為喺種情況下擴展卡曼濾波器同常規濾波器係相同嘅）。此外，若果狀態嘅初始估計錯唨，或者個過程建模嘸正確，係噉個濾波器可能會由於個線性化處理而迅速發散。擴展卡曼濾波器嘅另一個問題係估計嘅協方差矩陣好多時係低估唨真正嘅協方差矩陣，係噉嘸去添加「穩定噪音」嘅話，有可能啲估計風險喺統計意義上會變得嘸一致。

雖然係噉，擴展卡曼濾波器都可以提供到合理嘅表現，而且可以講係導航系統同 GPS 嘅 de facto 標準。

普適化

連續時間擴展卡曼濾波器

模型

{\begin{aligned}{\dot {\mathbf {x} }}(t)&=f{\bigl (}\mathbf {x} (t),\mathbf {u} (t){\bigr )}+\mathbf {w} (t)&\mathbf {w} (t)&\sim {\mathcal {N}}{\bigl (}\mathbf {0} ,\mathbf {Q} (t){\bigr )}\\\mathbf {z} (t)&=h{\bigl (}\mathbf {x} (t){\bigr )}+\mathbf {v} (t)&\mathbf {v} (t)&\sim {\mathcal {N}}{\bigl (}\mathbf {0} ,\mathbf {R} (t){\bigr )}\end{aligned}}

初始化

{\hat {\mathbf {x} }}(t_{0})=E{\bigl [}\mathbf {x} (t_{0}){\bigr ]}{\text{, }}\mathbf {P} (t_{0})=Var{\bigl [}\mathbf {x} (t_{0}){\bigr ]}

預測更新

{\begin{aligned}{\dot {\hat {\mathbf {x} }}}(t)&=f{\bigl (}{\hat {\mathbf {x} }}(t),\mathbf {u} (t){\bigr )}+\mathbf {K} (t){\Bigl (}\mathbf {z} (t)-h{\bigl (}{\hat {\mathbf {x} }}(t){\bigr )}{\Bigr )}\\{\dot {\mathbf {P} }}(t)&=\mathbf {F} (t)\mathbf {P} (t)+\mathbf {P} (t)\mathbf {F} (t)^{\top }-\mathbf {K} (t)\mathbf {H} (t)\mathbf {P} (t)+\mathbf {Q} (t)\\\mathbf {K} (t)&=\mathbf {P} (t)\mathbf {H} (t)^{\top }\mathbf {R} (t)^{-1}\\\mathbf {F} (t)&=\left.{\frac {\partial f}{\partial \mathbf {x} }}\right\vert _{{\hat {\mathbf {x} }}(t),\mathbf {u} (t)}\\\mathbf {H} (t)&=\left.{\frac {\partial h}{\partial \mathbf {x} }}\right\vert _{{\hat {\mathbf {x} }}(t)}\end{aligned}}

戥離散時間擴展卡曼濾波器嘸同，預測𨂾同更新𨂾喺連續時間擴展卡曼濾波器度耦合到（coupled）。 ^[4]

離散時間測量

大多數物理系統都表示為連續時間模型，而離散時間測量經常透過數字處理器進行狀態估計。係噉，系統模型同測量模型由下式畀出

{\begin{aligned}{\dot {\mathbf {x} }}(t)&=f{\bigl (}\mathbf {x} (t),\mathbf {u} (t){\bigr )}+\mathbf {w} (t)&\mathbf {w} (t)&\sim {\mathcal {N}}{\bigl (}\mathbf {0} ,\mathbf {Q} (t){\bigr )}\\\mathbf {z} _{k}&=h(\mathbf {x} _{k})+\mathbf {v} _{k}&\mathbf {v} _{k}&\sim {\mathcal {N}}(\mathbf {0} ,\mathbf {R} _{k})\end{aligned}}

其中 $\mathbf {x} _{k}=\mathbf {x} (t_{k})$ 。

初始化

{\hat {\mathbf {x} }}_{0|0}=E{\bigl [}\mathbf {x} (t_{0}){\bigr ]},\mathbf {P} _{0|0}=E{\bigl [}\left(\mathbf {x} (t_{0})-{\hat {\mathbf {x} }}(t_{0})\right)\left(\mathbf {x} (t_{0})-{\hat {\mathbf {x} }}(t_{0})\right)^{T}{\bigr ]}

預測

{\begin{aligned}{\text{solve }}&{\begin{cases}{\dot {\hat {\mathbf {x} }}}(t)=f{\bigl (}{\hat {\mathbf {x} }}(t),\mathbf {u} (t){\bigr )}\\{\dot {\mathbf {P} }}(t)=\mathbf {F} (t)\mathbf {P} (t)+\mathbf {P} (t)\mathbf {F} (t)^{\top }+\mathbf {Q} (t)\end{cases}}\qquad {\text{with }}{\begin{cases}{\hat {\mathbf {x} }}(t_{k-1})={\hat {\mathbf {x} }}_{k-1|k-1}\\\mathbf {P} (t_{k-1})=\mathbf {P} _{k-1|k-1}\end{cases}}\\\Rightarrow &{\begin{cases}{\hat {\mathbf {x} }}_{k|k-1}={\hat {\mathbf {x} }}(t_{k})\\\mathbf {P} _{k|k-1}=\mathbf {P} (t_{k})\end{cases}}\end{aligned}}

其中

\mathbf {F} (t)=\left.{\frac {\partial f}{\partial \mathbf {x} }}\right\vert _{{\hat {\mathbf {x} }}(t),\mathbf {u} (t)}

更新

\mathbf {K} _{k}=\mathbf {P} _{k|k-1}\mathbf {H} _{k}^{\top }{\bigl (}\mathbf {H} _{k}\mathbf {P} _{k|k-1}\mathbf {H} _{k}^{\top }+\mathbf {R} _{k}{\bigr )}^{-1}

{\hat {\mathbf {x} }}_{k|k}={\hat {\mathbf {x} }}_{k|k-1}+\mathbf {K} _{k}{\bigl (}\mathbf {z} _{k}-h({\hat {\mathbf {x} }}_{k|k-1}){\bigr )}

\mathbf {P} _{k|k}=(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k})\mathbf {P} _{k|k-1}

其中

{\textbf {H}}_{k}=\left.{\frac {\partial h}{\partial {\textbf {x}}}}\right\vert _{{\hat {\textbf {x}}}_{k|k-1}}

啲更新方程戥離散時間擴展卡曼濾波器相同。

高階擴展卡曼濾波器

上述遞歸係針對一階擴展卡曼濾波器。高階啲嘅 EKF 可以透過保留多啲嘅泰勒級數展開項嚟獲取。譬如，二階同三階 EKF經已得到描述。 ^[5]之但係，啲高階 EKF 好多時唯有喺測量噪音比較細嗰陣喺性能上正有優勢。

隱式擴展卡曼濾波器

喺某啲情況下，非線性系統嘅觀測模型冇辦法求解得到 ${\boldsymbol {z}}_{k}$ ，但可以表示成隱函數：

h({\boldsymbol {x}}_{k},{\boldsymbol {z'}}_{k})={\boldsymbol {0}}

其中 ${\boldsymbol {z}}_{k}={\boldsymbol {z'}}_{k}+{\boldsymbol {v}}_{k}$ 係帶噪音嘅觀察。

傳統嘅擴展卡曼濾波器可以做噉樣嘅替換： ^[6] ^[7]

{{\boldsymbol {R}}_{k}}\leftarrow {{\boldsymbol {J}}_{k}}{{\boldsymbol {R}}_{k}}{{\boldsymbol {J}}_{k}^{T}}

{\tilde {\boldsymbol {y}}}_{k}\leftarrow -h({\hat {\boldsymbol {x}}}_{k|k-1},{\boldsymbol {z}}_{k})

其中：

{{\boldsymbol {J}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {z}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1},{\hat {\boldsymbol {z}}}_{k}}

當中個原始觀測協方差矩陣 ${{\boldsymbol {R}}_{k}}$ 變唨型，而且新息 ${\tilde {\boldsymbol {y}}}_{k}$ 嘅定義亦都嘸同。Jacobi 矩陣 ${{\boldsymbol {H}}_{k}}$ 定義跟前便一樣，但係係由隱式觀測模型 $h({\boldsymbol {x}}_{k},{\boldsymbol {z}}_{k})$ 確定到。

睇埋

考

↑ Julier, S.J.; Uhlmann, J.K. (2004). "Unscented filtering and nonlinear estimation" (PDF). Proceedings of the IEEE. 92 (3): 401–422. doi:10.1109/jproc.2003.823141. S2CID 9614092.
↑ Courses, E.; Surveys, T. (2006). Sigma-Point Filters: An Overview with Applications to Integrated Navigation and Vision Assisted Control. Nonlinear Statistical Signal Processing Workshop, 2006 IEEE. pp. 201–202. doi:10.1109/NSSPW.2006.4378854. ISBN 978-1-4244-0579-4. S2CID 18535558.
↑ ^3.0 ^3.1 Simon, Dan (2006). Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches. John Wiley & Sons. ISBN 978-0-471-70858-2.
↑ Brown, Robert Grover; Hwang, Patrick Y.C. (1997). Introduction to Random Signals and Applied Kalman Filtering (第3版). New York: John Wiley & Sons. pp. 289–293. ISBN 978-0-471-12839-7.
↑ Einicke, G.A. (2019). Smoothing, Filtering and Prediction: Estimating the Past, Present and Future (2nd ed.). Amazon Prime Publishing. ISBN 978-0-6485115-0-2.
↑ Quan, Quan (2017). Introduction to multicopter design and control. Singapore: Springer. ISBN 978-981-10-3382-7.
↑ Zhang, Zhengyou (1997). "Parameter estimation techniques: a tutorial with application to conic fitting" (PDF). Image and Vision Computing. 15 (1): 59–76. doi:10.1016/s0262-8856(96)01112-2. ISSN 0262-8856.

[Julier20042-1] Julier, S.J.; Uhlmann, J.K. (2004). "Unscented filtering and nonlinear estimation" (PDF). Proceedings of the IEEE. 92 (3): 401–422. doi:10.1109/jproc.2003.823141. S2CID 9614092.

[Courses20062-2] Courses, E.; Surveys, T. (2006). Sigma-Point Filters: An Overview with Applications to Integrated Navigation and Vision Assisted Control. Nonlinear Statistical Signal Processing Workshop, 2006 IEEE. pp. 201–202. doi:10.1109/NSSPW.2006.4378854. ISBN 978-1-4244-0579-4. S2CID 18535558.

[:0-3] 3.0 ^3.1 Simon, Dan (2006). Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches. John Wiley & Sons. ISBN 978-0-471-70858-2.

[4] Brown, Robert Grover; Hwang, Patrick Y.C. (1997). Introduction to Random Signals and Applied Kalman Filtering (第3版). New York: John Wiley & Sons. pp. 289–293. ISBN 978-0-471-12839-7.

[5] Einicke, G.A. (2019). Smoothing, Filtering and Prediction: Estimating the Past, Present and Future (2nd ed.). Amazon Prime Publishing. ISBN 978-0-6485115-0-2.

[Quan_2017_p.-6] Quan, Quan (2017). Introduction to multicopter design and control. Singapore: Springer. ISBN 978-981-10-3382-7.

[Zhang_1997_pp._59–76-7] Zhang, Zhengyou (1997). "Parameter estimation techniques: a tutorial with application to conic fitting" (PDF). Image and Vision Computing. 15 (1): 59–76. doi:10.1016/s0262-8856(96)01112-2. ISSN 0262-8856.

[1]

[2]

[3]

[4]

[5]

[6]

[7]