# Deriving equations for IMU Kalman Filter

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$$

• Hi Jacob, I'm not sure why you cross-posted, since this question still suffers from the same problem as the original one. As I previously wrote, the term $\dot X_j T + I_6$ doesn't make sense, since you are adding a column vector with a matrix. Until this is addressed, this question can't be answered. Please check the formulation in the papers you mentioned, and ensure that they are correct. Apr 3, 2023 at 13:23
• I have written the formula as it appears in the paper. If you are not going to help someone, least you could do is not close the question. I don't have a maths background but I thought someone could help. If youre not that person then its better if you just move on with your day instead of making it harder for me to get an answer. Apr 3, 2023 at 15:07
• @mhdadk Besides this equation can be solved if its rewritten as I put it on the question itself. Apr 3, 2023 at 15:08
• @mhdadk Pretty unfriendly attitude towards a new poster on this forum as well. Apr 3, 2023 at 15:13
• The equation is incorrect. I've clarified this in my answer. Just because your post was closed on one SE site, doesn't mean that it should stay open on another. In the future, please try to perform debugging yourself. Apr 3, 2023 at 16:04

## 1 Answer

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.

• Thank you for the reply but I still find it bewildering that you expected me to recognise a mistake in a published paper with multiple co-authors while not being an expert myself. Apr 3, 2023 at 16:13
• @JacobSánchez To recognize these mistakes in the future, try to take courses on Linear Algebra (helps you to recognize dimensions), Linear Systems Theory (covers linearization), and Optimal Filtering (covers Kalman filtering). Apr 3, 2023 at 16:18