5
$\begingroup$

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.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
4
$\begingroup$

I encountered this problem myself when making an extended Kalman filter for a quadrotor.

You have to check if the estimate goes above or below +/- pi, and then correct it if it does.

You can do this with a simple if statement:

if (angle > pi)
    angle = angle - 2*pi;
else if (angle < -pi)
    angle = angle + 2*pi;

You could also throw in a while loop over that, if the value ever becomes larger than 2*pi for some reason, due to spikes etc. This looks like so:

while (abs(angle) > pi) {    
    if (angle > pi)
        angle = angle - 2*pi;
    else if (angle< -pi)
        angle = angle + 2*pi;
}

Remember to have this check after your prediction, as well as on the difference between your measured compass angle and your prediction, that you use to calculate the corrected estimate.

After this you also check the corrected estimate for the same, which should fix your problem.

Improve this answer
$\endgroup$
6
  • $\begingroup$ This is a kind of band-aid for the problem. The issue still exists in the angular velocity component of the Kalman Filter, although it should correct itself quickly. A better solution is to use orientation parametrisations that do not suffer from gimbal lock as Euler angles do (quaternions or rotation matrices) in the filters. $\endgroup$
    – Gouda
    May 29, 2016 at 1:58
  • 1
    $\begingroup$ I think you are misunderstanding the problem. This is how you keep the heading within +/- 180 degrees. Also, it is a differential two wheeled robot, so you will never encounter a gimbal lock, and if you do (pitch or roll at 90 degrees, depending on the euler rep, you have other problems on your hands). What do you mean by "the issue still exists in the angular velocity" ? The angular velocity can be as high or low as it want to be, doesn $\endgroup$
    – trudesagen
    May 29, 2016 at 15:27
  • 1
    $\begingroup$ @Gouda I think you misunderstand the issue here. It is not a band aid, this is how you keep your heading calculations within +/- pi (180) degrees. This is needed when controlling the robot, so that when you want to turn 90 degrees and your controller subtracts this from your current heading, you don't have to turn 5 times to get there, if the robot is at 5*2pi heading. Gimbal lock is never an issue with planar two wheeled robots, only for pitch or roll, depending on your euler representation. Also, this is NOT an issue for angular velocity, which is the diff between on stample and the next. $\endgroup$
    – trudesagen
    May 29, 2016 at 15:32
  • 1
    $\begingroup$ Had to comment twice, as my connection went down mid comment and lost my 5 min edit window $\endgroup$
    – trudesagen
    May 29, 2016 at 15:34
  • $\begingroup$ @trudesagen, I agree. Perhaps what I meant to say was, the angular velocity is determined by the heading residual, which can be easily incorrectly calculated as the difference between a predicted angle and measured angle. eg in the case case of estimating 1 degree then measuring 359 degrees the angular velocity is very large, when in fact it (probably) is very small and negative. Although it's easy to account for that in the 2D case too :). The reason I mention quaternions is because it will take care of both the angle wrap and angular velocity problems simultaneously. $\endgroup$
    – Gouda
    Jun 2, 2016 at 14:23

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.