# Error-state Kalman Filter
**中文版:** https://hackmd.io/OByeQok7TjymyJ3MolKiFw
**Author:** Teng-Hu Cheng
**email:** tenghu@nycu.edu.tw
**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
Assuming we want to integrate IMU and GPS while considering the signal noise and drift of the IMU, we must use an "error state filter" for more precise filtering. The overall "error state filter" can be represented by the following block diagram.
## The error-state Kalman filter explained
In error-state filter formulations, we speak of true-, nominal- and error-state values, the true-state being expressed as a suitable composition (linear sum, quaternion product or matrix product) of the nominal- and the error- states.
The idea is to consider the nominal- state as large-signal (integrable in non-linear fashion) and the error-state as small signal (thus linearly integrable and suitable for linear-Gaussian filtering).
The error-state filter can be explained as follows. On one side, high-frequency IMU data $\mathbf{u}_m$ is integrated into a nominal-state $\mathbf{x}$. This nominal state does not take into account the noise terms $\mathbf{w}$ and other possible model imperfections. As a consequence, it will accumulate errors. These errors are collected in the error-state $\delta \mathbf{x}$ and estimated with the Error-State Kalman Filter (ESKF), this time incorporating all the noise and perturbations. The error-state consists of small-signal magnitudes, and its evolution function is correctly defined by a (time-variant) linear dynamic system, with its dynamic, control and measurement matrices computed from the values of the nominal-state. In parallel with integration of the nominal-state, the ESKF predicts a Gaussian estimate of the error-state. It only predicts, because by now no other measurement is available to correct these estimates. The filter correction is performed at the arrival of information other than IMU ($\textit{e.g.}$, GPS, vision, etc.), which is able to render the errors observable and which happens generally at a much lower rate than the integration phase. This correction provides a posterior Gaussian estimate of the error-state. After this, the error-state’s mean is injected into the nominal-state, then reset to zero. The error-state’s covariances matrix is conveniently updated to reflect this reset. The system goes on like this forever.
### Definition of Variables
Let's define the notations for the subsequent analysis:
The error state signal $\delta \mathbf{q}$ and $\delta \mathbf{R}$ can be approximated as：
$$
\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 is the ideal state since it does not contain noise、bias，but in reality, this cannot be achieved, so we settle for the next best option, which is to use the 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*}$$
In the above equation, the true acceleration $\mathbf{a}_t$ and angular velocity $\boldsymbol{\omega}_t$ are obtained from the IMU with noisy sensor readings, which are the acceleration $\mathbf{a}_m$ and angular velocity $\boldsymbol{\omega}_m$ in the body frame coordinate system:
$$
\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$$
where $\mathbf{R}_t \triangleq \mathbf{R}(\mathbf{q}_t)$. Through this, the true values can be separated (which means we have transformed the measurement equation),
$$\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$$
Substituting the above equations, we obtain the kinematic system:
$$
\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 $$
In this way, we replace the true state on the right side of the equation with the nominal state. We can denote it as $$\dot{\mathbf{x}}_t = \mathbf{f}_t(\mathbf{x}_t, \mathbf{u}, \mathbf{w})$$
The state of this system is $\mathbf{x}_t$, controlled by the IMU readings $\mathbf{u}_m$, and perturbed by white Gaussian noise $\mathbf{w}$. The definitions of all variables are as follows:
$$
\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
The following are the detailed steps of the Error-State Kalman Filter (ESKF) iteration process:
### 初始化
- **State Initialization:**
- Define the initial nominal state $\mathbf{x}_0$.
- Initialize the error state $\delta \mathbf{x}_0$ (usually zero).
- Define the initial covariance matrix $\mathbf{P}_0$.
- **Process and Measurement Noise Covariance:**
- Define the process noise covariance matrix $\mathbf{Q}$.
- Define the measurement noise covariance matrix $\mathbf{R}$.
### 時間更新（預測）
- **Predict Nominal State (excluding measurement noise):**
$$
\mathbf{x}_{k|k-1} = f(\mathbf{x}_{k-1})
$$
The discrete form of the dynamic model $f$ after expansion is defined as follows：
$$\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*}
$$
- **Predict Error State Covariance**
The discrete form of the error state dynamics is as follows:
$$\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*}$$
where $\mathbf{v}_i$, $\boldsymbol{\theta}_i$, $\mathbf{a}_i$, and $\boldsymbol{\omega}_i$ are random impulses applied to the velocity, orientation, and bias estimates, modeled by a white Gaussian process. Their means are zero, and the covariance matrices are obtained by integrating the covariances of $\mathbf{a}_n$, $\boldsymbol{\omega}_n$, $\mathbf{a}_w$, and $\boldsymbol{\omega}_w$ over the timestep $\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],
$$
where $\sigma_{\mathbf{a}_n} \, [\text{m}/\text{s}^2]$, $\sigma_{\boldsymbol{\omega}_n} \, [\text{rad}/\text{s}]$, $\sigma_{\mathbf{a}_w} \, [\text{m}/\text{s}^2\sqrt{\text{s}}]$, and $\sigma_{\boldsymbol{\omega}_w} \, [\text{rad}/\text{s}\sqrt{\text{s}}]$ need to be obtained from the IMU datasheet or measured experimentally.
### Error-state Jacobian and perturbation matrix
The Jacobian matrix is obtained by simply observing the error state difference equations from the previous section.
To simplify these equations, we consider the nominal state vector $\mathbf{x}$, error state vector $\delta \mathbf{x}$, input vector $\mathbf{u}_m$, and disturbance impulse vector $\mathbf{i}$, as shown below:
Is there anything from this conversation you’d like me to remember for next time?
$$\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}$$
The linearized error state system is represented as:
$$\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}$$
The ESKF prediction equations are represented as:
$$\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
$$
where $\delta \mathbf{x} \sim \mathcal{N}\{\delta \hat{\mathbf{x}}, \mathbf{P}\}$, $\mathbf{F}_{\mathbf{x}}$ and $\mathbf{F}_{\mathbf{i}}$ are the Jacobian matrices of the function \(f()\) with respect to the error and disturbance vectors; $\mathbf{Q}_{\mathbf{i}}$ is the covariance matrix of the disturbance impulses.
The expressions for the Jacobian matrices $\mathbf{F}_{\mathbf{x}}$, $\mathbf{F}_{\mathbf{i}}$, and the covariance matrix $\mathbf{Q}_{\mathbf{i}}$ are as follows. All state-related values are directly extracted from the nominal state.
$$\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
$$
### The Implementation Procedure
1. **Prediction:**
- Obtain the measurement value $\mathbf{z}_k$
- Predict the measurement based on the nominal state
$$
\mathbf{z}_{k|k-1} = h(\mathbf{x}_{k|k-1})
$$
- Compute the measurement residual
$$
\mathbf{y}_k = \mathbf{z}_k - \mathbf{z}_{k|k-1}$$
2. **Kalman filter gain:**
- Linearize the measurement model around the nominal state to obtain the Jacobian matrix $\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}}
$$
- Calculate the covariance
$$
\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top + \mathbf{R}
$$
- compute the Kalman gain
$$
\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^\top \mathbf{S}_k^{-1}
$$
3. **Update the nominal state $\mathbf{x}_{k|k-1}$:**
- Update the error state using the Kalman gain and the innovation
$$
\delta \mathbf{x}_k = \mathbf{K}_k \mathbf{y}_k
$$
- Correct the nominal state
$$
\mathbf{x}_k = \mathbf{x}_{k|k-1} \oplus \delta \mathbf{x}_k
$$
where $\oplus$ denotes the appropriate operation to combine the nominal state and the error state. For linear systems, this may be addition, while for nonlinear systems, it could be a more complex operation.
4. **Update the error state covariance:**
- Update the error state covariance
$$
\mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}
$$
- After updating the nominal state using the error state, set the error state to zero
$$
\delta \mathbf{x}_k \leftarrow \mathbf{0}
$$
5. **Iterate:**
- Repeat the time update and measurement update steps for each time step $k$
Sign in with Wallet
Connect another wallet