Navigating with Magnetometers: Electronic Compass Headings in C++, Python, and MATLAB

Compass headings can be essential for many autonomous mobile robots, and magnetometers such as those included in the popular LSM303, MPU-6050, and BNo055 Inertial Measurement Units (IMUs) are commonly used for this task. Magnetometers are sometimes called a digital compass, but this is a bit misleading as there is more to it than that. This article aims to help you get the function of a digital compass out of a plain old magnetometer.

Magnetometers measure the Earth's magnetic field, allowing us to calculate a compass heading for our mobile robots or other applications based on these measurements. This article assumes you have the sensor data and just need to calculate headings with it. If you aren't quite there yet or want to learn more, my book, "Practical Robotics in C++," provides an in-depth guide to working with sensors, including sample programs. You can also find sensor and serial data tutorials at the Practical Robotics YouTube channel.

Products Mentioned in this article:

LSM303 Inertial Measurement Unit

MPU-6050 Inertial Measurement Unit

BNo055 Absolute Orientation Sensor

Practical Robotics in C++

Disclaimer: Please note that the products linked on this page are not sponsored, but as an Amazon Associate I earn from qualifying purchases.

While the math to get a heading from raw magnetometer data is straightforward, there are some variations for many applications that makes it impossible to give a simple formula or chunk of code and send you on your way. Generally, though, these are the steps to calculate a heading:

• Get magnetometer data for at least the X and Y axes

• Maybe negate the Y value, maybe not

• Use trig to calculate the direction of the magnetic field vector

• Apply a correction to align with your coordinate convention

• Compensate for magnetic declination

We'll talk about each of these steps in this article and add a couple more in the future to compensate for less than perfect conditions.

Some Background Info

The true north and south poles refer to the geographic points located at the top and bottom of the earth's rotational axis, respectively. These points mark the earth's axis of rotation and are used as a reference point for navigation and mapping purposes. On the other hand, the magnetic north and south poles are determined by the Earth's magnetic field, which is generated by the movement of molten iron in the planet's core. The magnetic north and south poles are not located at the same place as the true north and south poles. In fact, the Earth's magnetic north pole is currently located in northern Canada, while the magnetic south pole is located off the coast of Antarctica. Lastly, it's worth noting that what is commonly referred to as magnetic north is actually the location of the earth's magnetic south pole. This can be a bit confusing, but it's because the Earth's magnetic field lines emerge from the magnetic south pole and converge at the magnetic north pole. Figure 1 is my attempt at illustrating this stuff.

Figure 1. The earth's true and magnetic poles.

Magnetometers measure the strength of the magnetic field, and we can determine the direction of the field if we measure in the X, Y, and Z axes at once (most magnetometers do this). For heading, we are concerned with the direction of the horizontal component of that direction, so we will start with using just the X and Y measurements. This works fine if the sensor is more or less level, but we will need to build on this method and include Z data later on to compensate for tilt. Figure 2 shows a magnetometer placed with its y-axis aligned with the Earth's magnetic field.

Figure 2. Visualizing the earth's magnetic field measured with a magnetometer.

The strength of the field on the x-axis is nearly zero because it is perpendicular to the field, and the strength of the field in the y-direction is either at its maximum or minimum (depending on whether it's facing North or South and how near or far you are from the poles). Whether the reading is positive or negative, we can say that the absolute value of the y measurement is at its maximum.

If the sensor is rotated so the x-axis is pointing north, the absolute value of x-axis measurement will increase to the same value and the absolute value of y-axis measurement will decrease until it reaches zero. If the wording above feels clunky or unclear, just takeaway three key things:

• As it the sensor rotates, the strength of the field is always increasing on one axis and decreasing on the other.

• The measurement will be essentially zero for an axis facing East or West. ("Essentially" because we must expect sensor noise and imperfections)

• The measurement will be at its maximum absolute value for an axis that is facing North or South. The value will be equal but opposite facing the opposite direction.

Thanks to these predictable characteristics, we can easily use the trigonometry arctangent function to calculate the angle between the sensor's axes and the earth's magnetic field. Without further delay, lets dive into why you really came here and work the steps I listed above.

Get Magnetometer Data

As I mentioned earlier, getting data from your sensor is outside the scope of this article. The rest of this article assumes you have readings called X, Y, and Z. Your sensor might return values in either Tesla or Gauss, but because the calculations rely on the ratios and not the absolute values it does not matter which for our purposes. I will note that ROS convention is to use Tesla.

Maybe Negate the Y Value or Maybe Not

The direction of the earth's magnetic field can sometimes be significantly vertical and even cause a Y value to read negative when the sensor is pointed north (or tilted just little). The same sensor further south could read a positive value when pointed north. I have so far not found a definitive resource or map that indicates in a simple way where a person might need to negate that Y value, but I have found that Michigan, USA is far enough north and in a region where it is necessary. The best advice I have is to complete the calculations without negating Y, then do some tests. Heading values should increase when turning the sensor counter clockwise and decrease when turning clockwise. If this is not the case, try negating Y for the arctangent step below.

Arctangent: atan() vs atan2()

Most programming languages I've used have two arctangent functions we can use to calculate the angle between the x axis of the magnetometer and magnetic South. The first is atan(y/x), which only takes one argument (y/x is just a single value) and can only give a result in the first or fourth quadrant of the unit circle (from -pi/2 to pi/2). This requires extra code to determine the correct quadrant and heading so I suggest using atan2(y, x) as it uses y and x separately to calculate for all four quadrants, providing a result between -pi and pi. Quite simply, let's use:

magneticHeading = atan2(Y, X)
//or
magneticHeading = atan2(-Y, X)

Note that the parameter order for atan2 is y before x every programming languag I've used it in (C++, Java, Matlab, and Python). Also don't forget that we use radians instead of degrees in robotics and trigonometry, but they can be easily converted using the formula degrees = radians * (180/pi) if needed.

Apply a correction to align with your coordinate convention

The result from the atan2() function is an angle between the magnetic field lines and the x axis of the magnetometer. However, to use this angle as a compass heading in a specific application, we need to adjust it to match the convention used by that application. For example, in ROS (Robot Operating System), the convention for compass headings is to use an angle between the positive y-axis and the direction of magnetic North. To convert the angle from atan2() to this convention, we need to add 90 degrees (or pi/2 radians) to the result.

magneticHeading += M_PI / 2 

Now we just need to ensure that the result stays within the bounds of -PI to +PI:

while (magneticHeading > M_PI) 
    magneticHeading -= 2 * M_PI
while (magneticHeading <= -M_PI) 
    magneticHeading += 2 * M_PI

Compensate for magnetic declination

There is a difference between the magnetic axis of the Earth and it's axis of rotation. Maps typically adhere to rotational axis the truth, but our calculations so far are relative to the magnetic axis. The difference is measured as a declination angle, which varies in different parts of the world. In Detroit, Michigan the declination is about -8 degrees or -0.139 radians. We simply add this to our calculated magnetic heading to align with the Earth's rotational axis and thus, with most maps, GPS coordinates, etc.

declination = -0.139
trueHeading = magneticHeading + declination

Declination varies a LOT just around the United States, so be sure to ask Google for the declination value for your location. It's probably in degrees so don't forget to convert it to radians.

Putting it all together: The C++, Python, and MATLAB code

All of my rambling on so far can be summarized with this formula:

heading = magnetic_field_direction + convention_offset + declination

Assuming you already have variables called X and Y populated with magnetometer data, the following code blocks put it together:

C++

#include <iostream>
#include <cmath>

//radians. Location specific.
double declination = -0.139;
// adjustment needed for ROS convention
double convention_offset = -1.57;

// calculate magnetic field direction
double magnetic_field_direction = atan2(-Y, X);

// calculate true heading
double trueHeading = magnetic_field_direction + convention_offset + declination;

// ensure result is in bounds of -PI to +PI
while (trueHeading < -M_PI) 
    trueHeading += 2.0 * M_PI;
while (trueHeading > M_PI)
    trueHeading -= 2.0 * M_PI;

Python

import math

# radians. Location specific.
declination = -0.139
# adjustment needed for ROS convention
convention_offset = -1.57

# calculate magnetic field direction. Negate Y if necessary
magnetic_field_direction = math.atan2(Y, X)

# calculate true heading
trueHeading = magnetic_field_direction + convention_offset + declination

# ensure result is in bounds of -PI to +PI
trueHeading = (trueHeading + math.pi) % (2*math.pi) - math.pi

MATLAB

% radians. Location specific.
declination       = -0.139
% adjustment needed for ROS convention
convention_offset = -1.57

% calculate magnetic field direction. Negate Y if necessary
magnetic_field_direction = atan2(Y, X)

% calculate true heading
trueHeading = magnetic_field_direction + convention_offset + declination

% ensure result is in bounds of -PI to +PI
trueHeading = wrapToPi(trueHeading);

Why are my headings incorrect?

Compass headings can be calculated using magnetometer data, but it is important to note that the data may be distorted by nearby magnets or iron objects, as well as the sensor itself. If the disturbance is fixed in relation to the sensor (like parts of the robot itself), calibration can be used to measure and cancel out the effect. I will show you how to easily do this in my very next article, again in C++, Python, and MATLAB.

It is also important to know that the result is only reliable if the sensor is relatively flat. Compensating for tilt will be the topic of the final article in this series.

Peace out

I hope you found this article helpful in calculating compass headings using magnetometers. Remember, while the math may seem straightforward, there are variations and nuances to consider based on your application. Don't hesitate to seek additional resources such as our book "Practical Robotics in C++" and tutorials on the Practical Robotics YouTube channel. Thanks for reading and keep exploring and experimenting to optimize your robot's navigation!