FIXING BUG when Using Wire library to obtain compass heading

Question

I am using the HMC-5983 magnetometer module to obtain the compass heading of a autonomous car I am building using the arduino platform.I'musing the Z axis and X axis to obtain the heading.

However when the headingFiltered variable is at 359 and the car turns to Zero,the headingFiltered becomes to 0 via all the values between 0 and 360 rather than just becoming 0.This causes my car be constantly steering side to side when heading in this relevant direction. Also if there is anyway that this function could be improved,I'm open to suggestions.

The function I'm using is as follows.

void chead() {
 //---- X-Axis
 Wire.beginTransmission(Magnetometer); // transmit to device
 Wire.write(Magnetometer_mX1);
 Wire.endTransmission();
 Wire.requestFrom(Magnetometer,1);
 if(Wire.available()<=1)
 {
 mX0 = Wire.read();
 }
 Wire.beginTransmission(Magnetometer); // transmit to device
 Wire.write(Magnetometer_mX0);
 Wire.endTransmission();
 Wire.requestFrom(Magnetometer,1);
 if(Wire.available()<=1)
 {
 mX1 = Wire.read();
 }
 //---- Y-Axis
 Wire.beginTransmission(Magnetometer); // transmit to device
 Wire.write(Magnetometer_mY1);
 Wire.endTransmission();
 Wire.requestFrom(Magnetometer,1);
 if(Wire.available()<=1)
 {
 mY0 = Wire.read();
 }
 Wire.beginTransmission(Magnetometer); // transmit to device
 Wire.write(Magnetometer_mY0);
 Wire.endTransmission();
 Wire.requestFrom(Magnetometer,1);
 if(Wire.available()<=1)
 {
 mY1 = Wire.read();
 }
 //---- Z-Axis
 Wire.beginTransmission(Magnetometer); // transmit to device
 Wire.write(Magnetometer_mZ1);
 Wire.endTransmission();
 Wire.requestFrom(Magnetometer,1);
 if(Wire.available()<=1)
 {
 mZ0 = Wire.read();
 }
 Wire.beginTransmission(Magnetometer); // transmit to device
 Wire.write(Magnetometer_mZ0);
 Wire.endTransmission();
 Wire.requestFrom(Magnetometer,1);
 if(Wire.available()<=1)
 {
 mZ1 = Wire.read();
 }
 //---- X-Axis
 mX1=mX1<<8;
 mX_out =mX0+mX1; // Raw data
 // From the datasheet: 0.92 mG/digit
 Xm = mX_out*0.00092; // Gauss unit
 //* Earth magnetic field ranges from 0.25 to 0.65 Gauss, so these are the values that we need to get approximately.
 //---- Y-Axis
 mY1=mY1<<8;
 mY_out =mY0+mY1;
 Ym = mY_out*0.00092;
 //---- Z-Axis
 mZ1=mZ1<<8;
 mZ_out =mZ0+mZ1;
 Zm = mZ_out*0.00092;
 // ==============================
 //Calculating Heading
 heading = atan2(Zm, Xm);// arc tangent of z/x
 // Correcting the heading with the declination angle depending on your location
 declination = 0.03717551307 ;
 heading += declination;
 // Correcting when signs are reveresed
 if(heading <0) heading += 2*PI;
 // Correcting due to the addition of the declination angle
 if(heading > 2*PI)heading -= 2*PI;
 headingDegrees = heading * 180/PI; // The heading in Degrees unit
 // Smoothing the output angle / Low pass filter
 headingFiltered = headingFiltered*0.85 + headingDegrees*0.15;
 //Sending the heading value through the Serial Port to Processing IDE
 //this was done to fix an error.which was when the compass was made to turn by 180° the reading obtained did not change by 180°.
 if(headingFiltered >= 0 && headingFiltered <= 89){
 c_head=map(headingFiltered,0,90,270,359);
 }
 if(headingFiltered > 89 && headingFiltered <= 204){
 c_head=map(headingFiltered,90,204,0,90);
 }
 if(headingFiltered > 204 && headingFiltered <= 290){
 c_head=map(headingFiltered,205,290,91,180);
 }
 if(headingFiltered > 290){
 c_head=map(headingFiltered,291,359,181,269);
 }
 //Further calibration
 if(c_head>=0 && c_head<8){
 c_head=352+c_head;
 }
 if(c_head>=8){
 c_head=c_head-8;
 }
 // Serial.print("Degrees - ");
 // Serial.println(c_head);
 float teta1 = radians(clatval);
 float teta2 = radians(dlatval);
 float delta1 = radians(dlatval-clatval);
 float delta2 = radians(dlongval-clongval); //==================Heading Formula Calculation================//
 float y = sin(delta2) * cos(teta2);
 float x = cos(teta1)*sin(teta2) - sin(teta1)*cos(teta2)*cos(delta2);
 float brng = atan2(y,x);
 degris = degrees(brng);// radians to degrees
 degris = ( ((int)degris + 360) % 360 );
 dif_h=c_head - degris;
 abs_dif_h=abs(dif_h);
}

Answer 1

The problem comes from the low-pass filter you are using:

// Smoothing the output angle / Low pass filter
headingFiltered = headingFiltered*0.85 + headingDegrees*0.15;

This filter is designed to smooth out rapid variations of the measured heading. If the heading suddenly jumps from 359° to 0°, the filter will replace that jump by a continuous change that goes through the intermediate values.

I can think of three different options for solving this problem.

Option 1: filter the Cartesian coordinates

Unlike the heading, the in-plane components of the magnetic field vary smoothly as the vehicle rotates. You can filter these components before computing the heading as follows:

const float filter_constant = 0.15;
float filtered_angle(float y, float x)
{
 static float x_f, y_f; // filtered components
 x_f += filter_constant * (x - x_f);
 y_f += filter_constant * (y - y_f);
 return atan2(y, x);
}

Then you would replace

heading = atan2(Zm, Xm);

by

heading = filtered_angle(Zm, Xm);

Option 2: filter the unwrapped heading

Another possible approach is to unwrap the heading before filtering it. This is similar to phase unwrapping: instead of requiring the heading to be limited to the interval [−π, π] (or [0, 360°) if you work with degrees), you let it take arbitrary values. Each time your vehicle does a full turn, the unwrapped heading varies by ±360°. Here is a simple unwrapping filter:

// Unwrap an angle in radians.
float unwrap(float angle)
{
 static int revolutions;
 static float previous_angle;
 float delta = angle - previous_angle;
 previous_angle = angle;
 if (delta >= M_PI)
 revolutions--;
 else if (delta < -M_PI)
 revolutions++;
 return 2*M_PI*revolutions + angle;
}

The unwrapped heading is continuous, and can be safely low-pass filtered without creating the kind of issue you have now. After the low-pass filter, you can wrap it again with fmod() if you wish.

Option 3: wrap the heading differences within the filter

The previous idea can be applied backwards: instead of unwrapping the heading, you can wrap the heading differences within the low-pass filter, in order to keep them within [−π, +π) (i.e. [−180°, +180°)). Then, if the filtered heading is 359° and you get a new raw heading of 0°, the difference is −359°, but you wrap it to +1°. This will cause the filtered output to just slightly increase to 359.15°, given your chosen filter constant of 0.15.

Here is a low-pass filter based on this idea, that is aware of how angles are supposed to wrap:

const float filter_constant = 0.15;
// Wrap an angle to [-pi, pi).
float wrap(float angle)
{
 while (angle < -M_PI) angle += 2*M_PI;
 while (angle >= M_PI) angle -= 2*M_PI;
 return angle;
}
// Low-pass filter an angle.
float filter_angle(float angle)
{
 static float filtered_angle;
 filtered_angle += filter_constant * wrap(angle - filtered_angle);
 filtered_angle = wrap(filtered_angle);
 return filtered_angle;
}

I think I like this option better that the other two because it does its arithmetics in a way that is consistent with how angles are supposed to wrap, and also because it keeps less state (only one static variable). Also, you have implemented something very similar to the wrap() function above, in order to wrap the heading to [0, 2π). You could instead reuse wrap() as:

// Wrap to [0, 2 pi).
heading = wrap(heading - M_PI) + M_PI;

Edit: There is a recently published Arduino library that implements this third option. It "provides a class, runningAngle, that computes an exponentially weighted running average of a series of angles, such as compass readings. It is aware of how angles wrap modulo 360°." The library is called runningAngle and is available through the library manager of the Arduino IDE.