擴展卡爾曼濾波器

在估计理论中，擴展卡爾曼濾波器(extended Kalman filter），簡稱EKF，是卡尔曼滤波的非線性版本，會對平均和协方差的估測值進行線性化。在轉移模型定義良好的情形下，一般會將擴展卡爾曼濾波器視為非線性狀態觀測器、导航系统和全球定位系统的de facto標準。

歷史

建立卡爾曼濾波器數學基礎的論文是在1959年到1961年之間發行的。若是線性系統模型，在轉換系統和量測系統上有獨立白色噪音，卡尔曼滤波是最佳的線性估測器。只是，大部份工程領域的系統屬於非線性系統，因此許多人努力將濾波器應用在非線性系統上。大部份的研究是在艾姆斯研究中心完成的。擴展卡爾曼濾波器應用微积分学裡的多變數泰勒级数展開，在工作點附近對模型線性化。若系統模型未知或是不準確，也可以用蒙地卡羅方法（特別是粒子濾波器）來進行估測。蒙地卡羅方法出現的時間比擴展卡爾曼濾波器要早，但在中等大小的狀態空間中，其運算量會比擴展卡爾曼濾波器高很多。

公式

在擴展卡爾曼濾波器，狀態轉換模型和觀測模型可以不是狀態的線性函數，只要是可微函数即可。

${\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}$

此處w_k和v_k是過程雜訊和觀測雜訊，假設是平均為0的多元正态分布雜訊，其协方差矩阵為Q_k和R_k。u_k是控制向量。

可以用函數f從過去的觀測值中計算預測值，並且用h從預測狀態中計算預測量測量。不過f和h無法直接計算协方差。會改計算偏微分矩陣（雅可比矩阵）。

在每一步時間，會用當時的預測狀態計算雅可比矩阵。矩陣可以用在卡爾曼濾波器方程中。此作法在本質上在目前的預測附近對非線性函數線性化。

離散時間預測方程和更新方程

標示${\hat {\mathbf {x} }}_{n\mid m}$表示$\mathbf {x}$在時間n的估測值，假設觀測值一直到m ≤ n。

預測

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

更新

新息（Innovation）或測量殘差	${\tilde {\boldsymbol {y}}}_{k}={\boldsymbol {z}}_{k}-h({\hat {\boldsymbol {x}}}_{k|k-1})$
新息或殘餘協方差	${\boldsymbol {S}}_{k}={{\boldsymbol {H}}_{k}}{\boldsymbol {P}}_{k|k-1}{{\boldsymbol {H}}_{k}^{T}}+{\boldsymbol {R}}_{k}$
接近最佳化的卡爾曼增益	${\boldsymbol {K}}_{k}={\boldsymbol {P}}_{k|k-1}{{\boldsymbol {H}}_{k}^{T}}{\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}$

其中狀態變換和估測矩陣可以用以下的雅可比矩阵定義

${{\boldsymbol {F}}_{k}}=\left.{\frac {\partial f}{\partial {\boldsymbol {x}}}}\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}}$

缺點和其他替代方案

卡爾曼濾波器是最佳的估測器，但擴展卡爾曼濾波器多半不是最佳估測器（若量測和狀態轉換模型都線性時，擴展卡爾曼濾波器是最佳估測器，但此時的擴展卡爾曼濾波器就是卡爾曼濾波器）此外，若狀態的初始估測錯誤，或是過程的建模不正確，因為線性化的關係，濾波器會快速發散。另一個擴展卡爾曼濾波器的問題是其估測协方差矩阵常常會低估，因此若沒有加入「穩定性雜訊」，在統計觀點上可能會有不一致的風險 。

另外也要考慮到非線性濾波問題在統計的本質上是無限維的，不適合用單一的平均和方差-協方差的估測器來完全表示最佳化濾波器。即使在非常簡單的一維系統（例如立方感測器，觀測值和實際狀態之間的關係是三次方的）下，擴展卡爾曼濾波器也可能會有很差的性能，其最佳化濾波器可能是雙模（兩個最大值）的，其且其結構很複雜，無法有效的用單一的均值和方差估測器來表示，對於二次感測器也有類似問題。 針對這些案例，已找到投影濾波器是替代方案，已應用在導航上。其他通用的非線性濾波器（像是全粒子濾波器）也可以考慮。

雖然如此，擴展卡爾曼濾波器因為有還可接受的性能，仍是導航和GPS的業界標準。

廣義的擴展卡爾曼濾波器

連續時間的擴展卡爾曼濾波器

其模型如下;

${\begin{aligned}{\dot {\mathbf {x} }}(t)&=f{\bigl (}\mathbf {x} (t),\mathbf {u} (t){\bigr )}\\\mathbf {z} (t)&=h{\bigl (}\mathbf {x} (t){\bigr )}(t)\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)^{T}-\mathbf {K} (t)\mathbf {H} (t)\mathbf {P} (t)+\mathbf {Q} (t)\\\mathbf {K} (t)&=\mathbf {P} (t)\mathbf {H} (t)^{T}\mathbf {S} (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}}$

和離散時間的擴展卡爾曼濾波器不同，在連續時間的擴展卡爾曼濾波器裡，預測和更新步驟是互相耦合的。

離散時間量測

大部份的物理系統是連續時間模型，但會用數位處理器在離散時間量測，以進行狀態估測。因此，系統模型和量測模型為

${\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)^{T}+\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}^{T}{\bigl (}\mathbf {H} _{k}\mathbf {P} _{k|k-1}\mathbf {H} _{k}^{T}+\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）。若保留泰勒級數展開的高次項，即可建構高次的擴展卡爾曼濾波器。有論文曾提及二階和三階的擴展卡爾曼濾波器。但高階擴展卡爾曼濾波器只有在量測誤差小的前提下，才能看出其性能上的優勢。

非加性雜訊的形成和方程式

典型的擴展卡爾曼濾波器假設可加性過程以及可加性量測雜訊。但在擴展卡爾曼濾波器實現時，不一定都能符合此一條件 。考慮以下通用型式的系統：

${\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})$

其中w_k和v_k是過程噪訊和量測噪訊，兩者都假設是平均為0的多元正态分布雜訊，其协方差矩阵 Q_k為R_k。其协方差預測和新息（innovation）方程為

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

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

其中矩陣${\boldsymbol {L}}_{k-1}$和矩陣${\boldsymbol {M}}_{k}$是雅可比矩陣：

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

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

預測的狀態估測和量測殘餘是以過程的平均以及量測雜訊項的平均來表示，兩者都假設為0。不然，非加性雜訊可以用加性雜訊的擴展卡爾曼濾波器的作法來實現。

隱式擴展卡爾曼濾波器

有時，非線性系統的觀測模型無法直接求解${\boldsymbol {z}}_{k}$，只能用隱函數表示：

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

其中${\boldsymbol {z}}_{k}={\boldsymbol {z'}}_{k}+{\boldsymbol {v}}_{k}$是有雜訊的觀測量。

可以在以下的調整之後，使用擴展卡爾曼濾波器：

${{\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},{\boldsymbol {z}}_{k}}$

此處原來的觀測共變異數矩陣已經轉換，以不同的方式定義新息（innovation）${\tilde {\boldsymbol {y}}}_{k}$。雅可比矩陣${{\boldsymbol {H}}_{k}}$定義不變，但是是用隱觀測模型$h({\boldsymbol {x}}_{k},{\boldsymbol {z}}_{k})$來決定。

相關條目

• 卡尔曼滤波
• 集合卡爾曼濾波器
• 快速卡爾曼濾波器
• 不變擴展卡爾曼濾波器
• 滾動時域估計
• 粒子濾波器
• 濾波問題
• 投影濾波器

延伸閱讀

• Anderson, B.D.O.; Moore, J.B. Optimal Filtering. Englewood Cliffs, New Jersey: Prentice–Hall. 1979. 

• Gelb, A. Applied Optimal Estimation. MIT Press. 1974. 

• Jazwinski, Andrew H. Stochastic Processes and Filtering. Mathematics in Science and Engineering. New York: Academic Press. 1970: 376. ISBN 978-0-12-381550-7. 

• Maybeck, Peter S. Stochastic Models, Estimation, and Control. Mathematics in Science and Engineering 141–1. New York: Academic Press. 1979: 423. ISBN 978-0-12-480701-3. 

外部連結

• Position estimation of a differential-wheel robot based on odometry and landmarks