user61182: 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);
}


user7508: 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.

61182: <p>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.</p>

<p>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.</p>

<p>The function I'm using  is as follows.</p>

<pre class="lang-cpp prettyprint-override"><code>void chead() {

  //---- X-Axis
  Wire.beginTransmission(Magnetometer); // transmit to device
  Wire.write(Magnetometer_mX1);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1);
  if(Wire.available()&lt;=1)
  {
    mX0 = Wire.read();
  }
  Wire.beginTransmission(Magnetometer); // transmit to device
  Wire.write(Magnetometer_mX0);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1);
  if(Wire.available()&lt;=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()&lt;=1)
  {
    mY0 = Wire.read();
  }
  Wire.beginTransmission(Magnetometer); // transmit to device
  Wire.write(Magnetometer_mY0);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1);
  if(Wire.available()&lt;=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()&lt;=1)
  {
    mZ0 = Wire.read();
  }
  Wire.beginTransmission(Magnetometer); // transmit to device
  Wire.write(Magnetometer_mZ0);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1);
  if(Wire.available()&lt;=1)
  {
    mZ1 = Wire.read();
  }

  //---- X-Axis
  mX1=mX1&lt;&lt;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&lt;&lt;8;
  mY_out =mY0+mY1;
  Ym = mY_out*0.00092;

  //---- Z-Axis
  mZ1=mZ1&lt;&lt;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 &lt;0) heading += 2*PI;
  // Correcting due to the addition of the declination angle
  if(heading &gt; 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 &gt;= 0 &amp;&amp; headingFiltered &lt;= 89){
    c_head=map(headingFiltered,0,90,270,359);
  }
  if(headingFiltered &gt; 89 &amp;&amp; headingFiltered &lt;= 204){
    c_head=map(headingFiltered,90,204,0,90);
  }
  if(headingFiltered &gt; 204 &amp;&amp; headingFiltered &lt;= 290){
    c_head=map(headingFiltered,205,290,91,180);
  }
  if(headingFiltered &gt; 290){
    c_head=map(headingFiltered,291,359,181,269);
  }

  //Further calibration
  if(c_head&gt;=0 &amp;&amp; c_head&lt;8){
    c_head=352+c_head;
  }
  if(c_head&gt;=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);
}
</code></pre>
 7508: <p>The problem comes from the low-pass filter you are using:</p>
<pre class="lang-cpp prettyprint-override"><code>// Smoothing the output angle / Low pass filter
headingFiltered = headingFiltered*0.85 + headingDegrees*0.15;
</code></pre>
<p>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.</p>
<p>I can think of three different options for solving this problem.</p>
<h2>Option 1: filter the Cartesian coordinates</h2>
<p>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:</p>
<pre class="lang-cpp prettyprint-override"><code>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);
}
</code></pre>
<p>Then you would replace</p>
<pre class="lang-cpp prettyprint-override"><code>heading = atan2(Zm, Xm);
</code></pre>
<p>by</p>
<pre class="lang-cpp prettyprint-override"><code>heading = filtered_angle(Zm, Xm);
</code></pre>
<h2>Option 2: filter the unwrapped heading</h2>
<p>Another possible approach is to <em>unwrap</em> the heading before
filtering it. This is similar to <a href="https://ccrma.stanford.edu/%7Ejos/filters/Phase_Unwrapping.html" rel="nofollow noreferrer">phase unwrapping</a>: 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:</p>
<pre class="lang-cpp prettyprint-override"><code>// 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 &gt;= M_PI)
        revolutions--;
    else if (delta &lt; -M_PI)
        revolutions++;
    return 2*M_PI*revolutions + angle;
}
</code></pre>
<p>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 <code>fmod()</code> if you wish.</p>
<h2>Option 3: wrap the heading differences within the filter</h2>
<p>The previous idea can be applied backwards: instead of unwrapping the
heading, you can wrap the heading <em>differences</em> 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.</p>
<p>Here is a low-pass filter based on this idea, that is aware of how
angles are supposed to wrap:</p>
<pre class="lang-cpp prettyprint-override"><code>const float filter_constant = 0.15;

// Wrap an angle to [-pi, pi).
float wrap(float angle)
{
    while (angle &lt; -M_PI) angle += 2*M_PI;
    while (angle &gt;= 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;
}
</code></pre>
<p>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 <code>static</code> variable). Also, you have
implemented something very similar to the <code>wrap()</code> function above, in
order to wrap the heading to [0, 2π). You could instead reuse
<code>wrap()</code> as:</p>
<pre class="lang-cpp prettyprint-override"><code>// Wrap to [0, 2 pi).
heading = wrap(heading - M_PI) + M_PI;
</code></pre>
<p><strong>Edit</strong>: There is a recently published Arduino library that implements
this third option. It “provides a class, <code>runningAngle</code>, 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 <a href="https://github.com/RobTillaart/runningAngle" rel="nofollow noreferrer">runningAngle</a> and is available through the library
manager of the Arduino IDE.</p>
