Pong

July26th 2015 Compass Smoothing

← July 24th 2015 QCompass | ● | July 27th 2015 Compass Widget →

On an Android phone, the compass azimuth is getting updated pretty quickly every 50ms. Since the readings are very noisy and tend to be off by up to ±4 degrees for each measurement, the compass readings look jumpy. They are not as steady as one is used to from using a real compass.

To smooth the compass azimuth readings $\omega_i$ (given in degrees), there are basically three options mentioned frequently on the web:

  • Simulate a real compass needle by updating the compass readings with a small delta factor:
    • $\omega' = \omega' + 0.15\cdot\Delta\omega$
    • with $\Delta\omega=(\omega_i-\omega'+180)$ $mod$ $360 - 180$.
    • The delta factor of 0.15 is somewhat arbitrary, but one could elaborate on that by deriving the above formula from the physical equations of a compass needle with a certain mass, which follows the original magnetic field readings with a certain momentum. The result is more or less the same.
  • Applying a Kalman filter.
  • Applying a low-pass filter by averaging the last $n$ readings. Averaging is done by summing up the $n$ direction vectors for each reading and taking the atan2 of the resulting vector. For more information see wikipedia.

Although the latter method is pretty popular, because is easy to implement and does what it promises, it shows a certain lag. For $n=10$ readings the lag is half a second. The question is: can we do better than this, while still coming up with a simple algorithm? The answer is yes: There is even a much simpler algorithm. But first let’s have a look at the naive averaging approach:

Foolishly averaging angular quantities:

  • Given a set of $n$ angular quantities $\omega_i$, $i=0..n-1$.
  • Then the average of the angular quantities $\omega_i$ is $\frac{1}{n}\sum_i\omega_i$.

What’s wrong with the above naive formula? Averaging only works for monotone scalar values, but the angular quantities are not monotone at 360 degrees. For example, if we average two values of 1 and 359 degrees, we’d expect a result of zero, but $(1+359)/2$ yields 180 degrees for the above formula.

This is due to the fact that we are not going clockwise from 1 to 359 but counter-clockwise from 1 to 359. This means that the angular delta is not +358 degrees clockwise but −2 degrees counter-clockwise. So wherever we encounter an angular delta with an absolute value larger than 180 degrees, we get unexpected results. Fortunately we can fix those deltas by adding multiples of 360 degrees, so that the delta stays within a maximum of 180 degrees clock-wise or −180 degrees counter-clockwise. The following revised averaging algorithm builds on that principle:

Averaging angular quantities made even simpler:

  • Given a set of $n$ angular quantities $\omega_i$, $i=0..n-1$.
  • Then the average $\omega$ of the angular quantities $\omega_i$ is simply the average of the monotone series of quantities $\omega_i'$ with
  • $\omega = \frac{1}{n}\sum_i\omega_i'$ and
  • $\omega_i' = \omega_i + q \cdot 360$ $(q\in N)$ so that $|\omega_i’-\omega_{i-1}’| \le 180.

The pseudo code for the above algorithm looks like that:

// INPUT: omega[i] with i=0..n-1

for (int i=1; i<n; i++)
{
   while (omega[i] - omega[i-1] > 180) omega[i] -= 360;
   while (omega[i-1] - omega[i] > 180) omega[i] += 360;
}

float average = 0;
for (i=0; i<n; i++) average += omega[i];
average /= n;

average = (average + 180) % 360 - 180;

// OUTPUT: angle average (in the range [-180..180])
[Get Code]

Yet another question: can we do better than this by getting rid of the lag without diving deep into Kalman filtering? Yes, we can!

Smoothing angular quantities by least squares fitting:

Assuming a monotone series of quantities $\omega_i'$, we can fit the linear function $f(t) = c_0+c_1t$ to the value pairs $(t_i, \omega_i')$ as described here with the following residual:

$ R = \sum_i (f(t_i)-\omega_i')^2 $

We need to find the coefficients $c_0$ and $c_1$, where the residual gets minimal by setting the gradient to zero: $\nabla R=0$.

Calculating the gradient and the corresponding partial derivatives leads to the following system of linear equations:

$ \left( \begin{array}{c c} n & \sum_i t_i \\ \sum_i t_i & \sum_i t_i^2 \end{array} \right) \left( \begin{array}{c} c_0 \\ c_1 \end{array} \right) = \left( \begin{array}{c} \sum_i \omega_i' \\ \sum_i \omega_i' t_i \end{array} \right) $

We solve for the parameter vector $(c_0, c_1)^T$ by multiplying with the inverse of the left-hand matrix:

$ c_0 = \frac{ \sum_i t_i^2 \sum_i w_i' - \sum_i t_i \sum_i w_i' t_i } { n \sum_i t_i^2 - (\sum_i t_i)^2 } $
$ c_1 = \frac{ n \sum_i w_i' t_i - \sum_i t_i \sum_i w_i' } { n \sum_i t_i^2 - (\sum_i t_i)^2 } $

Then the linearily smoothed angular quantity is $(f(t_{n-1})+180)$ $mod$ $360$ $-180$ $=$ $(c_0+c_1 \cdot t_{n-1}+180)$ $mod$ $360$ $-180$.

With the WLS module of libmini, we get the following implementation:

// INPUT: omega[i] with i=0..n-1

#include <mini/miniwls.h>

miniWLSA wlsa;

for (unsigned int t=0; t<n; t++)
{
   WLS_Point a = {(double)t,omega[t],1};
   wlsa.push_back(a);
}

wlsa.linear_fit();
double a = wlsa.eval(n-1);

// OUTPUT: smoothed angle a (in the range [-180..180])
[Get Code]

We can even use a fit to a quadratic function to improve the compass smoothness. In this case we use the quadratic_fit() method instead of the linear_fit() method.

If you happen to use that code, please link to this page!

Note: Please view with Firefox. Chrome and Safari won’t display the math formulas in this page properly.

← July 24th 2015 QCompass | ● | July 27th 2015 Compass Widget →

Options: