1

I'm receiving angle updates from a sensor in radians and I want to pass them through a low pass filtering function to smooth them slightly because of noise.

My low pass filter function looks like this 

protected void lowPass(float alpha, double[] input, double[] output) {
    for (int i = 0; i < input.length; i++) {
        output[i] = alpha * output[i] + (1 - alpha) * input[i];
    }
}

and it works great for the most parts.

The problem is that sometimes the angles "go full circle", e.g. from 2π to 0, 0 to -2π, etc. Of course this results in "incorrect" output, since the filter function treats 2π and 0 as simply 6.28 and 0.0.

How would I implement a filter function that can handle these kinds of angles correctly?

Magnus
  • 17,157
  • 19
  • 104
  • 189

3 Answers3

3

There is interesting approach used for angle averaging.

Get cosine and sine of angle alpha (as components of vector with this angle) and use them for separate filtering, then get angle for result vector

 cosa = alpha * cos(output) + (1 - alpha) * cos(input);
 sina = alpha * sin(output) + (1 - alpha) * sin(input);
 output = atan2(sina, cosa)
MBo
  • 77,366
  • 5
  • 53
  • 86
  • Interesting indeed. There is a little bias for α <> 1/2 because this constructs the point at the fraction α of the chord, which is not exactly at the fraction α of the arc. But this is a second order effect. –  Apr 23 '18 at 19:12
2

For exponential smoothing like that, the easiest approach is to add or subtract 2*pi from each input to make it as close as possible to the previous smoothed value. Or, equivalently, add or subtract 2*pi from the previous output before smoothing to the input. In either case, your outputs can temporarily pass outside the [0,2*pi) range, so make sure to wrap back around into that range if you need it.

Incidentally, your code has a bug, in that it reads from the wrong output index and doesn't work properly with the first time value.

Sneftel
  • 40,271
  • 12
  • 71
  • 104
1

You need to perform phase unwrapping, i.e. recover the 2π jumps that occur periodically. For this you can detect anomalies such as a value close to 2π immediately followed by a value close to 0, and conversely, and compensate.

Note that if the values change in large increments (say larger than π), the jumps are unrecoverable.

See phase unwrap issue (the unwrapping of the phases is not correcly)?.