博主这周研究了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文件。
本文代码作为分享发给大家,转载请注明出处哦!