擴展卡爾曼濾波器

在估計理論中，擴展卡爾曼濾波器（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}$$

此處wk和vk是過程雜訊和觀測雜訊，假設是平均為0的多元正態分布雜訊，其協方差矩陣為Qk和Rk。uk是控制向量。

可以用函數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{align} \dot{\mathbf{x}}(t) &= f\bigl(\mathbf{x}(t), \mathbf{u}(t)\bigr) \\ <pre> \mathbf{z}(t) &= h\bigl(\mathbf{x}(t)\bigr)(t) </pre> \end{align}$$

初始化

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

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

#### 離散時間量測

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

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

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

其中

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

其中wk和vk是過程噪訊和量測噪訊，兩者都假設是平均為0的多元正態分布雜訊，其協方差矩陣 Qk為Rk。其協方差預測和新息（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}^{T}_{k-1}}$$

$$\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})$來決定。

## 修改版本及其他替代方案

### 迭代擴展卡爾曼濾波器

迭代擴展卡爾曼濾波器（iterated extended Kalman filter）改進了擴展卡爾曼濾波器的線性化，作法是重覆的調整泰勒展開的中心點。這減少了線性化的誤差，其代價是計算量的增加。

### 強健擴展卡爾曼濾波器

強健擴展卡爾曼濾波器（robust extended Kalman filter）在目前狀態估測處對信號模型進行線性化，再用線性的卡爾曼濾波來預測下一個估測。此作法設法要產出區域最佳的濾波器，但不一定會收斂，因為底層黎卡提方程的解不保證正定。有一個改善性能的方式是偽代數Riccati技巧（faux algebraic Riccati technique）

會為在穩定性和最佳化之間權衡。此作法會維持熟悉的擴展卡爾曼濾波器結構，但是在增益設計時，選擇一個偽代數Riccati方程的正定解，以達到穩定性。

另一個提昇擴展卡爾曼濾波器性能的作法是導入強健控制中，H-infinity的結果。可以在設計Riccati方程時，加入正定項，以得到強健濾波器。新增的項有個純量的參數，讓設計者在均方誤差性能和最大誤差性能兩者之間取捨。

### 不變擴展卡爾曼濾波器

不變擴展卡爾曼濾波器（invariant extended Kalman filter，IEKF）是修改版的擴展卡爾曼濾波器，是針對有對稱（不變量）的非線性系統。此作法結合了EKF和近來引入對稱保持濾波器的優點。不變擴展卡爾曼濾波器不是使用以線性輸出誤差為基礎的線性修正項，此方法使用以不變量輸出誤差為基礎的幾何適應性校正項，同樣地，增益矩陣也不是用線性狀態誤差來更新，而是以不變量狀態誤差來更新。主要的好處是增益和增益和協方差方程式可以在比平衡點大得多的軌跡集上收斂到常數值（擴展卡爾曼濾波器只能在平衡點上收斂），因此估測的收斂情形會比較好。

### 無跡卡爾曼濾波器

無跡卡爾曼濾波器（unscented Kalman filter，簡稱UKF）是一個有可能可以改善擴展卡爾曼濾波器的非線性卡爾曼濾波器。在無跡卡爾曼濾波器中，會用確定的取樣點近似機率密度，取樣點將底層的分佈以正態分布來表示。這些點的非線性變換旨在估計後驗機率，其矩可以從變換後的樣本中導出。此轉換稱為無跡變換。在各方向的估計誤差上，無跡卡爾曼濾波器比擴展卡爾曼濾波器更加強健，也更加準確。

"擴展卡爾曼濾波器（EKF）可能是非線性系統最廣為使用的估測演演算法。不過，估測社群超過35年的經驗，證實此方法很難實現、很難調校、只在一些在更新時間尺度上幾乎線性的系統上是可靠的。很多的困難點就是來此方法所作的其線性化。"

2012年有一篇包括模擬結果的論文，指出一些已發表的UKF變體不比和二階擴展卡爾曼濾波器（Second Order Extended Kalman Filter，SOEKF）準確二階擴展卡爾曼濾波器也稱為增廣卡爾曼濾波器（augmented Kalman filter），比UKF早大約35年，最早是由Bass等人提出。要實現針對非線性狀態轉換的卡爾曼性濾波器，其困難點來自為了精度所需要的數值穩定性議題，而UKF也沒有躲過這個問題，其中也有用到線性化，稱為線性回歸。UKF的穩定性問題是來自協方差矩陣的方根，而EKF和SOEKF的穩定性問題來自沿著軌跡的泰勒級數近似。

### 集合卡爾曼濾波器

集合卡爾曼濾波器（Ensemble Kalman filter）是由Evensen在1994年發明，比UKF要晚。集合卡爾曼濾波器和UKF相比較的優點是其集合成員可以比狀態維度要少很多，因此可以用在非常高維的系統中，例如氣象預報，其狀態空間大小會到十億以上。

### 模糊卡爾曼濾波器

近來有提出模糊卡爾曼濾波器（Fuzzy Kalman filter）以及一種表示可能性分佈（possibility distributions）的方法，用可能性分佈取代機率分佈，從而獲得真正的可能性濾波器（genuine possibilistic filter），可以用非對稱的過程以及觀測雜訊，以及過程模型和觀測型的較高不準確性。

## 外部連結

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