Introduction
Finding your geographic location and direction is useful in robotics and embedded systems. Using GPS and a Compass Sensor it's easy to determine your location and your direction in relation to Magnetic North. But what if you want to know you direction in relation to True North?
It's not difficult to determine your offset from Magnetic North to True North, also known as Magnetic Declination. Magnetic Declination can be easily found on the web through a NOAA website. But I wanted a real time model that I could use in my embedded device through code, enter the World Magnetic Model (WMM)! Using GPS with the WMM in Linux can easily determine Magnetic Declination that you can then use to offset the Magnetic Bearing from your Compass. I will take you through the steps to determine Magnetic Declination and use it to adjust your compass bearing.
Background
I'm a LEGO maker and a winner in a LEGO prototype challenege. I have been working on a star finding camera robot made out of LEGO and Raspberry Pi components. To determine stellar coordinates I first need to know my robot's geographic coordinates and which direction it's facing in relation to true North with a reasonable degree of accuracy. Setting up the GPS and Compass was fairly straightforward and installing and accessing the World Magnetic Model was also fairly simple.
This is my first CodeProject article and my robot is still a work in progress but has made it a long way. I wanted to share some of the work I've already done with the community. I hope you like it!
You can find more images of my robot and my other LEGO devices at these links.
Starbot (Lego Camera)
Lego Digital Picture Frame
Point & Shoot IR Camera
Lego Computer
Components Used
You'll need to setup the following devices, instructions for setup are included in the links below. I will not cover these topics in detail so you'll need to be somewhat familiar with Raspberry Pi setup and Digital and I2C sensors in linux.
1 Raspberry Pi 2 Model B running Raspbian.
1 Adafruit Ultimate GPS Breakout (Setup Instructions)
1 HMC5883L Triple Axis Magnetometer (Setup Instructions)
Wiring Diagram
Required Libraries
You'll need a few libraries installed on your Raspberry Pi, these will give you access to your GPS and Magnetic Models through code. The compass does not require additional libriaries but it does use i2c-dev.h.
GPSd (Setup Instructions) (Access to GPS data using libgpsmm.h)
GeographicLib (Setup Instructions) (Access to WMM data and additional Geographic related algorithms)
Reading GPS Coordinates
The first step is to read the GPS coordinates from your device. Using GPSd and libgpsmm.h make it pretty simple to access your GPS data. We're going to make a class to manage this interface and supply us with values streamed from the device. The GPS uses a serial connection and the sentences returned are parsed for you by libgps. We can either use the raw coordinate values returned or they can be broken into components like degrees, minutes and seconds.
The GPS Class
I created a simple wrapper class to manage interactions with the GPS library and do coordinate conversions between raw and component values.
#ifndef __GPS_H_INCLUDED__
#define __GPS_H_INCLUDED__
#include "libgpsmm.h" // GPS
class gps
{
private:
gpsmm *gps_rec;
int degrees(float coordinate);
int minutes(float coordinate);
float seconds(float coordinate);
public:
int gps_fix;
float gps_lon;
float gps_lat;
float gps_alt;
int gps_sat;
int gps_use;
bool start();
bool update();
bool stop();
int latitude_degrees();
int latitude_minutes();
float latitude_seconds();
int longitude_degrees();
int longitude_minutes();
float longitude_seconds();
};
#endif
Initializing GPS Stream
Now that your class is setup you'll need to initialize your GPS stream. It's only a few lines of code, you'll be able to modify a few options here, GPS device address and stream formatting, but I used the defaults. You can do additional research on the GPSd site to determine how to use the parameters.
bool gps::start()
{
gps_rec = new gpsmm("localhost", DEFAULT_GPSD_PORT);
if (gps_rec->stream(WATCH_ENABLE | WATCH_JSON) == NULL)
{
return false;
}
return true;
}
Reading GPS Stream
Now that your device is setup you can read the values from the stream. This is also pretty simple to do, I created a function called update() that is then called once per frame of my main control loop. We'll use a data structure called gps_data_t provided by libgpsmm.h to store the raw values and then store them in our class level variables. gps_data_t returns a lot of values, you may wish refer to the online documentation to see if there are additional values you may need to use for your purpose.
bool gps::update()
{
struct gps_data_t* gps_data;
if (gps_rec->waiting(00050000))
{
if ((gps_data = gps_rec->read()) == NULL)
{
return false;
}
if (gps_data->status == STATUS_FIX &&
(gps_data->fix.mode == MODE_2D || gps_data->fix.mode == MODE_3D))
{
gps_fix = gps_data->fix.mode;
gps_lon = gps_data->fix.longitude;
gps_lat = gps_data->fix.latitude;
gps_alt = gps_data->fix.altitude;
gps_sat = gps_data->satellites_visible;
gps_use = gps_data->satellites_used;
}
}
return true;
}
Shutting Down GPS
When you're done you need to shutdown your stream and release your resources.
bool gps::stop()
{
if(gps_rec->stream(WATCH_DISABLE) == NULL)
{
delete gps_rec;
return false;
}
delete gps_rec;
return true;
}
That's it, as you can see it's pretty simple to setup and read coordinates from your GPS device. Now that you have raw coordinate values you may want to parse the component values from the decimal representation. The raw values represents the Degrees, Minutes, and Seconds components as a single decimal value.
Parsing Degrees
Degress are represented as the whole number part of the raw value, in order to determine the degress just truncate the floating point values. Here I just cast to an int and return the absolute value.
int gps::degrees(float angle)
{
return abs((int)angle);
}
Parsing Minutes
Minutes are one sixtyth of one degree. Once you remove you degree component from your raw value you're left with minutes and seconds. To convert to minutes you'll need to multiply by 60 and then truncate the floating point values to drop the seconds.
int gps::minutes(float angle)
{
int deg = degrees(angle);
int min = (int)((fabs(angle) - deg) * 60.00);
return min;
}
Parsing Seconds
Seconds are one sixtyth of one minute. To get the minutes you'll need to do two steps, First you remove the degrees from the raw coordinate value, next you perform the step to convert to minutes. Now that you have minutes you can deduct the whole value and again mulitply by 60, this will leave you with your seconds component. Seconds can be returned as a decimal value with fractional values.
float gps::seconds(float angle)
{
int deg = degrees(angle);
int min = minutes(angle);
float sec = (((fabs(angle) - deg) * 60.00) - (float)min) * 60.00;
return sec;
}
Determining East vs West and North vs South
There is an additional component encoded into the raw value of your gps coordinates. This value determines East/West for Longitude and North/South for Latitude. This is encoded into the sign bit of the raw GPS value. if your original value is negative this will indicate West/South and if it is positive it will indicate East/North.
gps_lat >= 0 ? 'N' : 'S'; gps_lon >= 0 ? 'E' : 'W';
Example Data
Decimal Latitude: -15.063888888888888
Converted Latitude: 15° 3' 50" W
Decimal Longitude: 45.33416666666667
Converted Longitude: 45° 20' 3" N
World Magnetic Model and Declination
Now that we have our GPS coordinates the next step is to determine our robot's Magnetic Declination. We can do this with GeographicLib and the World Magnetic Model but first we need to understand the concept of Magnetic Declination.
Declination
Quote: https://en.wikipedia.org/wiki/Magnetic_declination
Magnetic declination or variation is the angle on the horizontal plane between magnetic north (the direction the north end of a compass needle points, corresponding to the direction of the Earth's magnetic field lines) and true north (the direction along a meridian towards the geographic North Pole). This angle varies depending on position on the Earth's surface, and changes over time.
As you can see Magnetic declination is simply the angle of offset between Magnetic North and True North. This value changes with position and time. It's important to make sure to refresh this value since it could vary between frames in your control loop (It's unlikely but possible). There is also a similar concept called inclination, this determines the angle of the magnetic field on the vertical plane but we don't need this to determine our offset from Magnetic North.
Magnetic Model
To determine Magnetic Declination we need to be able to understand the magnetic field lines at our robot's location. From the description we know that Magnetic Declination can change with position and time. So how would we know what our actual declination is if it's constantly changing? The World Magnetic Model is the answer, it is a precomputed data set that is provided by NOAA and can be consumed by GeographicLib. The datasets provided are usually good for a few years at a time and need to be updated with regularity to ensure accurate information.
You can find more information and useful tools at the NOAA website.
Using GeographicLib
I created a simple wrapper around the GeographicLib library similar to the GPS wrapper. This has properties for the values we wish to query, such as declination and inclination. This code is actually easier to use then the GPS component and does not require a start() or stop() step.
#ifndef __WMM_H__
#define __WMM_H__
#include <GeographicLib/MagneticModel.hpp> // Magnetic Model
using namespace GeographicLib;
class wmm
{
private:
double magnetic_declination;
double magnetic_inclination;
double field_strength;
public:
double declination();
double inclination();
double strength();
void update(float lat, float lon, float alt);
};
#endif // !__WMM_H__
Getting Declination
We'll need to use the latitude, longitude and altitude returned by GPS Module for our update(...) function. This will give us a 3D position to determine the local magnetic field produced by the Earth. Additionally we also need to determine the current date and time to return accurate Magnetic Model information for the current moment.
void wmm::update(float lat, float lon, float alt)
{
double Bx, By, Bz, H;
time_t t = time(NULL);
tm* timePtr = localtime(&t);
MagneticModel mag("emm2015");
mag(timePtr->tm_year + 1900, lat, lon, alt, Bx, By, Bz);
MagneticModel::FieldComponents(Bx, By, Bz, H,
field_strength, magnetic_declination, magnetic_inclination);
}
Reading Compass Bearings
Our compass uses the i2c interface and does not require a library outside of i2c-dev. This makes it a little more complicated to setup than our GPS but not by much. The value returned should be the local Magnetic Bearing. This is the value we will need to offset to determine the bearing for True North. Magnetometers are prone to noise and may not return a consistent value over time due to magnetic interference from local magnetic fields. This can be due to other metals or electronics near your sensor or in my case the length of my jumper wires. You will need to ensure that your compass sensor is as isolated from random magnetic fields as possible.
Compass Class
The compass class is pretty simple, it interacts with i2c and then filters the values using a Kalman filter for smoother output.
#ifndef __COMPASS_H__
#define __COMPASS_H__
#include "kalman.h"
#define HMC5883L_I2C_ADDR 0x1E
class compass
{
private:
int i2c_fd;
bool select_i2c_device(int fd, int addr, char * name);
bool write_to_i2c(int fd, int reg, int val);
public:
float bearing;
int start();
int update();
};
#endif
Initializing the Compass
To initialize the compass we'll first need to select our i2c device and then write some data to the bus using the following select_i2c_device(...) and write_to_i2c(...) functions.
bool compass::select_i2c_device(int fd, int addr, char * name)
{
if (ioctl(fd, I2C_SLAVE, addr) < 0) {
return false;
}
return true;
}
bool compass::write_to_i2c(int fd, int reg, int val)
{
char buf[2];
buf[0]=reg;
buf[1]=val;
if (write(fd, buf, 2) != 2) {
return false;
}
return true;
}
Now in our start() function we can call the previous i2c functions to setup our device.
int compass::start()
{
init = true;
if ((i2c_fd = open("/dev/i2c-1", O_RDWR)) < 0)
{
return 0;
}
select_i2c_device(i2c_fd, HMC5883L_I2C_ADDR, "HMC5883L");
write_to_i2c(i2c_fd, 0x01, 32);
write_to_i2c(i2c_fd, 0x02, 0);
return 1;
}
Reading Compass Bearing
The compass should now be ready to read from. Like the GPS and WMM we want to keep updating our sensor value in case our position or orientation change, similar to the other components we will use the update() function.
int compass::update()
{
float angle = 0;
unsigned char i2c_buf[16];
i2c_buf[0] = 0x03;
if ((write(i2c_fd, i2c_buf, 1)) != 1)
{
return 0;
}
if (read(i2c_fd, i2c_buf, 6) != 6)
{
return 0;
}
else
{
short x = (i2c_buf[0] << 8) | i2c_buf[1];
short y = (i2c_buf[4] << 8) | i2c_buf[5];
angle = atan2(y, x) * 180 / M_PI;
}
bearing = angle;
return 1;
}
Here you see us reading from the i2c buffer and then converting the byte values into an angle. You'll also see that we're using something called a Kalman filter. This will reduce the amount of noise returned by the compass sensor (More on this in the next section).
Filtering Compass Values
The compass values are inherently noisy, due to interference from local electromagnetic sources and physical motion. You may notice that your value does not remain constant after subsequent reads, though it should be fairly consistent without large jumps between values. However, it's better to smooth these values over time for a cleaner reading. There are multiple filters that could be used but I chose a simple kalman filter the original implimentation can be found here.
Quote: https://en.wikipedia.org/wiki/Kalman_filter
Kalman filtering, also known as linear quadratic estimation (LQE), is an algorithm that uses a series of measurements observed over time, containing statistical noise and other inaccuracies, and produces estimates of unknown variables that tend to be more precise than those based on a single measurement alone, by using Bayesian inference and estimating a joint probability distributionover the variables for each timeframe. The filter is named after Rudolf E. Kálmán, one of the primary developers of its theory.
Filtering
Below is a graph that shows how the filtered values look in relation to the raw values. The darker line represents the noisy value returned from the sensor and the lighter line represents the smoothed values. As you can see the smoothed values will be more consitent than the unfiltered values. The values returned by filtering tend to be more uniform over time, it's important to keep in mind that we're making an prediction about where our value should be and it can slightly lag behind your raw value. This means that it might take slightly longer to get a "fix" on your target value while processing.
Kalman.h
There was no need to make a wrapper class for this since we're just storing a few values and then updating them. We have a struct that stores our state and two functions a kalman_init(...) and a kalman_update(...)
#ifndef __KALMAN_H__
#define __KALMAN_H__
typedef struct {
float q; float r; float x; float p; float k; } kalman_state;
kalman_state kalman_init(float q, float r, float p, float x);
void kalman_update(kalman_state *s, float m);
#endif // __KALMAN_H__
Initializing the Kalman Filter
It's pretty simple to initialize, we create a new kalman state struct and then just set the values of the state to those passed into the function. We pass in a Process Noise Covariance (q), Measurement Noise Covariance (r), Estimation Error Covariance (p), and an initial value (x)
kalman_state kalman_init(float q, float r, float p, float x) {
kalman_state s;
s.q = q;
s.r = r;
s.p = p;
s.x = x;
return s;
}
In our control loop we will initialize the kalman filter using the following lines of code. The values I chose here were estimates, I just adusted them until the values returned seemed reasonably smooth.
if(init)
{
state = kalman_init(0.025f, 16, 1, compass_sensor->bearing);
init = false;
}
Updating the Kalman Filter
To do an update, we pass in the previous state and the new measured value from the compass. First we need to make a prediction based on earlier states and then we update the measurement values.
void kalman_update(kalman_state * s, float m) {
s->p = s->p + s->q;
s->k = s->p / (s->p + s->r);
s->x = s->x + s->k * (m - s->x);
s->p = (1 - s->k) * s->p;
}
Finally, in our main control loop we call the update function for the kalman filter everytime we update the compass.
kalman_update(&state, compass_sensor->bearing);
float filtered_bearing = state.x;
That's it, we're now ready to put it all together and get our final Bearing for True North.
Putting It Together
Once all of the components are ready we can now build our main control loop. In this example I'm just printing the values to screen using an infinite loop. You'll likely want to create a well timed control loop for your purposes.
Initialization
We start by creating and initializing the GPS, WMM, Compass and Kalman filter components. Next we call any start() methods before entering the main loop.
bool init = true;
kalman_state state;
gps * gps_sensor = new gps();
wmm * magnetic_model = new wmm();
compass * compass_sensor = new compass();
gps_sensor->start();
compass_sensor->start();
Control Loop
Once we enter the main control loop you just update the GPS sensor, pass longitude, latitude and altitude values to the world magnetic model. Now we read from the compass, intialize the kalman filter and then filter the values.
while (1)
{
gps_sensor->update();
magnetic_model->update(gps_sensor->gps_lat, gps_sensor->gps_lon, gps_sensor->gps_alt);
compass_sensor->update();
if (init)
{
state = kalman_init(0.025f, 16, 1, compass_sensor->bearing);
init = false;
}
kalman_update(&state, compass_sensor->bearing);
float filtered_bearing = state.x;
}
True North
Once we have obtained the filtered compass value we can then use it to determine the offset to our bearing using the following line of code. It's a very simple calculation, you just add the declination offset to your filtered bearing value.
float true_bearing = filtered_bearing + magnetic_model->declination();
Printing Values
I'm just printing the values to the screen in this demo. However, in my case I use these values to help control a servo motor to control the pan of my telescope. You would want to use these values to control your device instead of just printing values, this is where you would perform any additional steps needed.
printf("\033[0;0H");
printf("Latitude: %3d° %2d' %2.3f\" %c Longitude: %3d° %2d' %2.3f\" %c\n\r",
gps_sensor->latitude_degrees(),
gps_sensor->latitude_minutes(),
gps_sensor->latitude_seconds(),
gps_sensor->gps_lat >= 0 ? 'N' : 'S',
gps_sensor->longitude_degrees(),
gps_sensor->longitude_minutes(),
gps_sensor->longitude_seconds(),
gps_sensor->gps_lon >= 0 ? 'E' : 'W');
printf("Altitude: %2.3f Declination: %2.3f",
gps_sensor->gps_alt),
magnetic_model->declination());
printf("Magnetic Bearing: %2.3f Filtered Bearing %2.3f True Bearing %2.3f\n\r",
compass_sensor->bearing,
filtered_bearing,
true_bearing);
Cleaning Up
Once you exit the main loop you want to remember to stop the GPS and cleanup your components.
gps_sensor->stop();
delete gps_sensor;
delete compass_sensor;
delete magnetic_model;
Complete Example
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "wmm.h"
#include "compass.h"
#include "gps.h"
#include "kalman.h"
int main()
{
bool init = true;
kalman_state state;
printf("starting...\n\r");
gps * gps_sensor = new gps();
wmm * magnetic_model = new wmm();
compass * compass_sensor = new compass();
gps_sensor->start();
compass_sensor->start();
printf("\033[2J\033[?25l");
while (1) {
gps_sensor->update();
magnetic_model->update(gps_sensor->gps_lat, gps_sensor->gps_lon, gps_sensor->gps_alt);
compass_sensor->update();
if (init) {
state = kalman_init(0.025f, 16, 1, compass_sensor->bearing);
init = false;
}
kalman_update(&state, compass_sensor->bearing);
float filtered_bearing = state.x;
float true_bearing = filtered_bearing + magnetic_model->declination();
printf("\033[0;0H");
printf("Latitude: %3d° %2d' %2.3f\" %c\n\rLongitude: %3d° %2d' %2.3f\" %c\n\r",
gps_sensor->latitude_degrees(),
gps_sensor->latitude_minutes(),
gps_sensor->latitude_seconds(),
gps_sensor->gps_lat >= 0 ? 'N' : 'S',
gps_sensor->longitude_degrees(),
gps_sensor->longitude_minutes(),
gps_sensor->longitude_seconds(),
gps_sensor->gps_lon >= 0 ? 'E' : 'W');
printf("Altitude: %2.3f\n\r\n\rDeclination: %2.3f\n\r",
gps_sensor->gps_alt,
magnetic_model->declination());
printf("\n\rMagnetic Bearing: %2.3f\n\rFiltered Bearing: %2.3f\n\rTrue Bearing %2.3f\n\r",
compass_sensor->bearing,
filtered_bearing,
true_bearing);
}
gps_sensor->stop();
delete gps_sensor;
delete compass_sensor;
delete magnetic_model;
return 0;
}
Using the Code
If you've set up your hardware following the diagram and the software prerequisites at the begging of the article you can download and run the sample using the following bash commands.
git clone https://github.com/GalacticSoft/Starbot-Command.git
cd Starbot-Command/src
make clean
make demo
sudo ./demo
That's it, if the demo works you should see your coordinates printed to the screen like the screenshot below.
Now you're ready to start using geographic location and compass bearings with your robot or IoT device, enjoy!
