Calculating the distance between two points

GPS/GNSS Development Boards
1

Does anyone know of a method of calculating the distance between two (very close together) GPS coordinates? The method must be easily calculated by an 8-bit microcontroller.

I’m building a system that takes a GPS reading once a second and calculates the distance between these one second intervals, and aggregates them.

I’ve seen formulas such as the “haversine” formula, but this seems like overkill for my microcontroller application. I basically just want a conversion that translates coordinate seconds into feet. Any ideas?

2

I’m using the Haversine formula on an Arduino Duemilanove. I thought it seemed a bit overkill too, but turns out to work fairly well. Using it with GPS data for trip meters on an [Arduino-based scooter computer.](http://www.janspace.com/b2evolution/arduino.php)

3

Thanks for the information kschulz. Did you write the code implementing the Haversine formula yourself? Or did you get it from an open source library?

Rob

4

I based my code using the JavaScript examples shown here:

http://www.movable-type.co.uk/scripts/latlong.html

You have to decide how often to aggregate the samples. I’m using 1/16th of a mile right now, but this might need tweaking as I get a better idea of the accuracy over time.

My code for the Arduino looks like:

#define MILES_PER_METER 0.00062137f

#define EARTH_RADIUS_METERS 6372795.0f

:

float distance = DistanceBetween2Points( latitude, longitude, f_lat, f_lon, MILES_PER_METER );

:

//************************

// Great Circle distance calculation

// Returns the distance between two lat/lon points on this great Earth

// (Note: Assumes sea level, does not take into account altitude. Close enough.)

//

float DistanceBetween2Points( float Lat1, float Lon1, float Lat2, float Lon2, float unit_conversion )

{

float dLat = radians( Lat2 - Lat1 );

float dLon = radians( Lon2 - Lon1 );

float a = sin( dLat / 2.0f ) * sin( dLat / 2.0f ) +

cos( radians( Lat1 ) ) * cos( radians( Lat2 ) ) *

sin( dLon / 2.0f ) * sin( dLon / 2.0f );

float d = 2.0f * atan2( sqrt( a ), sqrt( 1.0f - a ) );

return d * EARTH_RADIUS_METERS * unit_conversion;

}

5

I was looking for some code to do a similar thing a while back and found this code:

// Following code from <http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1259946578/6>
// "It's pretty much cut'n'paste from the Ardupilot project" which is LGPL
// here: <http://code.google.com/p/ardupilot/>
//
//
/*************************************************************************
 * //Function to calculate the distance between two waypoints
 *************************************************************************/
float calc_dist(float flat1, float flon1, float flat2, float flon2) {
  float dist_calc=0;
  float dist_calc2=0;
  float diflat=0;
  float diflon=0;

  //I've to spplit all the calculation in several steps. If i try to do it in a single line the arduino will explode.
    diflat=radians(flat2-flat1);
  flat1=radians(flat1);
  flat2=radians(flat2);
  diflon=radians((flon2)-(flon1));

  dist_calc = (sin(diflat/2.0)*sin(diflat/2.0));
  dist_calc2= cos(flat1);
  dist_calc2*=cos(flat2);
  dist_calc2*=sin(diflon/2.0);
  dist_calc2*=sin(diflon/2.0);
  dist_calc +=dist_calc2;

  dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));

  dist_calc*=6371000.0; //Converting to meters
  //Serial.println(dist_calc);
  return dist_calc;
}

Might be helpful for you?

–Philip;

6

Convert everything to UTM and then you can you can get the separation in meters via sqrt(dx^2+dy^2).

If you’re going to operate over a predefined region, you can set up a simple linear transformation to go from lat/long to UTM. You do this offline, once with a least squares fit with four points located around your region of interest. You’ll need the coordinates for these points in UTM and lat-long.

I used this approach for my AVC entry so that I could use waypoints specified in meters. Coordinates measured in meters on the ground are much more useful than lat-long over small scales. UTM is great.

If you’re using single precision, be sure to peel off the majority of the coordinates (what doesn’t change over your region of interest) before you do calculations. Otherwise your data will be in the last digit or two and when you compute the difference between two coordinates, you’ll suffer a nasty loss of precision.

7

Hi Scott,

Thanks for you’re tips. I was unfamiliar with the UTM system, as I havent really looked into GPS before now. I’ve quickly read through the Wikipedia page on UTM and now have a better idea of what it is. So thanks for bringing that to my attention!

I am left wondering whether the technique you describe will offer me any benifit over the Haversine formula, since I have to take the additional step of converting coordinates to UTM. My original goal was to save clock cycles in the process of converting changes in coordinates to distance. And my application does not operate within a predefined region.

8

If you’re not operating within a pre-defined area, work with Lat-Long and the more complex formula. You’ll get much better results.

9

I assumed that since the points were very close to each other, you’d be operating over a predefined area. The linear transformation from lat-long to UTM is very fast and uses no trig.

I’m not sure what the accuracy of the Haversine formula really is.

Finally, some GPS units can output UTM directly, which will make your life much easier.

This link has a great description of the conversion to UTM (in Mathematica). http://blog.wolfram.com/2009/04/17/mapping-gps-data/

Good luck.

tronic:
Hi Scott,

Thanks for you’re tips. I was unfamiliar with the UTM system, as I havent really looked into GPS before now. I’ve quickly read through the Wikipedia page on UTM and now have a better idea of what it is. So thanks for bringing that to my attention!

I am left wondering whether the technique you describe will offer me any benifit over the Haversine formula, since I have to take the additional step of converting coordinates to UTM. My original goal was to save clock cycles in the process of converting changes in coordinates to distance. And my application does not operate within a predefined region.

10

tronic:
My original goal was to save clock cycles in the process of converting changes in coordinates to distance. And my application does not operate within a predefined region.

I agree with utoxin -- start with the full haversine formula, and see if that works for you.

If that runs too slowly, then you might try some approximations.

If your 2 points are very close together, then perhaps the following approximation will give you adequate results.

In the normal case (you haven’t moved a “significant distance” since the last measurement), the following doesn’t use any trig functions – only multiplies and sqrt().

// by David Cary
// public domain
// WARNING: untested code
double haversine( double angle ){
    return( pow( sin(angle/2), 2 ) );
};
double archaversine( double x ){
    return( 2*asin( min(1.0, sqrt(x)) ) );
};
double approx_haversine( double angle ){
    return( angle*angle*(1.0/(2*2)) );
};
double approx_archaversine( double x ){
    return( 2*sqrt(x) );
};
// classic mean radius -- see Wikipedia: Earth radius.
static const double r_earth = 6371.0e3; // in meters
/*
Calculate the distance between two *very* nearby points on Earth.
The points are (latitude1, longitude1) and (latitude2, longitude2) in degrees.
Returns a distance in meters.
*/
double fast_calculate_distance( double nLat1, double nLon1, double nLat2, double nLon2 ){
    assert( fabs(nLat1 - nLat2) < 1 );
    assert( fabs(nLon1 - nLon2) < 1 );
    static double average_latitude = 0;
    static double cos2_avl = 1;
 
    nLat1 = ToRad(nLat1);
    nLat2 = ToRad(nLat2);
    const double nDLon = ToRad( nLon2-nLon1 );

    // 1 km, in radians
    const double significant_distance = 1000 / r_earth;
    if( abs( average_latitude - nLat2 ) > significant_distance ){
        average_latitude = nLat2;
        double t = cos( average_latitude );
        cos2_avl = t*t;
    }; 
    double nA = approx_haversine(nLat1-nLat2) + cos2_avl * approx_haversine(nDLon);
    double nC = approx_archaversine(nA);
    double nD = r_earth * nC;
    return nD; // Return our calculated distance
}

Is that what you are looking for?

11

Interesting thread.

Subscribing.

12

The problem with Haversine is that it uses floating point… problematical on a small micro and containing its own errors.

Here are two representative functions. One Havesine, and one interger math using the Pythagorean theorem. for short distances I’d use the later.

Havesine

/*----------------------------------------------------------------------**
**     Haversine Formula                                                **
** (from R. W. Sinnott, "Virtues of the Haversine," Sky and Telescope,  **
** vol. 68, no. 2, 1984, p. 159):                                       **
**                                                                      **
**   dLon = lon2 - lon1                                                 **
**   dLat = lat2 - lat1                                                 **
**   a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2      **
**   c = 2 * atan2(sqrt(a), sqrt(1-a))                                  **
**   d = R * c                                                          **
**                                                                      **
** will give mathematically and computationally exact results. The      **
** intermediate result c is the great circle distance in radians. The   **
** great circle distance d will be in the same units as R.              **
**----------------------------------------------------------------------*/

float calcDist(float CurrentLatitude, float CurrentLongitude, float SavedLatitude, float SavedLongitude)
{
// HaverSine version
    const float Deg2Rad = 0.01745329252;               // (PI/180)  0.017453293, 0.0174532925
    //const double EarthRadius = 6372.795;              //6372.7976 In Kilo meters, will scale to other values
    const float EarthRadius = 20908120.1;              // In feet  20908128.6
    float DeltaLatitude, DeltaLongitude, a, Distance;

    // degrees to radians
    CurrentLatitude = (CurrentLatitude + 180) * Deg2Rad;     // Remove negative offset (0-360), convert to RADS
    CurrentLongitude = (CurrentLongitude + 180) * Deg2Rad;
    SavedLatitude = (SavedLatitude + 180) * Deg2Rad;
    SavedLongitude = (SavedLongitude + 180) * Deg2Rad;

    DeltaLatitude = SavedLatitude - CurrentLatitude;
    DeltaLongitude = SavedLongitude - CurrentLongitude;

    a =(sin(DeltaLatitude/2) * sin(DeltaLatitude/2)) + cos(CurrentLatitude) * cos(SavedLatitude) * (sin(DeltaLongitude/2) * sin(DeltaLongitude/2));
    Distance = EarthRadius * (2 * atan2(sqrt(a),sqrt(1-a)));

    return(Distance);
}

Pythagorean theorem

float calcDist_i(sint32 CurrentLatitude, sint32 CurrentLongitude, sint32 SavedLatitude, sint32 SavedLongitude, sint8 LATDD)
{
    /******************************************************
    // lat is represented bd DDddddddd
    // lon is represented by DDDddddddd
    // This is in sint32 bit for, the max number is +-2147483648
    Lat/Lon place shows how many 'd' to the right of the major D's
    To get standard format /10^LatPlace
    uBlox would be 10^7  or 10000000

    The idea is to convert floating point to interger, by multiplying the decimal point away.

    Then do all the calcutionas, and when done, shift back down to floating point representation



    **************************************************/

// pythagorean theorem version


    sint32  delta_x, delta_y;
    uint32 Distance_i;
    float Distance_f;


    delta_x = (cos_10000[LATDD] * (CurrentLongitude - SavedLongitude)) / 10000;
    delta_y = (CurrentLatitude - SavedLatitude);


    Distance_i = uisqrt32((uint32)(delta_x * delta_x) + (uint32)(delta_y * delta_y));
    //Distance_i = sqrt(delta_x * delta_x + delta_y * delta_y);
    Distance_i = (Distance_i * 1000) / 2743;
    //Distance = sqrt(delta_x * delta_x + delta_y * delta_y);
    Distance_f = Distance_i * 0.1;

    return (Distance_f);
}
13

wb8wka,

which of those 2 functions is more accurate?.

14

You might want to think about what you’re going to do with the distance once you calculate it, and how big the distance is likely to be. The haversine approach will be more accurate than the example pythagorean code, even over short distances and on parts of the globe where the pythagorean approximation is close to reality, if only because the example pythagorean code is using a 14+ bit cosine table.

As you can see, neither formula cares about altitude. If you care about altitude, you need a different distance measurement altogether. These formulas are approximating the great-circle distance between two points, which is the distance at sea level (as if the earth were a perfectly flat sea-level sphere).

For fans of accuracy overkill, you can also measure distance on a reference ellipsoid (GPS uses the WGS84 ellipsoid). That adds some additional terms to the haversine formula to account for the earth’s polar flattening.