Go Down

Topic: Arduino Haversine Program (Read 2619 times) previous topic - next topic

ewplayer3

So.... I'm currently working on an Auto-Pilot based quadcopter using an Arduino Mega 2560 with a bunch of add-on modules. That will probably prompt its own thread at some point.

In the mean time, one of the things I needed to work out was distance and bearing calculations from the drone's current GPS coordinates to its destination GPS coordinates. I thought maybe it would be useful to others to share this here.

The reason I thought others might find this useful was for the bearing calculations. I found every wrong way possible to do this scattered online. To make matters worse, I'm not a math genius by any stretch of the imagination.

It took me decomposing the functions on the igismap website and doing the math by hand to figure out where I was going wrong. It took me the better part of a day to get to this solution.

This program uses the Haversine Formulas for both distance and bearing. The distance functions were derived from: http://www.movable-type.co.uk/scripts/latlong.html and the bearing functions were derived from http://www.igismap.com/formula-to-find-bearing-or-heading-angle-between-two-points-latitude-longitude/.

Code: [Select]
#include <math.h>

double lat1;
double lat2;
double lon1;
double lon2;
double latR1;
double latR2;
double lonR1;
double lonR2;
double dlon;
double dlat;
double a;
double e;
double d;
double R = 6371.00;
double toDegrees = 57.295779;
char sb[10];
/*-----------------------------Setup---------------------------------*/
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);

}


/*-----------------------------Main Loop---------------------------------*/
void loop() {
  //Get Starting GPS Coordinates
  Serial.println("\nInput Start Latitude: ");
  while(Serial.available() == 0){}
  Serial.readBytes(sb, 10);
  lat1 = atof(sb);
  Serial.println("Input Start Longitude: \n");
  while(Serial.available() == 0){}
  Serial.readBytes(sb, 10);
  lon1 = atof(sb);
  String Start = "GPS Starting Location: " + String(lat1, 6) + ", " + String(lon1, 6);
  Serial.print(Start);

  //Get Destination GPS Coordinates
  Serial.println("\nInput Destination Latitude: ");
  while(Serial.available() == 0){}
  Serial.readBytes(sb, 10);
  lat2 = atof(sb);
  Serial.println("Input Destination Longitude: \n");
  while(Serial.available() == 0){}
  Serial.readBytes(sb, 10);
  lon2 = atof(sb);
  String Dest = "GPS Destination Location: " + String(lat2, 6) + ", " + String(lon2, 6);
  Serial.print(Dest);
  Serial.println();

  calcDist(); //Call the distance and bearing calculation function

}

/*-----------------------------Distance & Bearing Calculator---------------------------------*/
void calcDist(){ //This is a haversine based distance calculation formula
  //This portion converts the current and destination GPS coords from decDegrees to Radians
  lonR1 = lon1*(PI/180);
  lonR2 = lon2*(PI/180);
  latR1 = lat1*(PI/180);
  latR2 = lat2*(PI/180);
 
  //This portion calculates the differences for the Radian latitudes and longitudes and saves them to variables
  dlon = lonR2 - lonR1;
  dlat = latR2 - latR1;
 
  //This portion is the Haversine Formula for distance between two points. Returned value is in KM
  a = (sq(sin(dlat/2))) + cos(latR1) * cos(latR2) * (sq(sin(dlon/2)));
  e = 2 * atan2(sqrt(a), sqrt(1-a)) ;
  d = R * e;

  Serial.println();
  Serial.print("Distance to destination(KM): ");
  //Serial.println(a);
  //Serial.println(e);
  Serial.println(d, 6);
  Serial.println();
 
  //This portion is the Haversine Formula for required bearing between current location and destination. Returned value is in Degrees
  double x = cos(latR2)*sin(lonR2-lonR1); //calculate x
 
  Serial.print("X = ");
  Serial.println(x, 6);

  double y = cos(latR1)*sin(latR2)-sin(latR1)*cos(latR2)*cos(lonR2-lonR1); //calculate y

  Serial.print("Y = ");
  Serial.println(y, 6);
  float brRad = atan2(x, y); //return atan2 result for bearing. Result at this point is in Radians

  Serial.print("atan2(x, y) (Radians) = ");
  Serial.println(brRad, 6);

  float reqBear = toDegrees*brRad;
  Serial.print("Bearing: ");
  Serial.println(reqBear, 4);
}

A side note about the bearing calculations. atan2(x, y) returns a value in Radians. Not only that, but the Start and End GPS Coordinates must be converted from Decimal Degrees in to Radians before they're plugged in to the equation. The GIS site says nothing about that and I spent 2 hours using their equations as written on their site and kept coming out with the wrong answers. After I figured out that I needed to convert to Radians and then back from Radians, it was a pretty quick task.

P.S. Entering GPS coordinates to 6 decimal places is not accurate. The Serial Output of the coordinates does not match. I know it's because double is not truly double and it's rounding as a result. Despite that, I've still found distance to be accurate within about 15 feet and bearing within about .001 degrees on average. For my uses, it'll be close enough.

Hopefully, this will save someone time in the future.

jremington

#1
Apr 12, 2016, 09:54 pm Last Edit: Apr 12, 2016, 09:55 pm by jremington
The order of the arguments in the call for atan2 in your code is reversed from the C/C++ convention: atan2(y,x). Hopefully you have taken this into account.

Those of us interested in accurate navigational calculations on Arduino avoid floating point. Instead, units of decimal degrees*1x106 are usually used.

For short distances, the equirectangular approximation works better than the haversine forumula and is much faster. Code has been posted several times on the Arduino forum for all of these calculations.

ewplayer3

The order of the arguments in the call for atan2 in your code is reversed from the C/C++ convention: atan2(y,x). Hopefully you have taken this into account.

Those of us interested in accurate navigational calculations on Arduino avoid floating point. Instead, units of decimal degrees*1x106 are usually used.

For short distances, the equirectangular approximation works better than the haversine forumula and is much faster. Code has been posted several times on the Arduino forum for all of these calculations.
I did notice that I had the atan2 function reversed from the C++ convention. It seems to work though and I'm not entirely certain why.

I've not looked in to equirectangular approximation. When you say short distances, how short are we talking? I'm looking at the potential for this drone to travel on the order of about 20 miles or so.

jremington

Reversing the arguments in atan2() returns the complement of the angle (PI/2 - theta).

The equirectangular approximation is discussed here and when using lat/lon units of degrees*1x106, maintains accuracy to a few cm, up to some 10s of km (where the Earth's curvature starts to become important).

ewplayer3

Reversing the arguments in atan2() returns the complement of the angle (PI/2 - theta).

The equirectangular approximation is discussed here and when using lat/lon units of degrees*1x106, maintains accuracy to a few cm, up to some 10s of km (where the Earth's curvature starts to become important).
Thanks for the link! I'll read in to it. Who knows... I may dump Haversine. It's been a heck of a lot of fun to figure it out though.

I'll shift the atan2 from x,y to y,x and see what changes. I tracked my whole trip home and the angle in Degrees was quite accurate until about the last 100ft or so (assuming the floating point vars are not keeping accurate track of the destination coordinates).

ewplayer3

It's so unusual. For some reason, if I run the function as atan2(y, x), the answer comes out completely wrong. I don't know if I maybe just have the x and y equations assigned to the wrong variables or what, but I'm not going to look a gift horse in the mouth. For the moment, atan2(x, y) appears to be working appropriately.

el_supremo

The reason that you get the right answer using atan2(x,y) is that you've named those two variables the wrong way round compared to what is used in the description of the bearing calculation here: http://www.movable-type.co.uk/scripts/latlong.html

Pete
Don't send me technical questions via Private Message.

ewplayer3

The reason that you get the right answer using atan2(x,y) is that you've named those two variables the wrong way round compared to what is used in the description of the bearing calculation here: http://www.movable-type.co.uk/scripts/latlong.html

Pete
Interesting. I had suspected that, but wasn't sure. On the igismap website, it has them listed in the order I programmed them. Apparently their website is wrong and I reaped the benefits by accident.

el_supremo

I suppose, strictly speaking, it wouldn't matter what the names of the variables were, e.g. fred and aardvark :), as long as they were specified in the correct order to the atan2 function.

Pete
Don't send me technical questions via Private Message.

MarkT

atan2(y,x) is easy to remember because of the definition of atan(y/x) == atan2(y, x) (for 1st quadrant).
y/x is the slope of the vector (x,y)

Of course it helps to be familiar with complex numbers and angles too, which is the reason maths
uses anti-clockwise from x-axis convention.
[ I will NOT respond to personal messages, I WILL delete them, use the forum please ]

blueteam

Those of us interested in accurate navigational calculations on Arduino avoid floating point. Instead, units of decimal degrees*1x106 are usually used.

Hi jremington.

What do you mean by "units of decimal degrees*1x106 are usually used" ?

Thanks!

jremington

#11
Apr 23, 2017, 06:40 am Last Edit: Apr 23, 2017, 06:40 am by jremington
Quote
What do you mean by "units of decimal degrees*1x106 are usually used" ?
Multiply decimal degrees (in floating point representation) by 1000000 to get an integer.

blueteam

Multiply decimal degrees (in floating point representation) by 1000000 to get an integer.
Thanks for the fast response!

I get it, so for example

lat1 = 52.035901   -->   52035901
lon1 = .054210     -->       054210


However, new doubts have arisen:

1) Should we declare them as an integer (long)? If so, how can we convert a decimal degree value to a radian value, being that 1 decimal degree = PI / 180, aprox. 0.017453 (also a floating point value)?
If we also multiply the conversion value by a factor of 10^6 (0.017453 --> 17453), the conversion can produce a number above the 32 bits available, which can lead to wrap around.


2) sin, cos and tan of (x) are not the same as sin, cos and tan of (x*10^6), so when valuating with lat and lon multiplied by a factor of 10^6, on both equirectangular and haversine methods the results will be different than with no factor.

3) Considering the maximum posible angle difference (180 - 0 = 180  --> 180000000), wouldn't some operations on both equirectangular and haversine methods, like (lat2-lat1)^2 (worst case: 180000000^2) cause overflow?

Thanks!

jremington

#13
Apr 23, 2017, 10:29 pm Last Edit: Apr 23, 2017, 10:40 pm by jremington
Of course you must use long integers to store coordinates in that format.

You want to avoid floating point operations on the Arduino if you can, because they are limited to 6-7 digits of accuracy. For calculating trigonometric functions, you cannot avoid using single precision floats. However, this works fine if you are careful and understand the accuracy limitations.

For example, I use the following routine to calculate accurate bearings and distances between waypoints using the equirectangular approximation (good to roughly 100 km). For distances larger than that, the Earth's curvature becomes important.

Code: [Select]

// find the bearing and distance in meters from point 1 to 2,
// using the equirectangular approximation
// lat and lon are degrees*1.0e6, 10 cm precision

float course_to(long lat1, long lon1, long lat2, long lon2, float* distance) {

float dlam,dphi,radius= 6371000.0;

dphi = DEG2RAD*(lat1+lat2)*0.5e-6; //average latitude in radians
float cphi=cos(dphi);

dphi = DEG2RAD*(lat2-lat1)*1.0e-6; //differences in degrees (to radians)
dlam = DEG2RAD*(lon2-lon1)*1.0e-6;

dlam *= cphi;  //correct for latitude

float bearing=RAD2DEG*atan2(dlam,dphi);
if(bearing<0) bearing=bearing+360.0;

*distance = radius * sqrt(dphi*dphi + dlam*dlam);
return bearing;
}

MarkT

Without floating point sin/cos/atan2 are often calculated using CORDIC, so that angles are naturally
expressed in units of 2 pi / 2^16 or in units of 2 pi / 2^32.

This works for DDS also.
[ I will NOT respond to personal messages, I WILL delete them, use the forum please ]

Go Up