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

Is this your problem?

After desired angle changes it's sign(ie. 180->-180) 
then suddenly control input(ie. P * error) becomes bigger.

For example.

--------------------------------
|  desired  |  real  |  error  |
|-----------|--------|---------|
|    170    |   160  |   10    |
|   -170    |   160  |  -330   |
--------------------------------

If your problem is same as I understood, how about this? Before we go, note that It is assumed that every angle values are in range of [-pi,pi].

error = desired - real;
if(error > 180)
   error = error - 360;
else if(error < -180)
   error = error + 360;
else
   error = error;//do nothing

This method always chooses a direction such that control input becomes smaller.(You know there are two choices for direction)

Answer 2

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.