How to deal with the discontinuity of yaw angle at 180 degree

Question

I'm working on a vehicle control and using a 9DOF sensor (accelerometer, magnetometer and gyroscope). For the yaw angle, I have a discontinuity problem at pi rad. (180 deg.). I'm controlling the vehicle with a PID controller and when the vehicle turns more than 180 deg, the sign suddenly changes (from 180 to -180) and this makes the controller act weird. The same problem will occur when it turns more than -180 deg too.

As the method, I'm using a direction cosine matrix to calculate euler angles. (recommended method for the sparkfun sensor.)

My question is what kind of approach should I use? How to deal with this discontinuity in the case of using a PID controller to control yaw angle.

Answer 1

I had the same problem and I'm doing the following:

#define MOD(a) ((a > 180.0) ? (a - 360.0) : ((a < -180.0) ? (a + 360.0) : a))

The difference of 2 angles can simply be put back to -180d/+180d with such formula.