GNSS说第(五)讲---利用python实现RTKLIB中的定位误差收敛曲线绘制功能

博主这周研究了python怎么绘制定位收敛曲线图，参考RTKLIB中的RTKPLOT，终于实现了这个功能，此次发出来的为测试版本，后续会发出最新版本。

我的效果图：

RTKPLOT效果图：

和成熟代码还是有差距的哈！但是误差均值都是0均值，说明程序没有问题，至于算出来的NEU三个方向上的RMSE与RTKPLOT中的不相等，博主还在与实验室师兄商讨！
下面废话不多说，直接上代码！

# -*- coding:utf-8 -*-
"""

created on Wed Apr 28 14:08:25 2021
@author: xymeng

"""
import numpy as np
import math as mt
import matplotlib.pyplot as plt
import sklearn.metrics as ms

'''以下为WGS84椭球参数'''
a = 6378137.0         # 参考椭球的长半轴, 单位 m
b = 6356752.31414     # 参考椭球的短半轴, 单位 m

'''以下为三角函数调用'''
sqrt = mt.sqrt
sin = mt.sin
cos = mt.cos
atan = mt.atan

'''以下为列表定义'''
X = []
Y = []
Z = []
B = []
L = []
H = []
N = []
E = []
U = []

'''以下为弧度转换角度函数'''
def rad2angle(r):
    """
    该函数可以实现弧度到角度的转换.
    :param r:  弧度
    :return:  a, 对应的角度
    """
    a = r*180.0/mt.pi
    return a

'''以下为角度转换弧度函数'''
def angle2rad(a):
    """
    该函数可以实现角度到弧度的转换.
    :param a:  角度
    :return:  r, 对应的弧度
    """
    r = a*mt.pi/180.0
    return r

'''以下为XYZ转BLH函数'''
def XYZ2BLH(X, Y, Z, a, b):
    """
    该函数实现把某点在参心空间直角坐标系下的坐标（X, Y, Z)转为大地坐标（B, L, H).
    :param X:  X方向坐标，单位 m
    :param Y:  Y方向坐标, 单位 m
    :param Z:  Z方向坐标, 单位 m
    :param a: 地球长半轴，即赤道半径，单位 m
    :param b: 地球短半轴，即大地坐标系原点到两级的距离, 单位 m
    :return:  B, L, H, 大地纬度、经度、海拔高度 (m)
    """

    e = sqrt((a**2-b**2)/(a**2)) #椭球第一偏心率

    if X == 0 and Y > 0:
        L = 90
    elif X == 0 and Y < 0:
        L = -90
    elif X < 0 and Y >= 0:
        L = atan(Y/X)
        L = rad2angle(L)
        L = L+180
    elif X < 0 and Y <= 0:
        L = atan(Y/X)
        L = rad2angle(L)
        L = L-180
    else:
        L = atan(Y / X)
        L = rad2angle(L)

    b0 = atan(Z/sqrt(X**2+Y**2))
    N_temp = a/sqrt((1-e*e*sin(b0)*sin(b0)))
    b1 = atan((Z+N_temp*e*e*sin(b0))/sqrt(X**2+Y**2))

    while abs(b0-b1) > 1e-7:
        b0 = b1
        N_temp = a / sqrt((1 - e * e * sin(b0) * sin(b0)))
        b1 = atan((Z + N_temp * e * e * sin(b0)) / sqrt(X ** 2 + Y ** 2))

    B = b1
    N = a/sqrt((1-e*e*sin(B)*sin(B)))
    H = sqrt(X**2+Y**2)/cos(B)-N
    B = rad2angle(B)
    return B, L, H   # 返回大地纬度B、经度L、海拔高度H (m)

'''以下函数为大地坐标转站心地平坐标'''
def BLH2NEU(B,L,X,Y,Z,X0,Y0,Z0):
    """
        该函数实现把某点在参心空间直角坐标系下的坐标（X, Y, Z)转为站心地平坐标(E,N,U).
        :param X:  X方向坐标，单位 m
        :param Y:  Y方向坐标, 单位 m
        :param Z:  Z方向坐标, 单位 m
        :param B:  大地纬度，单位 度
        :param L:  大地经度, 单位 度
        :param H:  海拔高度, 单位 m
        :return:  N, E ,U, 东方向，北方向，天顶方向(m)
        """
    N = -sin(B)*cos(L)*(X-X0)-sin(B)*sin(L)*(Y-Y0)+cos(B)*(Z-Z0)
    E = -sin(L)*(X-X0)+cos(L)*(Y-Y0)
    U = cos(B)*cos(L)*(X-X0)+cos(B)*sin(L)*(Y-Y0)+sin(B)*(Z-Z0)
    return N,E,U


if __name__ == '__main__':
    # path = input('请输入pos定位结果文件：') #E://rtklib//data//mypos.pos
    #
    # '''以下代码是读取所有行的XYZ'''
    # with open(path,'r') as f:
    #     lines = f.readlines()
    #     for line in lines[8:len(lines)]:
    #         X.append(float(line.split('  ')[1]))
    #         Y.append(float(line.split('  ')[2]))
    #         Z.append(float(line.split('  ')[3]))
    #
    # '''以下行是将定位均值设为XYZ真值'''
    # X0 = np.mean(X)
    # Y0 = np.mean(Y)
    # Z0 = np.mean(Z)
    # p,d,q = BLH2NEU(43.790,125.443,X0,Y0,Z0,-2674377.0,3757198.0,4391503.0)
    # print(p)
    # print(d)
    # print(q)
    # print(type(q))

    path = input('请输入pos定位结果文件：') #E://rtklib//data//mypos.pos

    '''以下代码是读取所有行的XYZ'''
    with open(path,'r') as f:
        lines = f.readlines()
        for line in lines[8:len(lines)]:
            X.append(float(line.split('  ')[1]))
            Y.append(float(line.split('  ')[2]))
            Z.append(float(line.split('  ')[3]))

    '''以下行是将定位均值设为XYZ真值'''
    X0 = np.mean(X)
    Y0 = np.mean(Y)
    Z0 = np.mean(Z)

    '''以下是计算真值对应的BLH'''
    B0,L0,H0 = XYZ2BLH(X0,Y0,Z0,a,b)

    '''下面为计算出各行对应的NEU'''
    for l in range(len(X)):
        N0,E0,U0 = BLH2NEU(B0,L0,X[l],Y[l],Z[l],X0,Y0,Z0)
        N.append(N0)
        E.append(E0)
        U.append(U0)

    '''以下为定义收敛图的x坐标范围'''
    xx = [i for i in range(1,len(X)+1)]

    '''以下为定义NEU真值列表'''
    NN = []
    EE = []
    UU = []
    for k in range(len(X)):
        NN.append(0)
        EE.append(0)
        UU.append(0)

    '''下面为计算ENU的均值、标准差和均方根误差'''
    Emean = round(np.mean(E),4)
    Nmean = round(np.mean(N),4)
    Umean = round(np.mean(U),4)

    Estd = round(np.std(E, ddof=1),4)
    Nstd = round(np.std(N, ddof=1),4)
    Ustd = round(np.std(U, ddof=1),4)

    Ermse = round(sqrt(ms.mean_squared_error(EE,E)),4)
    Nrmse = round(sqrt(ms.mean_squared_error(NN,N)),4)
    Urmse = round(sqrt(ms.mean_squared_error(UU,U)),4)

    '''以下为绘制收敛曲线图'''
    plt.style.use('seaborn-whitegrid')
    plt.rcParams['font.sans-serif'] = ['KaiTi']
    plt.rcParams['axes.unicode_minus'] = False
    plt.figure(dpi=500, figsize=(12, 6))
    plt.subplot(311)
    plt.errorbar(xx,E,yerr=E, fmt='.r',ecolor='lightgray',elinewidth=0.05,
                 label='AVE={}m,STD={}m,RMSE={}m'.format(Emean,Estd,Ermse),markersize=1)
    plt.xlabel('历元/30s')
    plt.ylabel('E/m')
    plt.legend()

    plt.subplot(312)
    plt.errorbar(xx, N, yerr=N, fmt='.r', ecolor='lightgray', elinewidth=0.05,
                 label='AVE={}m,STD={}m,RMSE={}m'.format(Nmean, Nstd, Nrmse),markersize=1)
    plt.xlabel('历元/30s')
    plt.ylabel('N/m')
    plt.legend()

    plt.subplot(313)
    plt.errorbar(xx, U, yerr=U, fmt='.r', ecolor='lightgray', elinewidth=0.05,
                 label='AVE={}m,STD={}m,RMSE={}m'.format(Umean, Ustd, Urmse),markersize=1)
    plt.xlabel('历元/30s')
    plt.ylabel('U/m')
    plt.legend()
    plt.show()

程序用到的数据为从CDDIS下载的长春站年积日为114天的观测值数据和广播星历文件、RTKPOST结算出来的.pos文件。

本文代码作为分享发给大家，转载请注明出处哦！