One interesting observation when working with the standard model for constant acceleration in the Kalman filter is that the results tend to drift over time, even if the input to the system is zero and unbiased. I stumbled across this recently when integrating angular velocities measured using a gyroscope. Obviously, calibrating the gyroscope is the first step to take, but even then, after a while, the estimation will be off.

So the differential equations describing motion with constant acceleration are given as

\(\begin{align}

x(t) &= x_0 + v(t)\,\mathrm dt + \frac{1}{2}a(t)\,\mathrm dt^2 \\

v(t) &= v_0 + a(t)\,\mathrm dt \\

a(t) &= \mathrm{const}

\end{align}

\)

The continuous-time state-space representation of which being

\(\begin{align}

\dot{\vec{x}}(t) = \underbrace{\begin{bmatrix}

0 & \mathrm dt & 0.5\,\mathrm dt^2 \\

0 & 0 & \mathrm dt \\

0 & 0 & 0

\end{bmatrix}}_{\underline{A}} \cdot \underbrace{\begin{bmatrix}

x \\

v \\

a

\end{bmatrix}}_{\vec{x}}

\end{align}

\)

where the state vector \(\vec{x}\) would be initialized with \(\left[x_0, v_0, a_0\right]^T\). Modeled as a discrete-time system, we then have

\(\begin{align}

\vec{x}_{k+1} = \begin{bmatrix}

1 & T & 0.5\,\mathrm T^2 \\

0 & 1 & \mathrm T \\

0 & 0 & 1

\end{bmatrix}_k \cdot \begin{bmatrix}

x \\

v \\

a

\end{bmatrix}_k

\end{align}

\)

with \(T\) being the time constant.

Now due to machine precision and rounding issues we’ll end up with an error in every time step that is propagated from the acceleration to the position through the double integration. Even if we could rule out these problems, we still would have to handle the case of drift caused by noise.

According to *Position Recovery from Accelerometric Sensors* (Antonio Filieri, Rossella Melchiotti) and *Error Reduction Techniques for a MEMS Accelerometer-based Digital Input Device* (Tsang Chi Chiu), the integration drift can be modeled as process noise in the Kalman filter.

Tsang (appendix B, eq. 7) shows that the drift noise is given as

\(\begin{align}

\underline{Q}_a = \begin{bmatrix}

\frac{1}{20} q_a \,T^5 & \frac{1}{8} q_a \,T^4 & \frac{1}{6} q_a \,T^3 \\

\frac{1}{8} q_a \,T^4 & \frac{1}{3} q_a \,T^3 & \frac{1}{2} q_a \,T^2 \\

\frac{1}{6} q_a \,T^3 & \frac{1}{2} q_a \,T^2 & q_a \,T

\end{bmatrix}

\end{align}

\)

with \(q_a\) being the acceleration process noise (note that Tsang models this as \(q_c\) in continuous-time).