如何在浮点支持较差的处理器上计算GPS坐标之间的距离？

Question

我需要计算GPS坐标之间的距离以计算行驶距离。我已经尝试过Haversine和Vincenty算法，在我的桌面电脑上运行良好，但是当我将代码移植到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公里，我使用勾股定理来计算距离。是否有更简便的方法来改善我的算法？或者是否有其他不依赖于高精度正弦/余弦的算法？

- Roland Rabien

---

以下几点建议：（1）请确保FP常规函数（fabs、cos、sqrt）使用的是double而非float。有可能在调用时将double“降级”为float。在PIC平台上（假设使用Microchip工具），一切皆有可能。（2）您能否在PIC上进行代码步进调试（模拟器/仿真器）？建议在两个平台上都进行代码步进调试，查看临时值和返回值开始发散的位置。那就是你开始查找的地方。如果可以，消除猜测。 - Dan

我正在使用MikroElectronica的编译器而不是Microchip的。我开始相信他们的工具只是糟糕的，我应该换一个。 - Roland Rabien

我不想说，但听起来选错了处理器来完成这个任务。 - old_timer

@FigBug - 如果那是贝尔格莱德的那家公司，我亲眼见证了一个客户在寻找和验证他们编译器中的3个微妙（且极其严重）的错误上浪费了很多时间。这是4-5年前的事情了。如果可以选择，我会避免使用PIC，但当我必须采用这种方式时，我一定会避免MikroElectronica。祝好运！ - Dan

“明显的距离不超过10米” -- 呃？在这个上下文中，“apparent”是什么意思？ - Jason S

MikroElectronica的编译器仅支持单精度浮点数(float,double和long double均仅为32位)。要解析到10米，您需要至少7位数字的精度，而且可能需要解析到至少一米，因此需要8位数字。您需要使用不同的编译器或使用我建议的定点数。不幸的是，您的编译器也不支持64位整数类型，因此您需要做很多工作来实现这一点。有许多ARM Cortex-M3或M4型号可用，比dsPIC更便宜，拥有更多的芯片资源。 - Clifford

Answer 1

海上自动识别系统（Marine AIS）中使用的接受算法（在IEC61193-2中指定）是Rhumb Line算法。我已经成功地在目标上实现了这一点，使用了Anthony Williams' fixed point maths library，该库使用CORDIC算法，并且通常比软件浮点数提供约5倍的性能改进。

然而，该库是C ++而不是C，由于广泛的运算符重载，使其易于使用，但可能不是您要寻找的。然而，考虑为您的C代码使用C++编译，仅出于使用此库的好处。当然，问题在于Microchip的C31编译器奇怪地不支持C ++。

然而，一个警告是log()函数的查找表缺少一个值，并且需要在末尾添加一个额外的元素，其值为零。我发现这一点后，Anthony确认了这一点，但我不认为他已经更新了下载。

无论如何，答案可能是使用定点和CORDIC。

为了将经度或赤道弧分辨率设置为1m，您需要8位数字精度，因此单精度浮点数不足，使用双精度会显著减慢速度。检查MikroElectronica的C用户手册发现，编译器仅支持单精度-float，double和long double都是32位，因此在任何情况下都无法使用内置FP类型实现所需的精度。

如果有用的话，这是我的Rhumb Line代码，使用了Anthony的库：

头文件：

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

- Clifford

---

@Pablo：在log_two_power_n_reversed[]数组的末尾添加0x0LL（它应该有36个元素）。我最近发现sqrt()对于非常小的值来说精度不够，可以改进。我已经用*另一个问题*中呈现的最后一个代码片段替换了sqrt()函数。 - Clifford

Answer 2

一些注释：

• 您需要指定计算的范围和精度要求。 范围和精度在确定计算余弦所使用的方法方面非常重要。此外，如果纬度和经度的相对差异与极点的角距离相比较小，则您发布的勾股近似法可以很好地工作。如果纬度不接近，那么您的伪勾股算法在高纬度下效果不佳。（例如，在纬度43.001和43.002时，它将工作得很好，但在89.961和89.962时则不行）

• 经度需要考虑其循环性 - 您的算法将在国际日期变更线周围失败，但这可以通过对经度差异进行对称模360来轻松解决，其中smod(x,m) = mod(x+m/2,m)-m/2。（例如，-179.5 - +179.5 = -359度，但如果您计算smod(-359,360)，您将获得+1度。）

• 在设计时，请充分利用您的个人电脑。 您有一个非常强大的计算器可用，并且您可以为大量测试点评估高精度答案和您的近似值，并查看它们的差异以评估准确性。如果您从这些信息中推导出一种模式，您可以使用它来进行二阶近似以提高准确性。

---

更新：您表示您的范围/精度要求为+/-60度（在一个半球中减少范围没有优势）和1％的精度。在此范围内，x以度为单位的cos(x)的良好近似值为c₂(x)=0.9942-1.39*10^-4*x²=0.9942-(0.01179x)²；其在此范围内的误差最大值为0.006。

如果您需要更高的精度，请使用4次多项式（c₄(x)=0.999945-(0.01233015x)²+(0.007778x)⁴在此范围内的最大误差小于6x10^-5，但对参数误差和算术误差更加敏感），或将其分成多个范围。

- Jason S

---

我的系统目前是为水上运动而设计的，因此希望它不会在极地附近使用。我认为可以肯定地说，它不会在60°N以北或40°S以南使用。我觉得消费级GPS会比我的算法选择更限制我的精度。只要误差小于1％，我想我会很满意。 - Roland Rabien

我改进算法的另一个想法是使用曲率半径表(http://en.wikipedia.org/wiki/Latitude#Degree_length)，然后进行查找和插值。 - Roland Rabien

@FigBug：给自己一个方便：获取表格，进行多项式拟合，然后使用它，而不是表格+插值。表格插值实现起来很麻烦，需要比多项式逼近更多的内存和计算。它的优点在于当你有一个非常奇怪的波动函数时。 - Jason S

Answer 3

你可以尝试使用预计算的正弦和余弦表。这会占用更多的内存，在通用处理器上可能会影响缓存（但不是你的情况），但在你的处理器上将具有尽可能高的精度，并且速度相当快。

- Benjamin Monate

---

他正在使用一款带有几千字节内存的dsPIC。他需要大约11k个条目。在我看来，似乎表格不是一个选项。 - Adam Hawes

@Adam：OP没有指定他需要的范围和精度；高精度高范围意味着大表格，但在其他情况下可能更合适。（尽管我倾向于避免使用表格，而是选择多项式求值） - Jason S

Answer 4

你正在使用固定点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位）格式并且只说了一半真话。如果你仍然没有得到所需的结果，我建议你提交一个错误报告。

- Adam Hawes