Fast Kalman Filter#

Implementation of the Fast Kalman Filter algorithm for orientation estimation described in “Novel MARG-Sensor Orientation Algorithm Using Fast Kalman Filter” [GWWQ17].

The Fast Kalman Filter (FKF) is a quaternion-based orientation estimation algorithm that focuses on the fusion of accelerometer and magnetometer measurements.

In the FKF, the quaternion kinematic equation is the process model, similar to the EKF. Likewise, the measurement model is based on the accelerometer and magnetometer readings.

The FKF algorithm is computationally efficient, because it symbolically solves the linear system of equations of the observation model.

The steps of the KF are summarized as:

  1. State Prediction

\[\mathbf{q}_t^- = \Phi_t \mathbf{q}_{t-1} + \xi_t\]

  1. Covariance Prediction

\[\mathbf{\Sigma}_{\mathbf{q}_t^-} = \Phi_t \mathbf{\Sigma}_{\mathbf{q}_{t-1}} \Phi_t^T + \mathbf{\Sigma}_{\xi_t}\]

  1. Kalman Gain

\[\mathbf{G}_t = \mathbf{\Sigma}_{\mathbf{q}_t^-} \big [\mathbf{\Sigma}_{\mathbf{q}_t^-} + \mathbf{\Sigma}_{v_t}\big ]^{-1}\]

  1. State Update

\[\mathbf{q}_t = \mathbf{q}_t + \mathbf{G}_t (\mathbf{y}_t - \mathbf{q}_t)\]

  1. Covariance Update

\[\mathbf{\Sigma}_{\mathbf{q}_t} = (\mathbf{I}_4 - \mathbf{G}_t) \mathbf{\Sigma}_{\mathbf{q}_t^-}\]

We update the state, \(\mathbf{q}_t\), at every time \(t\), with the predicted state \(\mathbf{q}_t^-\), the measurements \(\mathbf{y}_t\), the state transition matrix \(\Phi_t\), the covariance matrix \(\mathbf{P}_t\), the process noise covariance matrix \(\boldsymbol\varepsilon_t\), and the Kalman Gain \(\mathbf{G}_t\).

Prediction Model#

The first two steps correspond to the prediction model, and are computed like in the common Extended Kalman Filter.

The state vector is defined by the quaternion itself, without considering biases over the sensors.

\[\mathbf{q}_t = \begin{bmatrix} q_w & q_x & q_y & q_z \end{bmatrix}^T\]

The state prediction (we can call it the “predicted quaternion”) is estimated as:

\[\boxed{\mathbf{q}_t^- = \Phi_t \mathbf{q}_{t-1} + \xi_t}\]

where \(\xi_t\) is the process noise, and the state transition matrix \(\Phi_t\) is:

\[\Phi_t = \mathbf{I}_4 + \frac{\Delta t}{2} \boldsymbol\Omega(\mathbf{\omega}_t)\]

with the omega operator is defined as:

\[\begin{split}\boldsymbol\Omega(\mathbf{\omega}) = \begin{bmatrix} 0 & -\mathbf{\omega}^T \\ \mathbf{\omega} & \lfloor\mathbf{\omega}\rfloor_\times \end{bmatrix} = \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix}\end{split}\]

We use the measured angular rates \(\mathbf{\omega}_t\) from the gyroscope to approximate the instantaneous change in the quaternion. Then, we multiply by \(\frac{\Delta t}{2}\) to numerically integrate it, and get the predicted quaternion \(\mathbf{q}_t^-\).

At step 2, the Covariance Prediction is computed with:

\[\boxed{\mathbf{\Sigma}_{\mathbf{q}_t^-} = \Phi_t \mathbf{\Sigma}_{\mathbf{q}_{t-1}} \Phi_t^T + \mathbf{\Sigma}_{\xi_t}}\]

where \(\mathbf{\Sigma}_{\xi_t}\) is the process noise covariance

\[\mathbf{\Sigma}_{\xi_t} = \Big(\frac{\Delta t}{2}\Big)^2 \mathbf{\Xi}_t \mathbf{\Sigma}_{\mathrm{gyro}} \mathbf{\Xi}_t^T\]

\(\mathbf{\Sigma}_{\mathrm{gyro}}\) is the angular rates’ covariance, usually defined as:

\[\begin{split}\mathbf{\Sigma}_{\mathrm{gyro}} = \begin{bmatrix} \sigma_{\omega_x}^2 & 0 & 0 \\ 0 & \sigma_{\omega_y}^2 & 0 \\ 0 & 0 & \sigma_{\omega_z}^2 \end{bmatrix}\end{split}\]

The terms \(\sigma_{\omega_x}\), \(\sigma_{\omega_y}\), and \(\sigma_{\omega_z}\) are the standard deviations at each axis of the gyroscope.

In this implementation, we assume they are equal, and we define \(\mathbf{\Sigma}_{\mathrm{gyro}}\) as:

\[\begin{split}\mathbf{\Sigma}_{\mathrm{gyro}} = \sigma_{\omega}^2 \mathbf{I}_3 = \begin{bmatrix} \sigma_{\omega}^2 & 0 & 0 \\ 0 & \sigma_{\omega}^2 & 0 \\ 0 & 0 & \sigma_{\omega}^2 \end{bmatrix}\end{split}\]

\(\mathbf{\Xi}_t\) is the matrix built from the quaternion’s elements:

\[\begin{split}\mathbf{\Xi}_t = \begin{bmatrix} q_x & q_y & q_z \\ -q_w & -q_z & q_y \\ q_z & -q_w & -q_x \\ -q_y & q_x & -q_w \end{bmatrix}\end{split}\]

Correction Model#

The last three steps are the correction model, where we update the predicted state \(\mathbf{q}_t^-\) with the measurements \(\mathbf{q}_{am}\).

The Kalman Gain is computed in step three as:

\[\boxed{\mathbf{G}_t = \mathbf{\Sigma}_{\mathbf{q}_t^-} \big [\mathbf{\Sigma}_{\mathbf{q}_t^-} + \mathbf{\Sigma}_{v_t}\big ]^{-1}}\]

where \(\mathbf{\Sigma}_{v_t}\) is the measurement quaternion’s variance approximated with:

\[\mathbf{\Sigma}_{v_t} = \mathbf{J} \mathbf{\Sigma}_{\mathrm{am}} \mathbf{J}^T\]

Defining \(\mathbf{\Sigma}_a\) and \(\mathbf{\Sigma}_m\) as the covariances of accelerometer and magnetomer, respectively, we define the covariance matrix \(\mathbf{\Sigma}_{\mathrm{am}}\) as:

\[\begin{split}\mathbf{\Sigma}_{\mathrm{am}} = \begin{bmatrix} \mathbf{\Sigma}_{\mathbf{a}} & \mathbf{0} \\ \mathbf{0} & \mathbf{\Sigma}_{\mathbf{m}} \end{bmatrix} = \begin{bmatrix} \sigma_{a_x}^2 & 0 & 0 & 0 & 0 & 0 \\ 0 & \sigma_{a_y}^2 & 0 & 0 & 0 & 0 \\ 0 & 0 & \sigma_{a_z}^2 & 0 & 0 & 0 \\ 0 & 0 & 0 & \sigma_{m_x}^2 & 0 & 0 \\ 0 & 0 & 0 & 0 & \sigma_{m_y}^2 & 0 \\ 0 & 0 & 0 & 0 & 0 & \sigma_{m_z}^2 \end{bmatrix}\end{split}\]

\(\mathbf{J}\) is the Jacobian matrix of the measurement model.

\[\begin{split}\begin{array}{rcl} \mathbf{J} &=& \frac{\partial \mathbf{q}_{am}}{\partial \Big\{ \big(\mathbf{A}^b\big)^T \, \big(\mathbf{M}^b\big)^T\Big\} } \\ &=& \begin{bmatrix} \frac{\partial q_w}{\partial a_x} & \frac{\partial q_w}{\partial a_y} & \frac{\partial q_w}{\partial a_z} & \frac{\partial q_w}{\partial m_x} & \frac{\partial q_w}{\partial m_y} & \frac{\partial q_w}{\partial m_z} \\ \frac{\partial q_x}{\partial a_x} & \frac{\partial q_x}{\partial a_y} & \frac{\partial q_x}{\partial a_z} & \frac{\partial q_x}{\partial m_x} & \frac{\partial q_x}{\partial m_y} & \frac{\partial q_x}{\partial m_z} \\ \frac{\partial q_y}{\partial a_x} & \frac{\partial q_y}{\partial a_y} & \frac{\partial q_y}{\partial a_z} & \frac{\partial q_y}{\partial m_x} & \frac{\partial q_y}{\partial m_y} & \frac{\partial q_y}{\partial m_z} \\ \frac{\partial q_z}{\partial a_x} & \frac{\partial q_z}{\partial a_y} & \frac{\partial q_z}{\partial a_z} & \frac{\partial q_z}{\partial m_x} & \frac{\partial q_z}{\partial m_y} & \frac{\partial q_z}{\partial m_z} \end{bmatrix} \end{array}\end{split}\]

and the measurement quaternion \(\mathbf{q}_{am}\) is computed from the accelerometer and magnetometer readings in the body frame.

\[\mathbf{q}_{am} = \frac{1}{4} \Big(\mathbf{W}_a + \mathbf{I}_4\Big) \Big(\mathbf{W}_m + \mathbf{I}_4\Big) \mathbf{q}_{t-1}\]

with:

\[\begin{split}\mathbf{W}_a = \begin{bmatrix} a_z & a_y & -a_x & 0 \\ a_y & -a_z & 0 & a_x \\ -a_x & 0 & -a_z & a_y \\ 0 & a_x & a_y & a_z \end{bmatrix}\end{split}\]

and:

\[\begin{split}\mathbf{W}_m = \begin{bmatrix} m_z & m_y & -m_x & 0 \\ m_y & -m_z & 0 & m_x \\ -m_x & 0 & -m_z & m_y \\ 0 & m_x & m_y & m_z \end{bmatrix}\end{split}\]

What makes the FKF computationally efficient is that the Jacobian matrix \(\mathbf{J}\), and the measurement quaternion \(\mathbf{q}_{am}\) are reduced symbolically, instead of numerically calculating them.

So, instead of performing several matrix multiplications and vector operations, the FKF algorithm solves the linear system of equations symbolically, and updates the state vector directly.

Then, we correct our predicted state \(\mathbf{q}_t^-\) with the measurements \(\mathbf{q}_{am}\) using the Kalman Gain \(\mathbf{G}_t\):

\[\boxed{\mathbf{q}_t = \mathbf{q}_t^- + \mathbf{G}_t (\mathbf{q}_{am} - \mathbf{q}_t^-)}\]

Finally, we update the covariance matrix:

\[\boxed{\mathbf{\Sigma}_{\mathbf{q}_t} = (\mathbf{I}_4 - \mathbf{G}_t) \mathbf{\Sigma}_{\mathbf{q}_t^-}}\]
class ahrs.filters.fkf.FKF(gyr: ndarray | None = None, acc: ndarray | None = None, mag: ndarray | None = None, **kwargs)#

Class of Fast Kalman Filter algorithm

Parameters:

  • gyr (numpy.ndarray, default: None) – N-by-3 array with measurements of angular velocity in rad/s

  • acc (numpy.ndarray, default: None) – N-by-3 array with measurements of acceleration in in m/s^2

  • mag (numpy.ndarray, default: None) – N-by-3 array with measurements of magnetic field in nT

  • frequency (float, default: 100.0) – Sampling frequency in Hz.

  • Dt (float, default: 0.01) – Sampling period in seconds. If frequency is given, Dt is computed as 1/frequency.

  • sigma_g (float, default: 0.01) – Standard deviation of the gyroscope noise.

  • sigma_a (float, default: 0.01) – Standard deviation of the accelerometer noise.

  • sigma_m (float, default: 0.01) – Standard deviation of the magnetometer noise.

  • Pk (float, default: 0.01) – Initial value of the diagonal values of covariance matrix.

Omega4(x: ndarray) → ndarray#

Omega operator.

Given a vector \(\mathbf{x}\in\mathbb{R}^3\), return a \(4\times 4\) matrix of the form:

\[\begin{split}\boldsymbol\Omega(\mathbf{x}) = \begin{bmatrix} 0 & -\mathbf{x}^T \\ \mathbf{x} & \lfloor\mathbf{x}\rfloor_\times \end{bmatrix} = \begin{bmatrix} 0 & -x_1 & -x_2 & -x_3 \\ x_1 & 0 & x_3 & -x_2 \\ x_2 & -x_3 & 0 & x_1 \\ x_3 & x_2 & -x_1 & 0 \end{bmatrix}\end{split}\]
Parameters:

x (numpy.ndarray) – Three-dimensional vector.

Returns:

Omega – Omega matrix.

Return type:

numpy.ndarray

kalman_update(q_1: ndarray, q_am: ndarray, Pk_1: ndarray, Phi: ndarray, Sigma_eps: ndarray, Sigma_v: ndarray) → Tuple[ndarray, ndarray]#

Kalman update step.

Parameters:

  • q_1 (numpy.ndarray) – Previous state vector.

  • q_am (numpy.ndarray) – Measurement vector.

  • Pk_1 (numpy.ndarray) – Previous covariance matrix.

  • Phi (numpy.ndarray) – State transition matrix.

  • Sigma_eps (numpy.ndarray) – Process noise covariance matrix.

  • Sigma_v (numpy.ndarray) – Measurement noise covariance matrix.

Returns:

  • xk (numpy.ndarray) – Updated state vector.

  • Pk (numpy.ndarray) – Updated covariance matrix.

measurement_quaternion_acc_mag(q: ndarray, acc: ndarray, mag: ndarray) → Tuple[ndarray, ndarray]#

Measurement model of Accelerometer and Magnetometer.

Parameters:

  • q (numpy.ndarray) – Predicted quaternion.

  • acc (numpy.ndarray) – Sample of tri-axial Accelerometer.

  • mag (numpy.ndarray) – Sample of tri-axial Magnetometer.

Returns:

q – Measurement quaternion.

Return type:

numpy.ndarray