• ## Kalman filter: Modeling integration drift

One inter­est­ing obser­va­tion when work­ing with the stan­dard mod­el for con­stant accel­er­a­tion in the Kalman fil­ter is that the results tend to drift over time, even if the input to the sys­tem is zero and unbi­ased. I stum­bled across this recent­ly when inte­grat­ing angu­lar veloc­i­ties mea­sured using a gyro­scope. Obvi­ous­ly, cal­i­brat­ing the gyro­scope is the first step to take, but even then, after a while, the esti­ma­tion will be off.

So the dif­fer­en­tial equa­tions describ­ing motion with con­stant accel­er­a­tion are giv­en 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 con­tin­u­ous-time state-space rep­re­sen­ta­tion 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 vec­tor $\vec{x}$ would be ini­tial­ized with $\left[x_0, v_0, a_0\right]^T$ . Mod­eled as a dis­crete-time sys­tem, 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 con­stant.

Now due to machine pre­ci­sion and round­ing issues we’ll end up with an error in every time step that is prop­a­gat­ed from the accel­er­a­tion to the posi­tion through the dou­ble inte­gra­tion. Even if we could rule out these prob­lems, we still would have to han­dle the case of drift caused by noise.

Accord­ing to Posi­tion Recov­ery from Accelero­met­ric Sen­sors (Anto­nio Fil­ieri, Rossel­la Mel­chiot­ti) and Error Reduc­tion Tech­niques for a MEMS Accelerom­e­ter-based Dig­i­tal Input Device (Tsang Chi Chiu), the inte­gra­tion drift can be mod­eled as process noise in the Kalman fil­ter.

Tsang (appen­dix B, eq. 7) shows that the drift noise is giv­en 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 accel­er­a­tion process noise (note that Tsang mod­els this as $q_c$ in con­tin­u­ous-time).