Code to calculate heading / bearing from two GPS latitude and longitude Points

/in /by

Example code to Estimate heading or course  of travel or bearing given two GPS locations taken from a recorded path.

A lot of GPS devices will report heading along with latitude and longitude, but what if you lust have a series of GPS locations recorded from a path, how can you estimate heading or course of travel at points along the path  given two consecutive GPS locations from the path?

Why might you want to know heading along a path?  Well imagine you have a mobile computer vision application, the effect of the Sun’s illumination may have a large effect on your imaging, if you can calculate the Sun’s position in the sky during the recorded path, and also calculate the camera’s “heading” then you can estimate the effect that the Sun will have on your images, for example are they back-lit, front-lit, will long shadows be cast etc.

The background to the calculation can be found here under the section called ‘bearing’.

Note that the terms ‘heading’, ‘bearing’ and ‘course’ have subtly different meanings especially for vehicles that might drift or yaw like boats or aircraft, but here we use them interchangeably to refer to the movement of the GPS measurement center.  As the points we choose are close together we don’t really have to worry about the difference between initial bearing and final bearing.

Here is some code that will calculate heading from 2 locations.  Pass in the latitude and longitude of both points, the order in which the points are passed will affect the calculated heading.  The two points that you choose should be close together to get a good estimate of ‘instantaneous’ heading, however if they are too close together the estimate may be noisy due to the positional accuracy limits of GPS, a distance of 100m to 200m could work well, experiment for your GPS device and path type.

 

#define _USE_MATH_DEFINES
#include <math.h>
#include "calculate_gps_heading.h"
//   Copyright 2022 Kevin Godden
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
static inline double to_rad(double theta) {
    return (theta * M_PI) / 180.0;
}
static inline double to_degrees(double theta) {
    return (theta * 180.0) / M_PI;
}
//
// Calculate the heading in decimal degrees between 2 (preferably quite close) locations 
// (lat1, lon1) --> First location
// (lat2, lon2) --> Second location
//
double calculate_gps_heading(double lat1, double lon1, double lat2, double lon2) {
    // Convert degrees to radians
    lat1 = to_rad(lat1);
    lon1 = to_rad(lon1);
    lat2 = to_rad(lat2);
    lon2 = to_rad(lon2);    
        
    double dlon = lon2 - lon1;
    double X = cos(lat2) * sin(dlon);
    double Y = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dlon);
    
    double heading = atan2(X,Y);
    
    // We want heading in degrees, not radians.
    heading = to_degrees(heading);
  
    // We want a uniform heading of >=0 and <360
    if (heading < 0)
        heading = 360.0 + heading;
    
    return heading;
}

The code along with some tests can be found in this Git repo.

Related Posts: