Search code examples
integerinteger-overflowfixed-pointkalman-filterinteger-arithmetic

Overflow-aware implementation of a kalman filter

I'm trying to implement a kalman filter to obtain the orientation of an object, using an 3 axis accelerometer and a 3 axis gyroscope as sensors.

Choosing the dynamic model for the predict phase of this filter is straight forward, it's:

new_angle = angle + angular_velocity * time
new_angular_velocity = angular_velocity

But I do not have floating point support at hand, and I need every single bit of precision to model the angle. Therefore, my plan was to represent the angle as 32 bit integer data, representing a full turn of 2 pi as 2^32 small steps. As consequence, integer overflow handles the wrap (2 pi being the same orientation as 0) for free.

But this also poses a problem to the filter: If the estimated angle would be, say 359°, and my measurement is 0°, then the filter is assuming a huge innovation, leading to uncertainty and odd values.

Is there any way of making the filter aware of this possible wrap? Giving an innovation of only 1° in the above case?

To circumvent the problem, I thought about using angle differences instead of angles, but I cannot find a suitable model.

Solution

  • I had the same problem with similar KF today... but I was not as constrained as you.

    Here is what I did:

    1. Just after the predicted_angle value is calculated:

      // avoid jumping between 0 and 360
      if (measured_angle < pi/2 && predicted_angle > 3*pi/2) predicted_angle = predicted_angle - 2*pi;
      if (measured_angle > 3*pi/2 && estimate_aAngle < pi/2) predicted_angle = predicted_angle + 2*pi;

    2. Normalize the estimated_angle after it is calculated.

    If you don't mind sacrificing a bit of precision you might switch to signed integers [-2*Pi,+2*Pi) range and do the same.

    P.S. If the maximum angle change per sample is small, I think you could sacrifice just a fraction of one bit precision using internal angle offset in the KF. The offset must be greater enough than that value. You'll have a 2^32 = 2*Pi + 2*OFFSET range instead of 2*pi. In the KF add this OFFSET to the local angle variables and return estimated_angle = normalize(offset_estimated_angle - OFFSET, 0-2*pi).

    HTH