How do I calculate distance between GPS co-ordinat

Question

I need to calculate the distance between GPS co-ordinates to calculate distance traveled. I've tried both the Haversine and Vincenty algorithms, which work fine on my desktop PC, but when I port the code to the dsPIC, they return 0 for points that are close (within several meters) due to a lack of floating point precision and poor implementations of sin and cos.

For my use case, my points will be no more than 10 meters apart and will all fall within 10km of each other. I've tried the following algorithm and the results seem ok:

double dist(double latA, double lonA, double latB, double lonB)
{
    double latD = fabs(latA - latB) * 111.3;
    double lonD = fabs(lonA - lonB) * 111.3 * cos(latA * 3.14159265 / 180);

    return sqrt(latD*latD + lonD*lonD) * 1000;
}

Assuming the distance for every 1° is 111.3km, I use the pythagorean theorem to calculate distance. Is there any easy way to improve my algorithm? Or are there any other algorithms that don't depend on a highly accurate sin/cos?

Answer 1

You may want to try to use a precomputed table for sin and cos. It uses more memory, can trash the cache on general purpose processor (not your case) but will have as much accuracy as possible on your processor and will be quite fast.

Answer 2

You're on a fixed point DSP (effectively) so you might want to look into fixed-point functions; they are likely to perform better.

Turns out that Microchip have a fixed-point library available: http://www.microchip.com/stellent/idcplg?IdcService=SS_GET_PAGE&nodeId=2680&dDocName=en552208 I don't know how helpful that will be - it may lack the precision you need.

And here is an example of how to do it yourself: http://www.coranac.com/2009/07/sines

Back on track - the Microchip page suggests that their compiler and library are IEEE-754 compatible for both single and double precision. Unless they're telling a half-truth and are using the half-precision (16-bit) format. If you still aren't getting the results you need I'd consider filing a bug report.

Answer 3

Some comments:

• You need to specify the range and the accuracy requirements of your computation. Range and accuracy are extremely important in determining what approach you use to calculate cosines. In addition, the pythagorean approximation you posted works well if the relative differences in latitude and longitude are small compared to the angular distance to the pole. Your pseudo-pythagorean algorithm won't work well at high latitudes if the latitudes aren't close together. (e.g. with latitude 43.001 and 43.002 it would work well, but not at 89.961 and 89.962)

• Longitudes need to be calculated in view of their circularness -- Your algorithm will fail around the international date line, but that can be easily remedied by taking the longitudinal difference symmetric-modulo 360, where smod(x,m) = mod(x+m/2,m)-m/2. (e.g. -179.5 - +179.5 = -359 degrees, but if you compute smod(-359,360) you get +1 degrees.)

• At design time, make good use of your PC. You have a very powerful calculator available, and you could evaluate for a large # of test points the high-precision answer and your approximation, and see how they differ, to evaluate accuracy. If you deduce a pattern in this information, you could use it to make a 2nd-order approximation to increase accuracy.

---

update: You state that your range/accuracy requirements are +/-60 degrees (no advantage in reducing range in one hemisphere) and 1% accuracy. A good approximation of cos(x), with x in degrees, within this range is c₂(x) = 0.9942 - 1.39*10^-4 * x² = 0.9942 - (0.01179x)²; its error over this range has a maximum value of 0.006.

If you want better accuracy use a 4th degree polynomial (c₄(x) = 0.999945-(0.01233015x)²+(0.007778x)⁴ has a maximum error over this range of less than 6x10^-5, but is much more sensitive to parameter errors and arithmetic errors) or split into multiple ranges.

Answer 4

The accepted algorithm for use in Marine AIS systems (specified in IEC61193-4) is the Rhumb Line algorithm. I have successfully implemented this on a target using Anthony Williams' fixed point maths library, which uses the CORDIC algorithm, and will I believe typically give a bout 5x performance improvement over software floating point.

However the library in is C++ rather than C, which makes it easy to use due to extensive operator overloading, but is not perhaps what you are looking for. Worth considering using C++ compilation for your C code however, just for the benefit of this library. The problem with that of course is that Microchip's C31 compiler bizarrely does not support C++.

One caveat however is that the look-up table for the log() function is too short by one value and needs an additional element at the end with value zero. This was confirmed by Anthony after I found it, but I do not believe that he has updated the download.

Either way, the answer is probably to use fixed point, and CORDIC.

To resolve to 1m of longitude or equatorial arc, you will need 8 digits of precision, so a single precision float will be insufficient, using double precision will slow things considerably. Checking MikroElectronica's C User Manual reveals that the compiler only supports single precision - float, double, and long double are all 32-bit, so there is no way to achieve the accuracy you need using the built-in FP types in any case with this compiler.

If it is of any use, here is my Rhumb Line code using Anthony's library:

Header:

#if !defined cRhumbLine_INCLUDE
#define cRhumbLine_INCLUDE

#include "fixed.hpp"

//! Rhumb Line method for distance and bearing between two geodesic points
class cRhumbLine
{
public:

    //! @brief Default constructor
    //!
    //! Defines a zero length line, bearing zero
    cRhumbLine() : m_update_bearing(false), m_update_distance(false), m_distance(0), m_bearing(0) {} 

    //! @brief Constructor defining a line
    //!
    //! @param startLatDeg  Start latitude in degrees, negative values are south of equator
    //! @param startLonDeg  Start longitude in degrees, negative values are west of meridian.
    //! @param endLatDeg    End latitude in degrees, negative values are south of equator
    //! @param endLonDeg    End longitude in degrees, negative values are west of meridian.
    cRhumbLine( fixed startLatDeg, fixed startLonDeg, fixed endLatDeg, fixed endLonDeg ) 
    {
        define( startLatDeg, startLonDeg, endLatDeg, endLonDeg ) ;
    }

    //! @brief Define a start/ent point
    //!
    //! @param startLatDeg  Start latitude in degrees, negarive values are south of equator
    //! @param startLonDeg  Start longitude in degrees, negative values are west of meridian.
    //! @param endLatDeg    End latitude in degrees, negarive values are south of equator
    //! @param endLonDeg    End longitude in degrees, negative values are west of meridian.
    void define( fixed startLatDeg, fixed startLonDeg, fixed endLatDeg, fixed endLonDeg ) ;

    //! @brief Get start-end distance in nautical miles
    //! @return Start-end distance in nautical miles.
    fixed distanceNm() { return distanceMetres() / ONE_NM_IN_METRE ; }

    //! @brief Get start-end distance in metres.
    //! @return Start-end distance in metres.
    fixed distanceMetres()  ;

    //! @brief Get start-end bearing in degreed.
    //! @return Start-end bearing in degreed (0 <= x < 360).
    fixed bearingDeg()  ;

private:
    static const int ONE_NM_IN_METRE = 1852 ;

    bool m_update_bearing ;
    bool m_update_distance ;
    fixed m_distance ;
    fixed m_bearing ;

    fixed m_delta_phi ;
    fixed m_delta_lon ;
    fixed m_delta_lat ;
    fixed m_lat1_radians ;
} ;

#endif

Body:

#include "cRhumbLine.h"

void cRhumbLine::define( fixed startLatDeg, fixed startLonDeg, fixed endLatDeg, fixed endLonDeg )
{
    fixed lat1 = startLatDeg / 180 * fixed_pi ;
    fixed lon1 = startLonDeg / 180 * fixed_pi ;
    fixed lat2 = endLatDeg / 180 * fixed_pi ;
    fixed lon2 = endLonDeg / 180 * fixed_pi ;

    m_update_bearing = true ;
    m_update_distance = true ;

    m_delta_phi = log( tan( lat2 / 2 + fixed_quarter_pi ) / tan( lat1 / 2 + fixed_quarter_pi ) ) ;
    m_delta_lat = lat1 - lat2 ;
    m_delta_lon = lon1 - lon2 ;
    m_lat1_radians = lat1 ;

    // Deal with delta_lon > 180deg, take shorter route across meridian
    if( m_delta_lon.abs() > fixed_pi )
    {
        m_delta_lon = m_delta_lon > 0 ? -(fixed_two_pi - m_delta_lon) : (fixed_two_pi + m_delta_lon) ;
    }
} 


fixed cRhumbLine::distanceMetres()
{
    if( m_update_distance )
    {
        static const fixed mean_radius = 6371009 ; // in metres. Source: International Union of Geodesy and Geophysics (IUGG)

        fixed q = m_delta_phi != 0 ? m_delta_lat / m_delta_phi : cos( m_lat1_radians ) ; // Deal with lines with constant latitude

        m_distance = sqrt( ( sqr(m_delta_lat) + sqr(q) * sqr(m_delta_lon) ) ) * mean_radius ;
        m_update_distance = false ;
    }

    return m_distance ;
}


fixed cRhumbLine::bearingDeg()
{
    if( m_update_bearing )
    {
        static const fixed mean_radius = 6371009 ; // in metres. Source: International Union of Geodesy and Geophysics (IUGG)

        m_bearing = atan2( m_delta_lon, m_delta_phi ) / fixed_pi * 180 ;
        if( m_bearing == -180 )
        {
            m_bearing == 0 ;
        }
        else if( m_bearing < 0 )
        {
            m_bearing += 360 ;
        }
        m_update_bearing = false ;
    }

    return m_bearing ;
}