I have trouble estimating the heading when close to the "pivot" point of the compass, and could use some input on how to solve it. I have set up my angles to be from 0-360 degrees during the testing but will be using radians (-pi, pi) from now on.
The setup is a differential robot with IMU, wheel encoders and a magnetic compass.
A complementary filter is used for fusing gyroZ and odo measurements to estimate the heading, and then correct it with a Kalman filter using the magnetic compass.
My problem occurs when the robot heading is close to -pi/pi .
The estimated heading is useless even though the robot is not even moving.
I am thinking this must be a very common problem and probably has a better solution than what I came up with, which was either re-initializing the integrator when crossing zero, adding 180 degrees each time the error is larger, or just ignoring the compass if the error is too large...
It's my first Kalman filter so I may have made a poor implementation if this is not a common issue...
Edit: trudesagen's solution solved my problem.