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

Space-efficient cosine approximation

This is a very accurate, space-efficient cosine approximation function. It is good to about 12 digits (base 10), which is probably overkill; we could remove some parts to save additional space and execution time.

Code:
#define pi2 1.570796326794896
#define p0 0.999999999781
#define p1 -0.499999993585
#define p2 0.041666636258
#define p3 -0.0013888361399
#define p4 0.00002476016134
#define p5 -0.00000026051495

float rcos (float deg) { // input is in degrees for simplicity in processing GPS coordinates
    // 9 bytes of local variables
    float frac, t;
    unsigned char quad;
    if (deg < 0)
        deg = -deg;
    deg /= 90.0;
    quad = (unsigned char)deg;
    frac = deg - quad;
    if (quad == 0 || quad == 2) t = frac * pi2;
    if (quad == 1) t = (1-frac) * pi2;
    if (quad == 3) t = (frac-1) * pi2;
    t = t*t;
    frac = p0 + (p1*t) + (p2*t*t) + (p3*t*t*t) + (p4*t*t*t*t) + (p5*t*t*t*t*t);
    if (quad == 2 || quad == 1)
        frac = -frac;
    return frac;
}

This takes up about 580 bytes, or 290 instructions on the PIC18F4550.

First post, space-efficient square root

The purpose of this blog is to collect all of my random embedded code snippets that may be useful ... most of this is targeted at Microchip PIC18 processors and their C18 compiler, but it should be pretty portable to other CPUs and build tools.

I'm going to do my best to credit any sources for the code here, if it is not entirely original. However, if there is an omission, please let me know and I will add the appropriate attributions. Thanks!

Our first example is a reasonably quick, but also very compact, square root function for floating point numbers. The goal here was to get 6-8 digits of accuracy without sacrificing much code space or ram, with less emphasis on execution time.

Code:
float rsqrt (float x) {
    // 9 bytes of local variables
    float out; // 32 bits, IEEE single format
    unsigned long xint; // 32 bits
    byte i; // 8 bits

    (*(unsigned long*)&x) &= 0x7FFFFFFF; // enforce positive #
    xint = 0x5f375a86 - ((*(unsigned long*)&x >> 1) & 0x7FFFFFFF); // initial approximation
    out = *(float*)&xint;
    for (i = 0; i < 5; i++)
        out = out * (1.5 - x*0.5*out*out); // Newton's method
    return out * x;
}

Using the 'magic number' for approximation was discussed here: http://www.codemaestro.com/reviews/9

Total space: 328 bytes = 164 instructions on PIC18F4550