# Error-state Kalman Filter (誤差狀態濾波器)
**English version:** https://hackmd.io/@2eDHc86oQa-I55HsX-Kmww/SJ-etBJYA
**Author:** Teng-Hu Cheng(程登湖)
**email:** [e-mail]
**Date:** July 2024
**Website:** https://ncrl.lab.nycu.edu.tw/member/
**Slides:** https://sites.google.com/nycu.edu.tw/robotics-aerial-robots2024/course-materials

## Block Diagram(系統方塊圖)
假設我們要融合IMU與GPS，並考量IMU的訊號雜訊與飄移，我們必須使用“誤差狀態濾波器”進行更精準的濾波。整體的“誤差狀態濾波器”可用以下的方塊圖表達。

## The error-state Kalman filter explained

在誤差狀態濾波器的設計中，我們談到真實狀態(true state)、名義狀態(nominal state)和誤差狀態值(error state)，其中真實狀態表達為名義狀態和誤差狀態的合適組合（線性和、四元數乘積或矩陣乘積）。這個概念是將名義狀態視為大信號（以非線性方式可積分)，而誤差狀態則視為小信號（因此可以線性積分並適用於線性高斯濾波)。

可以如下解釋誤差狀態濾波器。一方面，高頻率的 IMU 數據 $\mathbf{u}_m$ 被整合到名義狀態 $\mathbf{x}$ 中。這個名義狀態沒有考慮噪聲項 $\mathbf{w}$ 和其他可能的模型缺陷。因此，它將累積誤差。這些誤差被收集在誤差狀態 $\delta \mathbf{x}$ 中，並使用誤差狀態卡爾曼濾波器 (ESKF) 進行估計，此時包含所有的噪聲和干擾。誤差狀態由小信號幅度組成，其演變函數由（時變的）線性動態系統正確定義，其動態、控制和測量矩陣從名義狀態值計算得出。在名義狀態積分的同時，ESKF 預測誤差狀態的高斯估計。它僅進行預測，因為目前沒有其他測量可用來校正這些估計。濾波器校正在 IMU 之外的其他信息到達時進行（例如 GPS、視覺等），這些信息能夠使誤差可觀測，並且這種情況通常發生在積分階段的速度更低。這種校正提供了誤差狀態的後驗高斯估計。在此之後，誤差狀態的均值被注入到名義狀態中，然後重置為零。誤差狀態的協方差矩陣也被方便地更新以反映這一重置。系統就這樣永遠運行下去。

### Definition of Variables (變數定義)
先定義相關變數如下：
其中誤差狀態信號 $\delta \mathbf{q}$ 和 $\delta \mathbf{R}$ 可以近似為：

$$
\delta \mathbf{q} \approx \begin{bmatrix} 1 \\ \frac{\delta \boldsymbol{\theta}}{2} \end{bmatrix}
\]
\[
\delta \mathbf{R} \approx \mathbf{I} + [\delta \boldsymbol{\theta}]_\times
$$
### True State Dynamics (理想狀態動態)
True state為理想的狀態，因他不含雜訊與飄移(noise、bias)，但實際上無法達成，因此我們退而求其次，只尋找nominal state來使用。
$$\begin{align*}
\dot{\mathbf{p}}_t &= \mathbf{v}_t \\
\dot{\mathbf{v}}_t &= \mathbf{a}_t \\
\dot{\mathbf{q}}_t &= \frac{1}{2} \mathbf{q}_t \otimes \boldsymbol{\omega}_t \\
\dot{\mathbf{a}}_{bt} &= \mathbf{a}_w \\
\dot{\boldsymbol{\omega}}_{bt} &= \boldsymbol{\omega}_w \\
\dot{\mathbf{g}}_t &= 0
\end{align*}$$

在上式中，真實的加速度 $\mathbf{a}_t$ 和角速度 $\boldsymbol{\omega}_t$ 是從具有雜訊傳感器讀數的 IMU 中獲得的，這些讀數是在體座標系的加速度 $\mathbf{a}_m$ 和角速度 $\boldsymbol{\omega}_m$：
$$
\mathbf{a}_m = \mathbf{R}_t^\top (\mathbf{a}_t - \mathbf{g}_t) + \mathbf{a}_{bt} + \mathbf{a}_n
\]
\[
\boldsymbol{\omega}_m = \boldsymbol{\omega}_t + \boldsymbol{\omega}_{bt} + \boldsymbol{\omega}_n$$

其中 $\mathbf{R}_t$ $\triangleq \mathbf{R}(\mathbf{q}_t)$。透過這個，可以分離出真實值（這意味著我們對測量方程進行了轉換），
$$\mathbf{a}_t = \mathbf{R}_t (\mathbf{a}_m - \mathbf{a}_{bt} - \mathbf{a}_n) + \mathbf{g}_t
\]
\[
\boldsymbol{\omega}_t = \boldsymbol{\omega}_m - \boldsymbol{\omega}_{bt} - \boldsymbol{\omega}_n$$

將上述方程代入得到運動學系統：
$$
\dot{\mathbf{p}}_t = \mathbf{v}_t \quad
\]
\[
\dot{\mathbf{v}}_t = \mathbf{R}_t (\mathbf{a}_m - \mathbf{a}_{bt} - \mathbf{a}_n) + \mathbf{g}_t \quad
\]
\[
\dot{\mathbf{q}}_t = \frac{1}{2} \mathbf{q}_t \otimes (\boldsymbol{\omega}_m - \boldsymbol{\omega}_{bt} - \boldsymbol{\omega}_n) \quad
\]
\[
\dot{\mathbf{a}}_{bt} = \mathbf{a}_w \quad
\]
\[
\dot{\boldsymbol{\omega}}_{bt} = \boldsymbol{\omega}_w \quad
\]
\[
\dot{\mathbf{g}}_t = 0 \quad $$
如此一來，我們便將等式右邊的 true state 換成 nominal state。我們可以將其命名為 $$\dot{\mathbf{x}}_t = \mathbf{f}_t(\mathbf{x}_t, \mathbf{u}, \mathbf{w})$$這個系統的狀態為 $\mathbf{x}_t$，由 IMU 讀數 $\mathbf{u}_m$ 控制，並受白高斯雜訊$\mathbf{w}$ 擾動，所有變量的定義如下：

$$
\mathbf{x}_t = \begin{bmatrix}
\mathbf{p}_t \\
\mathbf{v}_t \\
\mathbf{q}_t \\
\mathbf{a}_{bt} \\
\boldsymbol{\omega}_{bt} \\
\mathbf{g}_t
\end{bmatrix}, \quad
\mathbf{u} = \begin{bmatrix}
\mathbf{a}_m - \mathbf{a}_n \\
\boldsymbol{\omega}_m - \boldsymbol{\omega}_n
\end{bmatrix}, \quad
\mathbf{w} = \begin{bmatrix}
\mathbf{a}_w \\
\boldsymbol{\omega}_w
\end{bmatrix}.
$$

## Error-state Kalman Filter (ESKF) Iteration Steps (迭代過程)

以下是誤差狀態卡爾曼濾波器 (ESKF) 迭代過程的詳細步驟：

### 初始化

- **狀態初始化：**
- 定義初始名義狀態 $\mathbf{x}_0$。
- 初始化誤差狀態 $\delta \mathbf{x}_0$（通常為零）。
- 定義初始協方差矩陣 $\mathbf{P}_0$

- **過程和測量噪聲協方差：**
- 定義過程噪聲協方差矩陣 $\mathbf{Q}$
- 定義測量噪聲協方差矩陣 $\mathbf{R}$

### 時間更新（預測）

- **預測名義狀態（不包括測量噪聲）：**
$$
\mathbf{x}_{k|k-1} = f(\mathbf{x}_{k-1})
$$
將動態模型$f$展開後的離散形式定義如下：
$$\begin{align*}
\mathbf{p} & \gets \mathbf{p} + \mathbf{v} \Delta t + \frac{1}{2} (\mathbf{R} (\mathbf{a}_m - \mathbf{a}_b) + \mathbf{g}) \Delta t^2 \\
\mathbf{v} & \gets \mathbf{v} + (\mathbf{R} (\mathbf{a}_m - \mathbf{a}_b) + \mathbf{g}) \Delta t \\
\mathbf{q} & \gets \mathbf{q} \otimes \mathbf{q} \{ (\boldsymbol{\omega}_m - \boldsymbol{\omega}_b) \Delta t \} \\
\mathbf{a}_b & \gets \mathbf{a}_b \\
\boldsymbol{\omega}_b & \gets \boldsymbol{\omega}_b \\
\mathbf{g} & \gets \mathbf{g}
\end{align*}
$$
- **預測誤差狀態協方差：**
誤差狀態動態的離散形式如下：
$$\begin{align*}
\delta \mathbf{p} & \gets \delta \mathbf{p} + \delta \mathbf{v} \Delta t \\
\delta \mathbf{v} & \gets \delta \mathbf{v} + (-\mathbf{R} [\mathbf{a}_m - \mathbf{a}_b]_\times \delta \boldsymbol{\theta} - \mathbf{R} \delta \mathbf{a}_b + \delta \mathbf{g}) \Delta t + \mathbf{v}_i \\
\delta \boldsymbol{\theta} & \gets \mathbf{R}^\top \{ (\boldsymbol{\omega}_m - \boldsymbol{\omega}_b) \Delta t \} \delta \boldsymbol{\theta} - \delta \boldsymbol{\omega}_b \Delta t + \boldsymbol{\theta}_i \\
\delta \mathbf{a}_b & \gets \delta \mathbf{a}_b + \mathbf{a}_i \\
\delta \boldsymbol{\omega}_b & \gets \delta \boldsymbol{\omega}_b + \boldsymbol{\omega}_i \\
\delta \mathbf{g} & \gets \delta \mathbf{g}
\end{align*}$$

其中，$\mathbf{v}_i$、$\boldsymbol{\theta}_i$、$\mathbf{a}_i$ 和 $\boldsymbol{\omega}_i$ 是應用於速度、方向和偏差估計的隨機脈衝，由白高斯過程建模。它們的均值為零，協方差矩陣通過對$\mathbf{a}_n$、$\boldsymbol{\omega}_n$、$\mathbf{a}_w$ 和 $\boldsymbol{\omega}_w$ 的協方差在步長時間$\Delta t$ 上進行積分得到。

$$
\mathbf{V}_i = \sigma_{\mathbf{a}_n}^2 \Delta t^2 \mathbf{I} \quad [m^2/s^2]
\]
\[
\boldsymbol{\Theta}_i = \sigma_{\boldsymbol{\omega}_n}^2 \Delta t \mathbf{I} \quad [\text{rad}^2]
\]
\[
\mathbf{A}_i = \sigma_{\mathbf{a}_w}^2 \Delta t \mathbf{I} \quad [m^2/s^4]
\]
\[
\boldsymbol{\Omega}_i = \sigma_{\boldsymbol{\omega}_w}^2 \Delta t \mathbf{I} \quad [\text{rad}^2/s^2]
$$

其中，$\sigma_{\mathbf{a}_n} [m/s^2] \)、\( \sigma_{\boldsymbol{\omega}_n} [\text{rad}/s] \)、\( \sigma_{\mathbf{a}_w} [m/s^2\sqrt{s}] \) 和 \( \sigma_{\boldsymbol{\omega}_w} [\text{rad}/s\sqrt{s}]$ 需從 IMU 資料表中獲得或通過實驗測量得到。

### 誤差狀Jacobian和擾動矩陣

雅可比(Jacobian)矩陣通過簡單觀察上一節中的誤差狀態差分方程得到。

為了將這些方程簡化，我們考慮名義狀態向量 $\mathbf{x} \)、誤差狀態向量 \( \delta \mathbf{x} \)、輸入向量 \( \mathbf{u}_m \) 和擾動脈衝向量 \( \mathbf{i}$，如下所示：

$$\mathbf{x} = \begin{bmatrix}
\mathbf{p} \\
\mathbf{v} \\
\mathbf{q} \\
\mathbf{a}_b \\
\boldsymbol{\omega}_b \\
\mathbf{g}
\end{bmatrix}, \quad
\delta \mathbf{x} = \begin{bmatrix}
\delta \mathbf{p} \\
\delta \mathbf{v} \\
\delta \boldsymbol{\theta} \\
\delta \mathbf{a}_b \\
\delta \boldsymbol{\omega}_b \\
\delta \mathbf{g}
\end{bmatrix}, \quad
\mathbf{u}_m = \begin{bmatrix}
\mathbf{a}_m \\
\boldsymbol{\omega}_m
\end{bmatrix}, \quad
\mathbf{i} = \begin{bmatrix}
\mathbf{v}_i \\
\boldsymbol{\theta}_i \\
\mathbf{a}_i \\
\boldsymbol{\omega}_i
\end{bmatrix}$$

線性化的誤差狀態系統表示為：

$$\delta \mathbf{x} \leftarrow f( \mathbf{x}, \delta \mathbf{x}, \mathbf{u}_m, \mathbf{i} ) = \mathbf{F}_{\mathbf{x}} ( \mathbf{x}, \mathbf{u}_m ) \cdot \delta \mathbf{x} + \mathbf{F}_{\mathbf{i}} \cdot \mathbf{i}$$

ESKF 預測方程表示為：

$$\mathbf{P} \leftarrow \mathbf{F}_{\mathbf{x}} \mathbf{P} \mathbf{F}_{\mathbf{x}}^\top + \mathbf{F}_{\mathbf{i}} \mathbf{Q}_{\mathbf{i}} \mathbf{F}_{\mathbf{i}}^\top
$$

其中，$\delta \mathbf{x} \sim \mathcal{N}\{\delta \hat{\mathbf{x}}, \mathbf{P}\}\)，\(\mathbf{F}_{\mathbf{x}}\) 和 \(\mathbf{F}_{\mathbf{i}}\) 是函數 \(f()\) 對誤差和擾動向量的雅可比矩陣；\(\mathbf{Q}_{\mathbf{i}}$ 是擾動脈衝的協方差矩陣。

上述雅可比矩陣$\mathbf{F}_{\mathbf{x}}$、$\mathbf{F}_{\mathbf{i}}$和協方差矩陣$\mathbf{Q}_{\mathbf{i}}$的表達式如下。所有與狀態相關的值均直接從名義狀態中提取。

$$\mathbf{F}_{\mathbf{x}} = \frac{\partial f}{\partial \delta \mathbf{x}} \bigg|_{\mathbf{x}, \mathbf{u}_m} =
\begin{bmatrix}
\mathbf{I} & \mathbf{I} \Delta t & 0 & 0 & 0 & 0 \\
0 & \mathbf{I} & -\mathbf{R}[\mathbf{a}_m - \mathbf{a}_b]_\times \Delta t & -\mathbf{R} \Delta t & 0 & \mathbf{I} \Delta t \\
0 & 0 & \mathbf{R}^\top \{(\boldsymbol{\omega}_m - \boldsymbol{\omega}_b) \Delta t\} & 0 & -\mathbf{I} \Delta t & 0 \\
0 & 0 & 0 & \mathbf{I} & 0 & 0 \\
0 & 0 & 0 & 0 & \mathbf{I} & 0 \\
0 & 0 & 0 & 0 & 0 & \mathbf{I}
\end{bmatrix} \quad
\]
\[
\mathbf{F}_{\mathbf{i}} = \frac{\partial f}{\partial \mathbf{i}} \bigg|_{\mathbf{x}, \mathbf{u}_m} =
\begin{bmatrix}
0 & 0 & 0 & 0 \\
\mathbf{I} & 0 & 0 & 0 \\
0 & \mathbf{I} & 0 & 0 \\
0 & 0 & \mathbf{I} & 0 \\
0 & 0 & 0 & \mathbf{I} \\
0 & 0 & 0 & 0
\end{bmatrix}, \quad
\mathbf{Q}_{\mathbf{i}} =
\begin{bmatrix}
\mathbf{V}_i & 0 & 0 & 0 \\
0 & \boldsymbol{\Theta}_i & 0 & 0 \\
0 & 0 & \mathbf{A}_i & 0 \\
0 & 0 & 0 & \boldsymbol{\Omega}_i
\end{bmatrix} \quad
$$

### 演算法實作概要流程

1. **計算量測誤差:**

- 獲取測量值 $\mathbf{z}_k$
- 根據名義狀態預測測量值
$$
\mathbf{z}_{k|k-1} = h(\mathbf{x}_{k|k-1})
$$
- 計算測量殘差
$$
\mathbf{y}_k = \mathbf{z}_k - \mathbf{z}_{k|k-1}$$
2. **計算卡爾曼增益:**
- 在名義狀態周圍線性化測量模型以獲得雅可比矩陣$\mathbf{H}_k$
$$
\mathbf{H}_k = \left. \frac{\partial h(\mathbf{x})}{\partial \delta \mathbf{x}_k} \right|_{\mathbf{x}=\mathbf{x}_{k|k-1}}
$$
- 計算協方差
$$
\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top + \mathbf{R}
$$
- 計算卡爾曼增益
$$
\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^\top \mathbf{S}_k^{-1}
$$

3. **更新名義狀態(nominal state):**
- 使用卡爾曼增益和創新量更新誤差狀態
$$
\delta \mathbf{x}_k = \mathbf{K}_k \mathbf{y}_k
$$
- 校正名義狀態
$$
\mathbf{x}_k = \mathbf{x}_{k|k-1} \oplus \delta \mathbf{x}_k
$$
其中，$\oplus$ 表示將名義狀態和誤差狀態組合的適當操作，對於線性系統可能是加法，對於非線性系統則可能是更複雜的操作。

4. **更新誤差狀態協方差:**
- 更新誤差狀態協方差矩陣
$$
\mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}
$$
- 在使用誤差狀態更新名義狀態後，將誤差狀態設為零
$$
\delta \mathbf{x}_k \leftarrow \mathbf{0}
$$

5. **重複:**
- 對每個時間步 $k$ 重複時間更新和測量更新步驟。