Putting GPS and Compass on the same circle


I want to have my robot go from point A to B (GPS coordinates). It will have a compass for direction. But I cannot seem to get the compass bearing and GPS bearing to agree with each other. GPS A is current location of robot and GPS B is point given to robot ahead of time.

For GPS bearing, I use this:

GPS_heading = (180 / M_PI) * (atan2(sin(GPS_B_long - GPS_A_long)*cos(GPS_B_lat), cos(GPS_A_lat)*sin(GPS_B_lat) - sin(GPS_A_lat)*cos(GPS_B_lat)*cos(GPS_B_long - GPS_A_long)));

For compass code, I used this:

float heading = atan2(event.magnetic.y, event.magnetic.x);
 float declinationAngle = 0.22;  // this will vary based on your GPS location
  heading += declinationAngle;
  // Correct for when signs are reversed.
  if(heading < 0)
    heading += 2*PI;
  // Check for wrap due to addition of declination.
  if(heading > 2*PI)
    heading -= 2*PI;
  // Convert radians to degrees for readability.
 // float headingDegrees = (heading * 180/M_PI); 
  headingDegrees = (heading * 180/M_PI);

When I compass results, there are about 60 degrees off (compass bearing is 60 degs > than GPS bearing.

Any ideas?

Have you calibrated the compass? If not, see http://www.bot-thoughts.com/2011/04/quick-and-dirty-compass-calibration-in.html Better: http://sailboatinstruments.blogspot.com/2011/08/improved-magnetometer-calibration.html

Well, I did calibrate it, and changed some things around. Compass reads +/- 3 degs of raw hand compass reading at each of the poles now.

I took 3 sample GPS points around me with line of sight, and pointed the compass directly at each one. Off of True North, I used 88 degs, 136 degs, 257 degs. So basically 3 points, spaced out around a circle.

I noticed that for each of these, the compass reading was +45 degs more than the calculated GPS reading.

I'm wondering if I should just scaled the compass reading -45 degrees in my code. I don't really know why it is off, but if it's a constant error, it seems to make sense.


I think you should describe in detail what you did to calibrate the compass, and post the code you are using to read the compass and display the reading.

Here is what I did (and all of these results are on the attached image):

I placed the (raw) digital compass pointed in the same direction as my hand compass, and took the x.y readings at 0, 90, 180, 270.

Then, I looked at the offsets for the x readings at 0 and 180, and again at 90 and 270. (I did the same for the y.)

I then took the absolute value the readings, and took the average these results to arrive at an offset. In pseudo code as an example:

x-value = [abs(x_180) – abs(x_0)] / 2
y-value = [abs(y_180) – abs(y_0)] / 2

(1) x-dir is more neg than positive, x_180 = -34.82 vs. x_0 = 27.36, thus need to ADD to event.x.
(2) At y_180, y_0, both are neg.

90, 270
x-value = [abs(x_270) – abs(x_90)] / 2
y-value = [abs(y_270) – abs(y_90)] / 2

(1) y-dir is more neg than positive, y_270 = -26.73 vs. y_90 = 22.36, thus need to ADD to event.y.
(2) (At x_270, x_90, both are neg.)

For the magnetic offset correction amounts, I noticed that offset value in the x-direction is somewhere between 0.275 – 3.73, so I did 3.73 – 0.275 = 3.455

I did similar for the y-direction, noticing that the value was between 0.14 – 2.185, so 2.185 – 0.14 = 2.045.

Summary for calibrating:
event.magnetic.y = event.magnetic.y + 2.045
event.magnetic.x = event.magnetic.x + 3.455

As you can see from the column, “Calibrated”, after the calibration, I redid the measurements at the 0, 90, 180, 270 hand compass markings, and the results appeared closer to what the compass read.

Only then after this, did I use the angle of declination. Being on the east coast of the USA, my angle of declination is negative, so converted this to radians, and updated my compass readings with it.

Then, this was converted to degrees for readability on a circle.

Here are compass code parts:

// Compass Libraries
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>

// Math Library
#include <math.h>

// I2C - Compass
/* Assign a unique ID to this sensor at the same time */
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);

             Compass Functions
int compass_delay = 1000; // delay time for compass readings output
float headingDegrees;

void displaySensorDetails(void)
  sensor_t sensor;
  Serial.print  ("Sensor:       "); Serial.println(sensor.name);
  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" uT");
  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" uT");
  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" uT");  

void compass(void)
   /* Get a new sensor event */ 
  sensors_event_t event; 

  // Display the results (magnetic vector values are in micro-Tesla (uT)) 
  Serial.print("X: "); Serial.print(event.magnetic.x); Serial.print("  ");
  Serial.print("Y: "); Serial.print(event.magnetic.y); Serial.print("  ");
  Serial.print("Z: "); Serial.print(event.magnetic.z); Serial.print("  ");Serial.println("uT");

  // Hold the module so that Z is pointing 'up' and you can measure the heading with x&y
  // Calculate heading when the magnetometer is level, then correct for signs of axis.
    // Note:  This has been calibrated

float heading = atan2(event.magnetic.y + 2.045, event.magnetic.x + 3.455);
  // Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
  // Find yours here: http://www.magnetic-declination.com/ (Note - NOT USED)
       // NOTE:  I used this website:  http://www.ngdc.noaa.gov/geomag-web/#declination

float declinationAngle = (M_PI/180) * (- 10.85);  // this will vary based on your GPS location and be in Radians
                                                // this converts Degrees to Radians

  heading += declinationAngle;  
  // Correct for when signs are reversed.
  if(heading < 0)
    heading += 2*PI;
  // Check for wrap due to addition of declination.
  if(heading > 2*PI)
    heading -= 2*PI;
  // Convert radians to degrees for readability.
 // float headingDegrees = (heading * 180/M_PI); 
  headingDegrees = (heading * 180/M_PI);

  Serial.print("Compass Heading (degrees): ");   

compass- raw vs calibrated.jpg

I don't really follow your reasoning. What compass is this? Does it provide bearing estimates, or magnetic field strengths along the axes? If the former, then the approaches I suggested won't work.

If the latter, you should not take the absolute value of the readings to compute the offsets, and you need to take into account a possible difference in gain along axes.

If you follow the procedure described here http://www.bot-thoughts.com/2011/04/quick-and-dirty-compass-calibration-in.html


Xoffset = (Xmax+Xmin)/2 Yoffset = (Ymax+Ymin)/2 (Ygain/Xgain) = (Ymax-Ymin)/(Xmax-Xmin)

and the corrected readings become

Xcorr = (X-Xoffset) Ycorr = (Y-Yoffset)*(Xgain/Ygain)

Ok, I’ve made some changes. The compass I’ve been using uses the Honeywell HMC5883L chip:

Anyhow, the compass does give magnetic field strengths along the axes (x,y,z). To get the raw data (which is attached - updated), I measured the x,y readings by rotating the compass clockwise alongside a hand compass at 0,90,180,270. The results are shown in the attached image with the raw x,y readings at each turn.

So using your suggested method, I came up with these results:

Xoffset = (Xmax+Xmin)/2 = (27.36 + -34.82) / 2 = -3.73

Yoffset = (Ymax+Ymin)/2 = (22.36 + -26.73) / 2 = -2.185

(Ygain/Xgain) = (Ymax-Ymin)/(Xmax-Xmin) = (22.36 - -26.73) / (27.36 - -34.82) = (49.09 / 62.18) = 0.789482149

and the corrected readings become

Xcorr = (X-Xoffset) = X - -3.73 = X + 3.73
Ycorr = (Y-Yoffset)(Xgain/Ygain) = (Y–2.185)(62.18/49.09) = (Y + 2.185)*(1.26665)

Here is my code for the heading (without declination shown):

/*  NOTE: Below values are referenced from excel spread sheet of RAW values at poles
Xoffset = (Xmax+Xmin)/2
Yoffset = (Ymax+Ymin)/2
(Ygain/Xgain) = (Ymax-Ymin)/(Xmax-Xmin)

and the corrected readings become

Xcorr = (X-Xoffset)
Ycorr = (Y-Yoffset)*(Xgain/Ygain)
 float heading = atan2(((event.magnetic.y - (-2.185))*(62.18/49.09)), event.magnetic.x + 3.73);

A question is in the formula on this line,

Ycorr = (Y-Yoffset)*(Xgain/Ygain)

Was this correct? I’m only asking because it says to compute the (Ygain/Xgain) earlier, which is not used in the calculations.

or did you mean:

Ycorr = (Y-Yoffset)*(Ygain/Xgain)

I HOPE the former as you said is correct, because the corrections you provided here get my REAL close to the GPS heading, ~5 deg off!


raw x and y values of digital compass.jpg

Xgain/Ygain = 1/(Ygain/Xgain), which you should do using a calculator. It was written this way to exchange a multiplication for a division, which is much slower on the Arduino.

Are you taking into account magnetic declination, or not?

i'm using magentic declination.

but WOW. I used what you gave (1st example), and I'm getting sometimes to 2% difference between compass heading and GPS heading. I can't believe it! Seriously, thank you so much! This really means a lot to me as I've been working on this for a while, but couldn't find the proper calibration method.

I looked at the website you provided, but couldn't find the equations you listed. Where is it listed?

Those equations don’t seem to be listed on the bot-thoughts.com page, but they follow from some very simple considerations and a little algebra.

The more complex procedure described here Sailboat Instruments: Improved magnetometer calibration (Part 1) works much better. With proper calibration and no nearby hard and soft iron interference, magnetometers of recent vintage are capable of about 1 degree accuracy.