How to smooth a list of angles.

Question

I'm not a math guy so maybe there is a super simple thing that my eyes cannot see. And sorry if my math terminology is not good at all. Please address me the right math terminology to use because probably this is one of the reason I'm not finding a good solution: I'm not finding the right question.

I'm in the realm of programming a 3D box that changes its orientation (roll, pitch, yawn) in respect to an accelerometer. Everything goes well, I'm receiving a list of numbers that represents angles in degree from the accelerometer along axis x,y,z and the box rotates in the middle of the screen.

But I'd like to post-process this signal - for example to take the direction of the movement (for instance: rotating clockwise or anti-clockwise?).

But the signal is a little bit noisy so.. the first post-process operation I'd like to do is smoothing. What to say, simple average, running average, kalman filter.. with those techniques I'm facing a big problem: I'm always talking about angles. And angles are periodic. So, in every way I try to resampling the angles ([0-360], [-180,180], etc) if I'm doing a complete round I'm going from 0 to 360 or vice versa. This let the smoothing filter (let's say I'm using a running average) to get crazy.

Normal angle values cannot be smoothed/interpolated. How to do it?

Answer 1

You need to use trigonometrical functions to normalize the angles which are discontinuous around the boundaries. It can be done using combination of sin and cos, arctan2 functions.

I use following java code for filtering 9-axis IMU sensor values. I hope this helps.

public class AngleSmoothingFilter {

    private FloatSmoothingFilter sinFilter = new FloatSmoothingFilter();
    private FloatSmoothingFilter cosFilter = new FloatSmoothingFilter();

    public void add(float angleInDegree) {
        float radian = (float) Math.toRadians(angleInDegree);
        float sinValue = (float) Math.sin(radian);
        float cosValue = (float) Math.cos(radian);

        sinFilter.add(sinValue);
        cosFilter.add(cosValue);
    }

    public float getAverage() {
        float sinAverage = sinFilter.getAverage();
        float cosAverage = cosFilter.getAverage();

        float radian = (float) Math.atan2(sinAverage, cosAverage);
        return (float) Math.toDegrees(radian);
    }

}

FloatSmoothingFilter is a normal smoothing filter.

Answer 2

You have to have enough information to know what cycle you are in. The easiest way is to sample the data often enough that the change in angle is always well less than $180^\circ$ between samples. Then you can just add or subtract $360^\circ$ as required to make the minimum change and smooth after that.