python_从文件读取经纬度并统计之间的距离

import math


# from math import radians,cos,sin,asin,sqrt,pi,atan,tan,atan2
# 这个算法和 geographiclib 算的几乎一样，相差<0.2米
def distVincenty(lat1, lon1, lat2, lon2):
    '精度更高的椭球计算,计算两个 WGS84 经纬度点的距离'
    a = 6378137.0  # vincentyConstantA(WGS84) ##单位:米
    b = 6356752.3142451  # vincentyConstantB(WGS84) ##单位:米
    f = 1 / 298.257223563  # vincentyConstantF(WGS84)
    L = math.radians(lon2 - lon1)
    U1 = math.atan((1 - f) * math.tan(math.radians(lat1)))
    U2 = math.atan((1 - f) * math.tan(math.radians(lat2)))
    sinU1 = math.sin(U1)
    cosU1 = math.cos(U1)
    sinU2 = math.sin(U2)
    cosU2 = math.cos(U2)
    lambda1 = L
    lambdaP = 2 * math.pi
    iterLimit = 20

    sinLambda = 0.0
    cosLambda = 0.0
    sinSigma = 0.0
    cosSigma = 0.0
    sigma = 0.0
    alpha = 0.0
    cosSqAlpha = 0.0
    cos2SigmaM = 0.0
    C = 0.0
    while (abs(lambda1 - lambdaP) > 1e-12 and --iterLimit > 0):
        sinLambda = math.sin(lambda1)
        cosLambda = math.cos(lambda1)
        sinSigma = math.sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (
                cosU1 * sinU2 - sinU1 * cosU2 * cosLambda))
        if (sinSigma == 0):
            return 0
        cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda
        sigma = math.atan2(sinSigma, cosSigma)
        alpha = math.asin(cosU1 * cosU2 * sinLambda / sinSigma)
        cosSqAlpha = math.cos(alpha) * math.cos(alpha)
        cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha
        C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha))
        lambdaP = lambda1
        lambda1 = L + (1 - C) * f * math.sin(alpha) * (
                sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)))

    if iterLimit == 0:
        return 0.0

    uSq = cosSqAlpha * (a * a - b * b) / (b * b)
    A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)))
    B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)))
    deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (
            cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (
            -3 + 4 * cos2SigmaM * cos2SigmaM)))
    s = b * A * (sigma - deltaSigma)
    d = s  ##单位:米
    return d


if __name__ == '__main__':
    # file_name = 'E:/231025/YT'
    file_name = 'E:/10.27/GGA_20231027_015005'
    '''
        YT文件内容如下：
            $GPZDA,063052.00,16,10,2023,,*61
            $GPGGA,063052.00,4349.7377413,N,12509.8354912,E,4,40,0.6,222.928,M,0.00,M,01,2445*69
            $GPZDA,063053.00,16,10,2023,,*60
            $GPGGA,063053.00,4349.7377412,N,12509.8354914,E,4,40,0.6,222.926,M,0.00,M,01,2445*61
            $GPZDA,063054.00,16,10,2023,,*67
            $GPGGA,063054.00,4349.7377415,N,12509.8354924,E,4,40,0.6,222.926,M,0.00,M,01,2445*62
    '''
    with open(file_name, 'r') as yt:
        # 临时存放
        tmp = []
        for line in yt.readlines():
            tmpSort = line.strip().split(",")
            # 筛选以GPGGA开头，经纬度不为空的行
            if (tmpSort[0] == '$GPGGA') and tmpSort[2] and tmpSort[4]:
                tmp.append(tmpSort)
        # print(tmp)

        sum = 0
        l = 0
        for i in range(len(tmp) - 1):
            # print(tmp[i][2], tmp[i][4])
            # 计算相邻两行的第3个值（lat）和第5个（lon），并累加
            lat1 = float(tmp[i][2][:2]) + float(tmp[i][2][2:]) / 60
            lon1 = float(tmp[i][4][:3]) + float(tmp[i][4][3:]) / 60
            lat2 = float(tmp[i + 1][2][:2]) + float(tmp[i + 1][2][2:]) / 60
            lon2 = float(tmp[i + 1][4][:3]) + float(tmp[i + 1][4][3:]) / 60
            sum += distVincenty(lat1, lon1, lat2, lon2)
            l += 1
            print("第%s次：" % (l) + "统计路程：" + str(sum) + "米")

    print("总路程：%s" % str(sum) + "米")