Return to Question

I have a IMU and GPS sensors on my PCB which contains a microcontroller. An IMU consists of a gyroscope, accelerometer and a magnetometer (the magnetometer is calibrated to compensate for the effects of hard and soft iron). I am using Madgwick's filter to estimate the rotation matrix needed to rotate the read out accelerometer values onto the North-East-Down (NED) coordinate system. After subtracting the gravity vector, I feed the obtained acceleration components to the Kalman Filter. Note that the Madgwick filter estimates the gyroscope bias.

The states of my Kalman filter are:

$$ \vec x = \begin{bmatrix} p_x & p_y & p_z & v_x & v_y & v_z \end{bmatrix}^T \tag 1$$

and the control vector is:

$$ \vec u = \begin{bmatrix} a_x & a_y & a_z & a_x & a_y & a_z \end{bmatrix}^T \tag 2$$

I constructed the discrete time model:

$$ {\vec x}_{k+1} = \begin{bmatrix} 1 & 0 & 0 & T & 0 & 0 \\ 0 & 1 & 0 & 0 & T & 0 \\ 0 & 0 & 1 & 0 & 0 & T \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} {\vec x}_k + \begin{bmatrix} {T^2\over 2} & 0 & 0 & 0 & 0 & 0 \\ 0 & {T^2\over 2} & 0 & 0 & 0 & 0 \\ 0 & 0 & {T^2\over 2} & 0 & 0 & 0 \\ 0 & 0 & 0 & T & 0 & 0 \\ 0 & 0 & 0 & 0 & T & 0 \\ 0 & 0 & 0 & 0 & 0 & T \end{bmatrix} {\vec u}_k = \textbf{A} {\vec x}_k + \textbf{B} {\vec u}_k \tag 3$$

I would like to expand my model to incorporate additional states that would estimate the accelerometer bias. However, I don't know how to do that. My intuition tells me that I need to compare my acceleration data to something to obtain the bias components, but I don't know what. As I said, I have a GPS, so I am assuming there is something there that I can use, but I don't know what.

So my question is, how can the accelerometer bias terms be estimated using the Kalman Filter?

I have a IMU and GPS sensors on my PCB which contains a microcontroller. An IMU consists of a gyroscope, accelerometer and a magnetometer (the magnetometer is calibrated to compensate for the effects of hard and soft iron). I am using Madgwick's filter to estimate the rotation matrix needed to rotate the read out accelerometer values onto the North-East-Down (NED) coordinate system. After subtracting the gravity vector, I feed the obtained acceleration components to the Kalman Filter. Note that the Madgwick filter estimates the gyroscope bias.

The states of my Kalman filter are:

$$ \vec x = \begin{bmatrix} p_x & p_y & p_z & v_x & v_y & v_z \end{bmatrix}^T \tag 1$$

and the control vector is:

$$ \vec u = \begin{bmatrix} a_x & a_y & a_z & a_x & a_y & a_z \end{bmatrix}^T \tag 2$$

I constructed the discrete time model:

\begin{align} {\vec x}_{k+1} &= \begin{bmatrix} 1 & 0 & 0 & T & 0 & 0 \\ 0 & 1 & 0 & 0 & T & 0 \\ 0 & 0 & 1 & 0 & 0 & T \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} {\vec x}_k + \begin{bmatrix} {T^2\over 2} & 0 & 0 & 0 & 0 & 0 \\ 0 & {T^2\over 2} & 0 & 0 & 0 & 0 \\ 0 & 0 & {T^2\over 2} & 0 & 0 & 0 \\ 0 & 0 & 0 & T & 0 & 0 \\ 0 & 0 & 0 & 0 & T & 0 \\ 0 & 0 & 0 & 0 & 0 & T \end{bmatrix} {\vec u}_k \\&= \textbf{A} {\vec x}_k + \textbf{B} {\vec u}_k \tag 3 \end{align}

I would like to expand my model to incorporate additional states that would estimate the accelerometer bias. However, I don't know how to do that. My intuition tells me that I need to compare my acceleration data to something to obtain the bias components, but I don't know what. As I said, I have a GPS, so I am assuming there is something there that I can use, but I don't know what.

So my question is, how can the accelerometer bias terms be estimated using the Kalman Filter?

I have a IMU and GPS sensors on my PCB which contains a microcontroller. An IMU consists of a gyroscope, accelerometer and a magnetometer (the magnetometer is calibrated to compensate for the effects of hard and soft iron). I am using Madgwick's filter to estimate the rotation matrix needed to rotate the read out accelerometer values onto the North-East-Down (NED) coordinate system. After subtracting the gravity vector, I feed the obtained acceleration components to the Kalman Filter. Note that the Madgwick filter estimates the gyroscope bias.

The states of my Kalman filter are:

$$ \vec x = \begin{bmatrix} p_x & p_y & p_z & v_x & v_y & v_z \end{bmatrix}^T \tag 1$$

and the control vector is:

$$ \vec u = \begin{bmatrix} a_x & a_y & a_z & a_x & a_y & a_z \end{bmatrix}^T \tag 2$$

I constructed the state space model:

$$ \dot{\vec x} = \begin{bmatrix} 1 & 0 & 0 & T & 0 & 0 \\ 0 & 1 & 0 & 0 & T & 0 \\ 0 & 0 & 1 & 0 & 0 & T \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \vec x + \begin{bmatrix} {T^2\over 2} & 0 & 0 & 0 & 0 & 0 \\ 0 & {T^2\over 2} & 0 & 0 & 0 & 0 \\ 0 & 0 & {T^2\over 2} & 0 & 0 & 0 \\ 0 & 0 & 0 & T & 0 & 0 \\ 0 & 0 & 0 & 0 & T & 0 \\ 0 & 0 & 0 & 0 & 0 & T \end{bmatrix} \vec u = A \vec x + B \vec u \tag 3$$

I would like to expand my state space model to incorporate additional states that would estimate the accelerometer bias. However, I don't know how to do that. My intuition tells me I need compare my acceleration data to something to obtain the bias components, but I don't know what. As I said, I have a GPS, so I am assuming there is something there that I can use, but I don't know what.

So my question is, how can the accelerometer bias terms be estimated using the Kalman Filter?

I have a IMU and GPS sensors on my PCB which contains a microcontroller. An IMU consists of a gyroscope, accelerometer and a magnetometer (the magnetometer is calibrated to compensate for the effects of hard and soft iron). I am using Madgwick's filter to estimate the rotation matrix needed to rotate the read out accelerometer values onto the North-East-Down (NED) coordinate system. After subtracting the gravity vector, I feed the obtained acceleration components to the Kalman Filter. Note that the Madgwick filter estimates the gyroscope bias.

The states of my Kalman filter are:

$$ \vec x = \begin{bmatrix} p_x & p_y & p_z & v_x & v_y & v_z \end{bmatrix}^T \tag 1$$

and the control vector is:

$$ \vec u = \begin{bmatrix} a_x & a_y & a_z & a_x & a_y & a_z \end{bmatrix}^T \tag 2$$

I constructed the discrete time model:

$$ {\vec x}_{k+1} = \begin{bmatrix} 1 & 0 & 0 & T & 0 & 0 \\ 0 & 1 & 0 & 0 & T & 0 \\ 0 & 0 & 1 & 0 & 0 & T \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} {\vec x}_k + \begin{bmatrix} {T^2\over 2} & 0 & 0 & 0 & 0 & 0 \\ 0 & {T^2\over 2} & 0 & 0 & 0 & 0 \\ 0 & 0 & {T^2\over 2} & 0 & 0 & 0 \\ 0 & 0 & 0 & T & 0 & 0 \\ 0 & 0 & 0 & 0 & T & 0 \\ 0 & 0 & 0 & 0 & 0 & T \end{bmatrix} {\vec u}_k = \textbf{A} {\vec x}_k + \textbf{B} {\vec u}_k \tag 3$$

I would like to expand my model to incorporate additional states that would estimate the accelerometer bias. However, I don't know how to do that. My intuition tells me that I need to compare my acceleration data to something to obtain the bias components, but I don't know what. As I said, I have a GPS, so I am assuming there is something there that I can use, but I don't know what.

So my question is, how can the accelerometer bias terms be estimated using the Kalman Filter?

I have a IMU and GPS sensors on my PCB which contains a microcontroller. An IMU consists of a gyroscope, accelerometer and a magnetometer (the magnetometer is calibrated to compensate for the effects of hard and soft iron). I am using Madgwick's filter to estimate the rotation matrix needed to rotate the read out accelerometer values onto the North-East-Down (NED) coordinate system. After subtracting the gravity vector, I feed the obtained acceleration components to the Kalman Filter. Note that the Madgwick filter estimates the gyroscope bias.

The states of my Kalman filter are:

$$ \vec x = \begin{bmatrix} p_x & p_y & p_z & v_x & v_y & v_z \end{bmatrix}^T \tag 1$$

and the control vector is:

$$ \vec u = \begin{bmatrix} a_x & a_y & a_z & a_x & a_y & a_z \end{bmatrix}^T \tag 2$$

I constructed the state space model:

$$ \dot{\vec x} = \begin{bmatrix} 1 & 0 & 0 & T & 0 & 0 \\ 0 & 1 & 0 & 0 & T & 0 \\ 0 & 0 & 1 & 0 & 0 & T \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \vec x + \begin{bmatrix} T^2 & 0 & 0 & 0 & 0 & 0 \\ 0 & T^2 & 0 & 0 & 0 & 0 \\ 0 & 0 & T^2 & 0 & 0 & 0 \\ 0 & 0 & 0 & T & 0 & 0 \\ 0 & 0 & 0 & 0 & T & 0 \\ 0 & 0 & 0 & 0 & 0 & T \end{bmatrix} \vec u = A \vec x + B \vec u \tag 3$$

I would like to expand my state space model to incorporate additional states that would estimate the accelerometer bias. However, I don't know how to do that. My intuition tells me I need compare my acceleration data to something to obtain the bias components, but I don't know what. As I said, I have a GPS, so I am assuming there is something there that I can use, but I don't know what.

So my question is, how can the accelerometer bias terms be estimated using the Kalman Filter?

EDIT

I would like to explain why I am missing one half in my control matrix $B$. The reason I am not answering in the comment section is because a couple of mathematical steps are needed and it is more clear if I write it here.

To formulate my control matrix, I first wrote:

$$ \begin{bmatrix} \dot {\vec p} \\ \dot {\vec v} \end{bmatrix} = \begin{bmatrix} {\vec v} \\ {\vec a} \end{bmatrix} \tag 4$$

Next, I went from continuous time domain to the discrete one. This allowed me to write the above equation as:

$$ \begin{bmatrix} { {\vec p_k - \vec p_{k-1}} \over T} \\ { {\vec v_k - \vec v_{k-1}} \over T} \end{bmatrix} = \begin{bmatrix} {\vec v_k} \\ {\vec a_k} \end{bmatrix} \tag 5$$

which can be reformulated as:

$$ \begin{bmatrix} { {\vec p_k} } \\ { {\vec v_k}} \end{bmatrix} = \begin{bmatrix} \vec p_{k-1} + T{\vec v_k} \\ \vec v_{k-1} + T{\vec a_k} \end{bmatrix} = \begin{bmatrix} \vec p_{k-1} + T{(\vec v_{k-1} + T{\vec a_k})} \\ \vec v_{k-1} + T{\vec a_k} \end{bmatrix} \tag 6$$

Now I grouped the terms to get:

$$ \begin{bmatrix} { {\vec p_k} } \\ { {\vec v_k}} \end{bmatrix} = \begin{bmatrix} 1 & T \\ 0 & 1 \end{bmatrix} \begin{bmatrix} { {\vec p_{k-1}} } \\ { {\vec v_{k-1}}} \end{bmatrix} + \begin{bmatrix} T^2 & 0 \\ 0 & T \end{bmatrix} \begin{bmatrix} { {\vec a_k} } \\ { {\vec a_k}} \end{bmatrix} \tag 7 $$

The reason I do not have a one half in my control matrix is because I used only the first order Taylor's expansion for both position and velocity. The one half appears if the position is estimated using the second order Taylor's expansion. However, it is probably better to use the second order Taylor's expansion.

I have a IMU and GPS sensors on my PCB which contains a microcontroller. An IMU consists of a gyroscope, accelerometer and a magnetometer (the magnetometer is calibrated to compensate for the effects of hard and soft iron). I am using Madgwick's filter to estimate the rotation matrix needed to rotate the read out accelerometer values onto the North-East-Down (NED) coordinate system. After subtracting the gravity vector, I feed the obtained acceleration components to the Kalman Filter. Note that the Madgwick filter estimates the gyroscope bias.

The states of my Kalman filter are:

$$ \vec x = \begin{bmatrix} p_x & p_y & p_z & v_x & v_y & v_z \end{bmatrix}^T \tag 1$$

and the control vector is:

$$ \vec u = \begin{bmatrix} a_x & a_y & a_z & a_x & a_y & a_z \end{bmatrix}^T \tag 2$$

I constructed the state space model:

$$ \dot{\vec x} = \begin{bmatrix} 1 & 0 & 0 & T & 0 & 0 \\ 0 & 1 & 0 & 0 & T & 0 \\ 0 & 0 & 1 & 0 & 0 & T \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \vec x + \begin{bmatrix} {T^2\over 2} & 0 & 0 & 0 & 0 & 0 \\ 0 & {T^2\over 2} & 0 & 0 & 0 & 0 \\ 0 & 0 & {T^2\over 2} & 0 & 0 & 0 \\ 0 & 0 & 0 & T & 0 & 0 \\ 0 & 0 & 0 & 0 & T & 0 \\ 0 & 0 & 0 & 0 & 0 & T \end{bmatrix} \vec u = A \vec x + B \vec u \tag 3$$

I would like to expand my state space model to incorporate additional states that would estimate the accelerometer bias. However, I don't know how to do that. My intuition tells me I need compare my acceleration data to something to obtain the bias components, but I don't know what. As I said, I have a GPS, so I am assuming there is something there that I can use, but I don't know what.

So my question is, how can the accelerometer bias terms be estimated using the Kalman Filter?