我需要计算GPS坐标之间的距离来计算行驶的距离。我已经尝试过Haversine和Vincenty算法,这两种算法在我的台式PC上运行良好,但是当我将代码移植到dsPIC时,由于缺乏浮点精度和sin和cos的糟糕实现,对于接近的点(在几米之内),它们返回0。
对于我的用例,我的点之间的距离不会超过10米,并且所有点之间的距离都在10公里以内。我尝试了以下算法,结果似乎还可以:
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;
}假设每1°的距离是111.3公里,我使用毕达哥拉斯定理来计算距离。有什么简单的方法可以改进我的算法吗?或者,有没有其他不依赖于高精度sin/cos的算法?
发布于 2011-08-20 03:56:48
在船舶AIS系统中使用的公认算法(在IEC61193-4中指定)是Rhumb Line算法。我已经成功地在使用Anthony Williams' fixed point maths library的目标上实现了这一点,它使用CORDIC算法,我相信通常会比软件浮点数提高大约5倍的性能。
然而,中的库是C++而不是C,由于大量的运算符重载,这使得它很容易使用,但可能不是您想要的。但是,为了这个库的好处,值得考虑对C代码使用C++编译。当然,这样做的问题是Microchip的C31编译器奇怪地不支持C++。
但是,需要注意的是,log()函数的查找表太短了一个值,并且在最后需要一个值为零的额外元素。安东尼在我找到它后证实了这一点,但我不相信他已经更新了下载。
无论哪种方式,答案可能是使用定点和CORDIC。
要解析为1米经度或赤道弧,您将需要8位精度,因此单精度浮点数将是不够的,使用双精度将大大减慢速度。查看MikroElectronica的C用户手册发现,该编译器只支持单精度- float、double和long double都是32位的,因此在任何情况下,该编译器都无法使用内置的FP类型达到所需的精度。
如果它有任何用处,下面是我使用Anthony的库编写的Rhumb Line代码:
标题:
#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正文:
#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 ;
}发布于 2011-08-15 14:35:49
您可能希望尝试对sin和cos使用预计算表。它使用更多的内存,可以在通用处理器(不是你的情况)上回收缓存,但在你的处理器上会有尽可能多的准确性,而且会相当快。
发布于 2011-08-15 14:43:01
您使用的是定点DSP (有效),因此您可能希望研究一下定点函数;它们可能会执行得更好。
事实证明Microchip有一个可用的定点程序库:http://www.microchip.com/stellent/idcplg?IdcService=SS_GET_PAGE&nodeId=2680&dDocName=en552208,我不知道这会有多大帮助-它可能缺乏您所需的精度。
下面是一个如何自己做的例子:http://www.coranac.com/2009/07/sines
回到正轨- Microchip页面建议他们的编译器和库在单精度和双精度方面都与IEEE-754兼容。除非他们说的是半真半假,并且使用的是半精度(16位)格式。如果你仍然没有得到你需要的结果,我会考虑提交一份错误报告。
https://stackoverflow.com/questions/7062046
复制相似问题