您的位置：首页 > 其它

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

2015-11-27 12:37 519 查看

3down
votefavorite
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

shareimprove
this question

edited Aug
19 '11 at 15:40

asked Aug 15 '11 at 5:52

FigBug

5,36632650

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 vote2down
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 ;
}

shareimprove
this answer

edited Aug
20 '11 at 12:42

answered Aug 19 '11 at 19:56

Clifford

38.3k43282

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 vote1down
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.

shareimprove
this answer

edited Aug
19 '11 at 16:15

answered Aug 19 '11 at 14:47

Jason S

76.2k83362645

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. – FigBugAug
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 vote0down
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.

shareimprove
this answer

answered Aug 15 '11 at 6:35

Benjamin Monate

25415

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

@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 vote0down
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.

shareimprove
this answer

3down
votefavorite
2

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;
}

shareimprove
this question

edited Aug
19 '11 at 15:40

asked Aug 15 '11 at 5:52

FigBug

5,36632650

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

"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 vote2down
vote

float

double

,
and

long
double

#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 ;
}

shareimprove
this answer

edited Aug
20 '11 at 12:42

answered Aug 19 '11 at 19:56

Clifford

38.3k43282

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[]

Thanks, I will try that – Pablo Dec
5 '12 at 6:48

add
a comment

up vote1down
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.

shareimprove
this answer

edited Aug
19 '11 at 16:15

answered Aug 19 '11 at 14:47

Jason S

76.2k83362645

add
a comment

up vote0down
vote

shareimprove
this answer

answered Aug 15 '11 at 6:35

Benjamin Monate

25415

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

add
a comment

up vote0down
vote

shareimprove
this answer

内容来自用户分享和网络整理，不保证内容的准确性，如有侵权内容，可联系管理员处理

标签：

相关文章推荐

新的分享

章节导航

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

4 Answers

4 Answers

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