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

• ## Linear (binary) integer programming in Matlab

So, suppose you’re in university, it’s that time of the year again (i.e. exams) and you already have written some of them. Some are still left though and you wonder: How hard can you fail – or: how good do you have to be – in the following exams given that you do not want your mean grade to be worse than a given value.

## Linear programming

Say you’re in Germany and the possible grades are [1, 1.3, 1.7, 2, .. 4] (i.e. a closed interval) with 1 being the best grade and 4 being only a minor step to a major fuckup. Given that you’ve already written four exams with the grades 1, 1, 1.3 and 1 and that you do not want to have a mean grade worse than 1.49 in the end (because 1.5 would be rounded to 2 on your diploma), but there still are 9 more exams to write, the question is: Which worst-case grades can you have in the following exams and what would that imply for the others?

This is what’s known to be a linear programming or linear optimization problem, and since the values (here: the number of grades) are constrained to be discrete values, it’s integer programming.

The goal of linear programming is to find the arguments $x$ of the objective function $f(x)$ such that $f(x)$ is maximized, given some constraints on $x$. In Matlab, all linear programming functions try to minimize the cost function, so the problem is formulated as

\begin{align} \underset{x}{\mathrm{min}} \, f(\vec{x}) \, \mathrm{such}\,\mathrm{that} \, \left\{\begin{matrix} \underline{A}\cdot \vec{x} \leq \vec{b} \\ \underline{A}_{eq} \cdot \vec{x} = \vec{b}_{eq} \end{matrix}\right. \end{align}

Obviously, maximizing an objective function is the same as minimizing it’s negative, so $\mathrm{max} \, f(\vec{x}) = -\mathrm{min} \left(\, -f(\vec{x}) \right)$. In Matlab, these kind of problems can be solved with the linprog function.

• ## Hosting the .NET 4 runtime in a native process

I recently remembered a fellow student who, a couple of years ago, told me that he injected a (native) DLL into a foreign process and started the .NET runtime there, that is, had C# code run in a foreign native process. Well, bright eyes on my side and then I forgot about it.

Anyway, some days ago I gave it a try and it found out that this isn’t too hard to achieve. Running your .NET assembly in a native application requires basically four steps using the .NET 4 Hosting Interfaces:

1. Retrieving an interface to the CLR (meta) host.
2. Requesting an interface to the required runtime.
3. Retrieving the actual interface of the runtime.
4. Executing the assembly in the default application domain.

So here we go, ignoring all return codes on the way: Weiterlesen »

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

• ## To tilt compensate, or not to tilt compensate

When diving into the field of attitude detection from sensor readings a couple of answers seem omnipresent:

• You need an accelerometer and a magnetometer.
• You need to tilt compensate.
• You need to low-pass filter.
• Or use the complimentary filter.
• Or better still, use the mystical Kalman filter (but it’s too complicated to explain).

Starting from the above and having an accelerometer and a magnetometer at hand, the old question a wise man once asked is:

To tilt compensate, or not to tilt compensate, that is the question—
Whether ’tis Nobler in the mind to suffer
The Slings and Arrows of outrageous StackOverflow answers,
Or to take Arms against a Sea of troubles,
And by using the attitude matrix, end them? To tilt: to roll.

In other words: Is tilt compensation required? Weiterlesen »