## Schlüsselwortarchiv

Du betrachtest das Archiv des Tags Integration Drift.

• ## 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).

• ## On integration drift

While imple­ment­ing a sen­sor fusion algo­rithm I stum­bled across the prob­lem that my well-cal­i­brat­ed gyro­scope would yield slow­ly drift­ing read­ings for the inte­grat­ed angles.
There are at least two rea­sons for this behav­iour: It is pos­si­ble that the gyro bias was not removed exact­ly — not so much because it’s a sto­chas­tic quan­ti­ty, but more because it’s a machine pre­ci­sion prob­lem after all — and drift induced dur­ing numer­ic inte­gra­tion due to round­ing errors.

Fouri­er states that every (infi­nite and peri­od­ic) sig­nal can be assem­bled by using only cosine and sine func­tions. Gauss­ian noise has a mean ampli­tude in all fre­quen­cies, but still a gauss­ian ampli­tude dis­tri­b­u­tion. In oth­er words: Gauss­ian noise con­tains dif­fer­ent­ly strong sine and cosine sig­nals for every fre­quen­cy.

Now the inte­gral of the sine and cosine func­tions is defined as fol­lows:

\begin{align}
\int cos(2 \pi f t) &= \quad \frac{1}{2 \pi f} sin(2 \pi f t) \\
\int sin(2 \pi f t) &= -\frac{1}{2 \pi f} cos(2 \pi f t)
\end{align}

What that means is that for every high fre­quen­cy sine-like sig­nal (i.e. $f \geq 1 \mathrm{Hz}$ ) , the ampli­tude of that sig­nal will be low­ered by fac­tor $2 \pi f$ . For every low fre­quen­cy sig­nal (i.e. $f \lt 1 \mathrm{Hz}$ ) the fre­quen­cy will be ampli­fied by $2 \pi f$ .

Now it’s just a ques­tion if your sig­nal con­tains gauss­ian noise or if your sys­tem oscil­lates. In any way, if there is a low fre­quen­cy com­po­nent, inte­gra­tion will turn it into a strong, slow sine wave shape — drift.