How can I estimate accelerometer bias using a GPS and the Kalman filter?

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:

\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?

Answer 1

I'm going to change your notation for (3), so that my answer will be compact enough to fit on one page.

Restating your model, $$\vec x_k = \left [ \begin{array}{c|c} \mathbf 1 & \mathbf T \\ \hline \mathbf 0 & \mathbf 1 \end{array} \right ] x_{k-1} + \begin{bmatrix}\mathbf{\frac {T^2}{2}} \\ \hline \mathbf T \end{bmatrix} u_k \tag a$$

This says the same thing, but leaves you to infer that the elements are all 3x3 identity matrices multiplied by the given factor. Note that I've trimmed your $u_k$ to just three elements to eliminate redundancy.

To add accelerometer bias into the mix, just add it as a state, so that your (1) becomes $$ \vec x = \begin{bmatrix} p_x & p_y & p_z & v_x & v_y & v_z & \Delta a_x & \Delta a_y & \Delta a_z \end{bmatrix}^T \tag b$$ with $\Delta a_x \cdots$ being the x, y, and z components of the accelerometer bias.

Now augment your model for the extra states: $$\vec x_k = \left [ \begin{array}{c|c|c} \mathbf 1 & \mathbf T & \mathbf{\frac{T^2}{2}} \\ \hline \mathbf 0 & \mathbf 1 & \mathbf T \\ \hline \mathbf 0 & \mathbf 0 & \mathbf 1 \end{array} \right ] x_{k-1} + \begin{bmatrix}\mathbf{\frac {T^2}{2}} \\ \hline \mathbf T \\ \hline \mathbf 0 \end{bmatrix} u_k \tag a$$

Then choose reasonable numbers for your accelerometer bias' process noise, and construct your Kalman filter as usual.

Because you are describing the dependency of the observed object motion on the accelerometer bias in your state transition matrix, that dependency will make it into the covariance ($\mathbf P$) matrix. That will, in turn, affect the Kalman gain, which means that deviations in the object's predicted motion from measured will affect the observed accelerometer bias.