Haversine calculations for sport GPS
We've been discussing distance calculations recently on one of the Garmin forums, and I thought it would be useful to record some of the conclusions in a blog post. I'm going to go into a bit of detail about some fairly simple maths. The context is the most economical but still accurate way to compute distances using Connect IQ on a Garmin watch; typically these don't have much program memory.
If you have two positions on the surface of the Earth defined by latitude and longitude, the distance between them can be calculated using the haversine formula. What this does is calculate the central angle between the two points (the angle between the lines joining the points to the centre of the earth) in radians, then multiplies it by the radius of the earth. This does approximate the Earth to a perfect sphere, so there is already a small error in the calculation by using this rather than a model like WGS-84, the main model used by the GPS system. (Bear with me. The focus of this post is getting usable accuracy with minimum calculation, for a problem that doesn't actually demand tremendous precision & runs on ConnectIQ so should be coded as concisely and battery-light as possible. I wouldn't run it like this on a desktop).
Here's a C++ code snippet lifted from my TCX-FIT conversion program (this post doesn't include MonkeyC versions, but the conversion is easy; focus on the maths). You supply the latitude and longitude of the two points in radians, and it returns the distance between them in metres.
Do we really need to do all those calculations?
Well, if you aren't running a marathon in Antarctica, no. Look at the sin calculations. They calculate the sin of the difference between the latitudes and the longitudes of the two points, and for the kind of distances you are typically considering in sporting GPS tracks, those differences are very, very small. For the sake of argument, let's suppose that the problem is calculating the distance from your current position to a waypoint at the end of the race, and that the start of the race is exactly one degree of latitude and one degree of longitude from the end. That's more than 100km in latitude; the distance in longitude shrinks as latitude increases, but even at 75° it's almost 30km. We're considering a difference of over 100km here, and that's pretty large in this context; often the problem is calculating the distance from one point to the next in a course, which will be less than 100m. For small values of \(\theta\),\(\sin(\theta)\approx\theta\). If this idea is new to you, it works like this.
\(\sin(\theta)\) can be written as a power series like this:
$$\sin(\theta)=\theta-\frac{\theta^3}{3!}+\frac{\theta^5}{5!}+...$$
The second term of this is the first term multiplied by \(\theta^2/6\). \(\theta\) is measured in radians, so for one degree change in latitude or longitude, \(\theta_1-\theta_2=\frac{\pi}{180}=0.01745\) and \(\frac{(\theta_1-\theta_2)^2}{6}=0.00005\). The second term in the series is almost 20,000 times smaller than the first. Over one degree of latitude and longitude, the error by ignoring the second and later terms - by just writing \(\theta\) instead of \(\sin(\theta)\) in your code - comes in at less than 10m. Between the spherical earth approximation, ignoring hills and valleys, and the little detail of GPS accuracy, this is of no importance at all. And \(\arcsin(x)\approx x\) as well, so the code above can be simplified to:
$$\cos(\theta_1)\cos(\theta_2)=\cos^2\left(\frac{\theta_1+\theta_2}{2}\right)-\sin^2\left(\frac{\theta_1-\theta_2}{2}\right)$$
For our one-degree step, \(\sin^2\left(\frac{\theta_1-\theta_2}{2}\right)\approx0.000076\); the error from leaving this term out is actually the same very small value whatever latitude you are at. It can be quite significant in the overall calculation if you are very close to one of the poles, where the \(\cos^2\) term is very small, and the lines of longitude are very close together so hs2 can be much larger than hs1. At this point, the sin approximation above is also going to break down because the lines of longitude are so close. Anywhere less than 80° from the Equator, you will do just fine with
One cos, two subtractions, one division, two additions, five multiplies and one square root. I ran a practical test on this by recomputing the distance of a TCX route using the full and simplified versions. Full version: 125875.8042m. Simplified version: 125875.8042m. The values are literally indistinguishable; the binary fit files are identical.
The specific problem we were discussing was using the current distance from an auto lap point to determine when to trigger an auto lap by position. razmichael pointed out that the simplest solution is to trigger when the watch enters a square, which can be computed by calling cos() just once and two abs and two comparisons per second. Again, when converting this code for a watch, you might want to combine the trigger distance and the Earth's radius into one parameter, saving memory and calculation (I have no idea if the Connect IQ compiler can optimise that sort of thing)
Using this method means that the lap will probably always trigger a little bit early. It is possible to trigger at the closest approach if, once you're within the square, you use the simplified haversine formula to compute the exact distance to the lap point, and save the previous distance from watch to lap point, compare it with the current distance, and trigger when the distance starts to increase again, which means you've run past the trigger point. This has a secondary advantage: you can make the side length of the square quite large, so that you will lap automatically even if you don't go exactly through the autolap point, or if the GPS accuracy causes paths past it to vary from lap to lap, but still trigger the lap as close as possible to the lap point.
And if you're interested: That cos formula in full, by the power of MathJax:
$$\begin{align*} M&=(\theta_1+\theta_2)/2\\ D&=(\theta_1-\theta_2)/2\\ \cos(M+D)&=\cos(M)\cos(D)-\sin(M)\sin(D)\\ \cos(M-D)&=\cos(M)\cos(D)+\sin(M)\sin(D)\\ \cos(\theta_1)\cos(\theta_2)&=\cos(M+D)\cos(M-D)\\ &=\cos^2(M)\cos^2(D)+\cos(M)\cos(D)\sin(M)\sin(D)\\ &-\cos(M)\cos(D)\sin(M)\sin(D)-\sin^2(M)\sin^2(D)\\ &=\cos^2(M)\left(1-\sin^2(D)\right)-\sin^2(M)\sin^2(D)\\ &=\cos^2(M)-\sin^2(D)(\cos^2(M)+\sin^2(M))\\ &=\cos^2(M)-\sin^2(D)\\ \end{align*}$$
If you have two positions on the surface of the Earth defined by latitude and longitude, the distance between them can be calculated using the haversine formula. What this does is calculate the central angle between the two points (the angle between the lines joining the points to the centre of the earth) in radians, then multiplies it by the radius of the earth. This does approximate the Earth to a perfect sphere, so there is already a small error in the calculation by using this rather than a model like WGS-84, the main model used by the GPS system. (Bear with me. The focus of this post is getting usable accuracy with minimum calculation, for a problem that doesn't actually demand tremendous precision & runs on ConnectIQ so should be coded as concisely and battery-light as possible. I wouldn't run it like this on a desktop).
Here's a C++ code snippet lifted from my TCX-FIT conversion program (this post doesn't include MonkeyC versions, but the conversion is easy; focus on the maths). You supply the latitude and longitude of the two points in radians, and it returns the distance between them in metres.
// Earth radius in metres for haversine calculation
#define DMMEANRADIUS (6371000.0)
double haversinedist(double rlat1, double rlong1, double rlat2, double rlong2)
{
double hs1, hs2;
hs1 = sin((rlat1-rlat2)/2.0);
hs2 = sin((rlong1-rlong2)/2.0);
return 2.0*DMMEANRADIUS*asin(sqrt(hs1*hs1+cos(rlat1)*cos(rlat2)*hs2*hs2));
}
Do we really need to do all those calculations?
Well, if you aren't running a marathon in Antarctica, no. Look at the sin calculations. They calculate the sin of the difference between the latitudes and the longitudes of the two points, and for the kind of distances you are typically considering in sporting GPS tracks, those differences are very, very small. For the sake of argument, let's suppose that the problem is calculating the distance from your current position to a waypoint at the end of the race, and that the start of the race is exactly one degree of latitude and one degree of longitude from the end. That's more than 100km in latitude; the distance in longitude shrinks as latitude increases, but even at 75° it's almost 30km. We're considering a difference of over 100km here, and that's pretty large in this context; often the problem is calculating the distance from one point to the next in a course, which will be less than 100m. For small values of \(\theta\),\(\sin(\theta)\approx\theta\). If this idea is new to you, it works like this.
\(\sin(\theta)\) can be written as a power series like this:
$$\sin(\theta)=\theta-\frac{\theta^3}{3!}+\frac{\theta^5}{5!}+...$$
The second term of this is the first term multiplied by \(\theta^2/6\). \(\theta\) is measured in radians, so for one degree change in latitude or longitude, \(\theta_1-\theta_2=\frac{\pi}{180}=0.01745\) and \(\frac{(\theta_1-\theta_2)^2}{6}=0.00005\). The second term in the series is almost 20,000 times smaller than the first. Over one degree of latitude and longitude, the error by ignoring the second and later terms - by just writing \(\theta\) instead of \(\sin(\theta)\) in your code - comes in at less than 10m. Between the spherical earth approximation, ignoring hills and valleys, and the little detail of GPS accuracy, this is of no importance at all. And \(\arcsin(x)\approx x\) as well, so the code above can be simplified to:
// Earth radius in metres for haversine calculation
#define DMMEANRADIUS (6371000.0)
double haversinedist(double rlat1, double rlong1, double rlat2, double rlong2)
{
double hs1, hs2;
hs1 = (rlat1-rlat2)/2.0;
hs2 = (rlong1-rlong2)/2.0;
return 2.0*DMMEANRADIUS*sqrt(hs1*hs1+cos(rlat1)*cos(rlat2)*hs2*hs2);
}
$$\cos(\theta_1)\cos(\theta_2)=\cos^2\left(\frac{\theta_1+\theta_2}{2}\right)-\sin^2\left(\frac{\theta_1-\theta_2}{2}\right)$$
For our one-degree step, \(\sin^2\left(\frac{\theta_1-\theta_2}{2}\right)\approx0.000076\); the error from leaving this term out is actually the same very small value whatever latitude you are at. It can be quite significant in the overall calculation if you are very close to one of the poles, where the \(\cos^2\) term is very small, and the lines of longitude are very close together so hs2 can be much larger than hs1. At this point, the sin approximation above is also going to break down because the lines of longitude are so close. Anywhere less than 80° from the Equator, you will do just fine with
// Earth radius in metres for haversine calculation
#define DMMEANRADIUS (6371000.0)
double haversinedist(double rlat1, double rlong1, double rlat2, double rlong2)
{
double hs1, hs2, c2;
hs1 = (rlat1-rlat2)/2.0;
hs2 = (rlong1-rlong2)/2.0;
c2 = cos((rlat1+rlat2)/2.0);
return 2.0*DMMEANRADIUS*sqrt(hs1*hs1+c2*c2*hs2*hs2);
}
// Earth radius in metres for haversine calculation
#define DMMEANRADIUS (6371000.0)
double haversinedist(double rlat1, double rlong1, double rlat2, double rlong2)
{
double hs1, hs2, c2;
hs1 = rlat1-rlat2;
hs2 = rlong1-rlong2;
c2 = cos((rlat1+rlat2)/2.0);
return DMMEANRADIUS*sqrt(hs1*hs1+c2*c2*hs2*hs2);
}
One cos, two subtractions, one division, two additions, five multiplies and one square root. I ran a practical test on this by recomputing the distance of a TCX route using the full and simplified versions. Full version: 125875.8042m. Simplified version: 125875.8042m. The values are literally indistinguishable; the binary fit files are identical.
The specific problem we were discussing was using the current distance from an auto lap point to determine when to trigger an auto lap by position. razmichael pointed out that the simplest solution is to trigger when the watch enters a square, which can be computed by calling cos() just once and two abs and two comparisons per second. Again, when converting this code for a watch, you might want to combine the trigger distance and the Earth's radius into one parameter, saving memory and calculation (I have no idea if the Connect IQ compiler can optimise that sort of thing)
// Earth radius in metres for haversine calculation
#define DMMEANRADIUS (6371000.0)
bool insquare(double rlat1, double rlong1, double rlat2, double rlong2, double coslat1 )
{
return( fabs(rlat1-rlat2)*DMMEANRADIUS < 20.0 && fabs(rlong1-rlong2)*DMMEANRADIUS*coslat1 < 20.0 );
}
Using this method means that the lap will probably always trigger a little bit early. It is possible to trigger at the closest approach if, once you're within the square, you use the simplified haversine formula to compute the exact distance to the lap point, and save the previous distance from watch to lap point, compare it with the current distance, and trigger when the distance starts to increase again, which means you've run past the trigger point. This has a secondary advantage: you can make the side length of the square quite large, so that you will lap automatically even if you don't go exactly through the autolap point, or if the GPS accuracy causes paths past it to vary from lap to lap, but still trigger the lap as close as possible to the lap point.
And if you're interested: That cos formula in full, by the power of MathJax:
$$\begin{align*} M&=(\theta_1+\theta_2)/2\\ D&=(\theta_1-\theta_2)/2\\ \cos(M+D)&=\cos(M)\cos(D)-\sin(M)\sin(D)\\ \cos(M-D)&=\cos(M)\cos(D)+\sin(M)\sin(D)\\ \cos(\theta_1)\cos(\theta_2)&=\cos(M+D)\cos(M-D)\\ &=\cos^2(M)\cos^2(D)+\cos(M)\cos(D)\sin(M)\sin(D)\\ &-\cos(M)\cos(D)\sin(M)\sin(D)-\sin^2(M)\sin^2(D)\\ &=\cos^2(M)\left(1-\sin^2(D)\right)-\sin^2(M)\sin^2(D)\\ &=\cos^2(M)-\sin^2(D)(\cos^2(M)+\sin^2(M))\\ &=\cos^2(M)-\sin^2(D)\\ \end{align*}$$
Comments
Post a Comment