Efficient GPS distance computation

The 'correct' way to compute the distance between two points on the globe is:


// x is longitude, y is latitude
float out;
float part1 = rcos(lastpt.y) * rcos(currpt.y);
float part2 = rcos(lastpt.x) * rcos(currpt.x) + rsin(lastpt.x) * rsin(currpt.x);
float part3 = part1 * part2 + rsin(lastpt.y) * rsin(currpt.y);
if (part3 > 1.0000) // just in case quantization errors have accumulated to the point of very slightly exceeding 1.0
    out = 0;
else
    out = acos(part3) * 3963.1;


The first problem with this algorithm is that it requires way too many complex floating-point operations. Also, it requires an unreasonable level of precision, especially for an embedded floating point calculation. For instance, let's say we're getting GPS points every second from a vehicle traveling between 0 and 100 mph:

Range of distance traversed (in 1 sec): 0.0000 - 0.0278 miles
Range of arccos result: 0.000000000-0.000007009 radians
Range of arccos input: 1.000000000000000-0.999999999975436

This is an issue because we cannot really represent the difference between the arccos input numbers with single precision floating point numbers, and not particularly well with double precision floating point numbers. Since we only have access to single precision on the embedded CPU (and even that is really slow), we need to come up with another algorithm...

But first, I came up with an approximation to acos() using sqrt() over this small input range:


double racos (double x) { // 1.000000000000000-0.999999999975436 input range
    return rsqrt(x) * 5604.0;
}


Interesting and accurate to about 0.5%. Too bad we can't use it...

So, to solve this issue, I decided to start with the assumption that we're not really going to be moving by more than 0.0004 degrees per second (except at the poles). We're definitely not going to be moving by more than 0.0004 degrees per second in latitude, as latitude lines are fixed distances apart (longitude lines vary in spacing as the latitude changes). We can calculate that the distance between two latitude degree lines is 69.16914359 miles. The longitude lines are a bit trickier, but it turns out that they are governed by 69.16914359 * cos(lat). Thus, assuming we're not trotting all over the globe every second, we can approximate the distance by:


#define MILESPERDEG 69.16914359

float latd = (currpt.y - lastpt.y) * MILESPERDEG; // latitude distance component
float lond = (currpt.x - lastpt.x) * rcos(currpt.y) * MILESPERDEG; // longitude distance component
float totald = rsqrt(latd * latd + lond * lond) * 100000.0; // distance * 100000 miles
unsigned long totaldist = (unsigned long) totald; // returns in integer form


This algorithm works very nicely for quite a wide range of input values, consumes very little code space, and yields even more accurate results (in the expected usage range) than the generic spherical algorithm.

Code space: ~350 bytes = ~175 instructions