Deriving equations for IMU Kalman Filter

Question

I am trying to model a Kalman Filter for an IMU (inertial measurement unit) with the method described by Zhou (2004) and Filippeschi (2017, pp.11-12).

In this method, the state vector is:

$$ X = \begin{bmatrix}g_x\\g_y\\g_z\\m_x\\m_y\\m_z\end{bmatrix} $$

Where $g$ is the gravity vector and $m$ is the magnetic vector.

The process model is as follows:

$$ X_{j+1}=(\dot X_jT+I_6)X_{j}+w_j $$

Where $\dot X_j$ is a vector containing the rate of change of $g$ and $m$, $T$ is the sample time, $I_6$ being the 6x6 identity matrix, and $w_j$ measurement noise.

$\dot X_j$ can be obtained from the raw measurements of the sensors.

$$ \dot X = \begin{bmatrix}\dot g\\ \dot m\end{bmatrix} $$

$$ \dot g = g x w $$

$$ \dot m = m x w $$

Where g is the accelerometer output, m is the magnetometer output, and w is the gyroscope output.

The measurement model is simple:

$$ Z_j = X_j + \delta_j $$

With $\delta_j$ being the white measurement noise.

I am not very familiar with kalman filters, but from what I have gathered, there's five main equations:

1. State extrapolation
2. Covariance extrapolation
3. State update
4. Covariance update
5. Kalman gain

My issue comes from the fact that most literature has the state extrapolation equation as follows (I'm ommiting the control vector):

$$ X_{j+1} = AX_j + w_j $$

However, I am confused about deriving the state extrapolation and covariance extrapolation equations, since the process model doesn't really have a state transition matrix). Can I simply substitute $A$ with $(\dot X_jT+I_6)$?

For instance, this would mean the state extrapolation eq ends up like this:

$$ X_{j+1} = \dot X_jTX_j + X_j $$

And perhaps the covariance extrapolation like this (which I have no idea how to solve):

$$ P_{j+1} = (\dot X_jT+I_6)P_j(\dot X_jT+I_6)^T + Q$$$$ $$

Answer 1

The authors in Filippeschi et al. (2017) made a mistake in eq. (5) of their paper: $$X_{j+1}=(\dot X_jT+I_6)X_{j}+w_j$$ In Zhou et al. (2004), the process model is (eq. (9) in Zhou's paper) $$ \begin{bmatrix}\dot{\mathbf{g}} \\ \dot{\mathbf{H}}\end{bmatrix} = \mathbf A \begin{bmatrix}\mathbf{g} \\ \mathbf{H}\end{bmatrix} \tag{1} \label{1} $$ where $\mathbf A$ is the $6 \times 6$ matrix $$ \begin{bmatrix}\mathbf S & \mathbf 0 \\ \mathbf 0 & \mathbf S\end{bmatrix} $$ where $\mathbf S$ is a $3 \times 3$ matrix given in eq. (9) in Zhou's paper. What the authors in Filippeschi et al. (2017) did is to approximate \eqref{1} linearly and then add process noise $\mathbf w_k$, which was not originally added in Zhou's paper. However, their process for linearizing \eqref{1} is incorrect. Here is what they should have done: \begin{align} \begin{bmatrix}\dot{\mathbf{g}} \\ \dot{\mathbf{H}}\end{bmatrix} &= \mathbf A \begin{bmatrix}\mathbf{g} \\ \mathbf{H}\end{bmatrix} \\ \frac{\begin{bmatrix}\mathbf{g}_{k+1} \\ \mathbf{H}_{k+1}\end{bmatrix} - \begin{bmatrix}\mathbf{g}_{k} \\ \mathbf{H}_{k}\end{bmatrix}}{T} &\approx \mathbf A \begin{bmatrix}\mathbf{g}_{k} \\ \mathbf{H}_{k}\end{bmatrix} \\ \begin{bmatrix}\mathbf{g}_{k+1} \\ \mathbf{H}_{k+1}\end{bmatrix} - \begin{bmatrix}\mathbf{g}_{k} \\ \mathbf{H}_{k}\end{bmatrix} &\approx \mathbf A T \begin{bmatrix}\mathbf{g}_{k} \\ \mathbf{H}_{k}\end{bmatrix} \\ \begin{bmatrix}\mathbf{g}_{k+1} \\ \mathbf{H}_{k+1}\end{bmatrix} &\approx \mathbf A T \begin{bmatrix}\mathbf{g}_{k} \\ \mathbf{H}_{k}\end{bmatrix} + \begin{bmatrix}\mathbf{g}_{k} \\ \mathbf{H}_{k}\end{bmatrix} \\ \begin{bmatrix}\mathbf{g}_{k+1} \\ \mathbf{H}_{k+1}\end{bmatrix} &\approx (\mathbf A T + I_6)\begin{bmatrix}\mathbf{g}_{k} \\ \mathbf{H}_{k}\end{bmatrix} \end{align} And then from here, Filippeschi et al. (2017) add their own process noise: $$\begin{bmatrix}\mathbf{g}_{k+1} \\ \mathbf{H}_{k+1}\end{bmatrix} \approx (\mathbf A T + I_6)\begin{bmatrix}\mathbf{g}_{k} \\ \mathbf{H}_{k}\end{bmatrix} + \mathbf w_k$$ To verify that this derivation is correct, have a look at eq. (12) in Filippeschi et al. (2017), where they perform a similar linearization, but correctly. I will leave the rest up to you.