## Schlüsselwortarchiv

Du betrachtest das Archiv des Tags Gyroscope.

• ## Kalman filter: Modeling integration drift

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

• ## On integration drift

While implementing a sensor fusion algorithm I stumbled across the problem that my well-calibrated gyroscope would yield slowly drifting readings for the integrated angles.
There are at least two reasons for this behaviour: It is possible that the gyro bias was not removed exactly – not so much because it’s a stochastic quantity, but more because it’s a machine precision problem after all – and drift induced during numeric integration due to rounding errors.

Fourier states that every (infinite and periodic) signal can be assembled by using only cosine and sine functions. Gaussian noise has a mean amplitude in all frequencies, but still a gaussian amplitude distribution. In other words: Gaussian noise contains differently strong sine and cosine signals for every frequency.

Now the integral of the sine and cosine functions is defined as follows:

\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 frequency sine-like signal (i.e. $f \geq 1 \mathrm{Hz}$) , the amplitude of that signal will be lowered by factor $2 \pi f$. For every low frequency signal (i.e. $f \lt 1 \mathrm{Hz}$) the frequency will be amplified by $2 \pi f$.

Now it’s just a question if your signal contains gaussian noise or if your system oscillates. In any way, if there is a low frequency component, integration will turn it into a strong, slow sine wave shape – drift.