地表两个点 的 距离计算公式

3 down vote favorite 2

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? c  embedded  geolocation  gps  floating-point share improve this question edited Aug 19 '11 at 15:40 asked  Aug 15 '11 at 5:52 FigBug 5,366 3 26 50

A couple things. (1) I would make sure that the FP routines (fabs, cos, sqrt) are taking doubles and not floats. Could be that your doubles are being "downgraded" to floats on the calls. With the PIC platform (assuming Microchip tools), anything is possible. (2) Do you have the ability to step through the code on the PIC (simulator / emulator)? I'd suggest stepping through the code on both platforms & seeing where the results (temp values, return values) start to diverge. That's where you start looking. Eliminate the guesswork if you can. –  Dan  Aug 15 '11 at 8:37  I'm using MikroElectronica's compiler rather than Microchip's. I'm beginning to believe their tools are just bad and I should switch. –  FigBug  Aug 15 '11 at 12:14 I hate to say it but it sounds like the wrong processor was chosen for the task. –  dwelch  Aug 15 '11 at 13:58 @FigBug - if that's the company in Belgrade, I witnessed a client lose a lot of time finding & verifying 3 subtle (and egregious) bugs in their compiler. This was 4-5 years ago. I avoid PIC if it's my choice, but when I have to go that route, I'll definitely avoid MikroElectronica. Good luck! –  Dan  Aug 15 '11 at 14:53 "will be no more than 10 meters apparent" -- huh? what does "apparent" mean in this context? –  Jason S  Aug 19 '11 at 14:48 show 2 more comments

4 Answers

active oldest votes

up vote 2 down vote

The accepted algorithm for use in Marine AIS systems (specified in IEC61193-4) is the Rhumb Linealgorithm. 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 ; } share improve this answer edited Aug 20 '11 at 12:42 answered  Aug 19 '11 at 19:56 Clifford 38.3k 4 32 82

Could you please send me the log correction? –  Pablo  Dec 4 '12 at 17:16  @Pablo: Add 0x0LL to the end of the log_two_power_n_reversed[] array (it should have 36 elements). I have also more recently found the sqrt() to be less precise that it could be for very small values - it can be improved upon. I have replaced the sqrt() function with the one presented as the last code fragment in another question. –  Clifford  Dec 5 '12 at 0:35  Thanks, I will try that –  Pablo  Dec 5 '12 at 6:48 add a comment

up vote 1 down vote

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 c2(x) = 0.9942 - 1.39*10-4 * x2 = 0.9942 - (0.01179x)2; its error over this range has a maximum value of 0.006. If you want better accuracy use a 4th degree polynomial (c4(x) = 0.999945-(0.01233015x)2+(0.007778x)4 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. share improve this answer edited Aug 19 '11 at 16:15 answered  Aug 19 '11 at 14:47 Jason S 76.2k 83 362 645

My system is currently for water sports, so hopefully it won't be used near the poles. I think it's safe to say it won't be used north of 60°N or south of 40°S. I feel consumer GPS is going to limit my accuracy more than my choice of algorithm. As long as error is less than 1%, I think I'll be happy. –  FigBug  Aug 19 '11 at 15:47 My other idea to improve my algorithm is use a table of radius of curvatures (en.wikipedia.org/wiki/Latitude#Degree_length) and then do a lookup and interpolate the radius. –  FigBug Aug 19 '11 at 15:49 @FigBug: Do yourself a favor: get the table, do a polynomial fit, and use that, rather than the table + interpolation. Table interpolation is a pain to implement, takes more memory + calculation than polynomial approximations. Its strengths are when you have a very weird wiggly function. –  Jason S  Aug 19 '11 at 16:18 add a comment

up vote 0 down vote

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. share improve this answer answered  Aug 15 '11 at 6:35 Benjamin Monate 254 1 5

1   He's on a dsPIC with a few K of memory. He needs about 11k entries. Looks like a table is not an option to me. –  Adam Hawes  Aug 15 '11 at 6:43 2   @Adam: OP has specified neither the range nor the accuracy he needs; high accuracy high range implies large tables, but in other cases perhaps they are appropriate. (although I tend to shun tables in favor of polynomial evaluation) –  Jason S  Aug 19 '11 at 14:37 add a comment

up vote 0 down vote

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. share improve this answer

3 down vote favorite 2

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? c  embedded  geolocation  gps  floating-point share improve this question edited Aug 19 '11 at 15:40 asked  Aug 15 '11 at 5:52 FigBug 5,366 3 26 50

A couple things. (1) I would make sure that the FP routines (fabs, cos, sqrt) are taking doubles and not floats. Could be that your doubles are being "downgraded" to floats on the calls. With the PIC platform (assuming Microchip tools), anything is possible. (2) Do you have the ability to step through the code on the PIC (simulator / emulator)? I'd suggest stepping through the code on both platforms & seeing where the results (temp values, return values) start to diverge. That's where you start looking. Eliminate the guesswork if you can. –  Dan  Aug 15 '11 at 8:37  I'm using MikroElectronica's compiler rather than Microchip's. I'm beginning to believe their tools are just bad and I should switch. –  FigBug  Aug 15 '11 at 12:14 I hate to say it but it sounds like the wrong processor was chosen for the task. –  dwelch  Aug 15 '11 at 13:58 @FigBug - if that's the company in Belgrade, I witnessed a client lose a lot of time finding & verifying 3 subtle (and egregious) bugs in their compiler. This was 4-5 years ago. I avoid PIC if it's my choice, but when I have to go that route, I'll definitely avoid MikroElectronica. Good luck! –  Dan  Aug 15 '11 at 14:53 "will be no more than 10 meters apparent" -- huh? what does "apparent" mean in this context? –  Jason S  Aug 19 '11 at 14:48 show 2 more comments

4 Answers

active oldest votes

up vote 2 down vote

The accepted algorithm for use in Marine AIS systems (specified in IEC61193-4) is the Rhumb Linealgorithm. 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 ; } share improve this answer edited Aug 20 '11 at 12:42 answered  Aug 19 '11 at 19:56 Clifford 38.3k 4 32 82

Could you please send me the log correction? –  Pablo  Dec 4 '12 at 17:16  @Pablo: Add 0x0LL to the end of the log_two_power_n_reversed[] array (it should have 36 elements). I have also more recently found the sqrt() to be less precise that it could be for very small values - it can be improved upon. I have replaced the sqrt() function with the one presented as the last code fragment in another question. –  Clifford  Dec 5 '12 at 0:35  Thanks, I will try that –  Pablo  Dec 5 '12 at 6:48 add a comment

up vote 1 down vote

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 c2(x) = 0.9942 - 1.39*10-4 * x2 = 0.9942 - (0.01179x)2; its error over this range has a maximum value of 0.006. If you want better accuracy use a 4th degree polynomial (c4(x) = 0.999945-(0.01233015x)2+(0.007778x)4 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. share improve this answer edited Aug 19 '11 at 16:15 answered  Aug 19 '11 at 14:47 Jason S 76.2k 83 362 645

My system is currently for water sports, so hopefully it won't be used near the poles. I think it's safe to say it won't be used north of 60°N or south of 40°S. I feel consumer GPS is going to limit my accuracy more than my choice of algorithm. As long as error is less than 1%, I think I'll be happy. –  FigBug  Aug 19 '11 at 15:47 My other idea to improve my algorithm is use a table of radius of curvatures (en.wikipedia.org/wiki/Latitude#Degree_length) and then do a lookup and interpolate the radius. –  FigBug Aug 19 '11 at 15:49 @FigBug: Do yourself a favor: get the table, do a polynomial fit, and use that, rather than the table + interpolation. Table interpolation is a pain to implement, takes more memory + calculation than polynomial approximations. Its strengths are when you have a very weird wiggly function. –  Jason S  Aug 19 '11 at 16:18 add a comment

up vote 0 down vote

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. share improve this answer answered  Aug 15 '11 at 6:35 Benjamin Monate 254 1 5

1   He's on a dsPIC with a few K of memory. He needs about 11k entries. Looks like a table is not an option to me. –  Adam Hawes  Aug 15 '11 at 6:43 2   @Adam: OP has specified neither the range nor the accuracy he needs; high accuracy high range implies large tables, but in other cases perhaps they are appropriate. (although I tend to shun tables in favor of polynomial evaluation) –  Jason S  Aug 19 '11 at 14:37 add a comment

up vote 0 down vote

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. share improve this answer