应粉丝需求,现发布较完整版,实用的一套源码,改源码可进行RTKLIB的误差分析,曲线图的绘制,结果图直观易懂
效果展示
源码1
# -*- 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()
源码2
# -*- coding:utf-8 -*-
"""
created on Fri Apr 29 14:19:25 2022
@author: xymeng
"""
import numpy as np
import math as mt
import matplotlib.pyplot as plt
import sklearn.metrics as ms
'''以下为WGS84椭球参数'''
ef = 1.0 / 298.257223563
e2 = ef * (2.0 - ef) # 椭球第一偏心率的平方
a = 6378137.0 # 参考椭球的长半轴, 单位 m
'''以下为三角函数调用'''
sqrt = mt.sqrt
sin = mt.sin
cos = mt.cos
atan = mt.atan
atan2 = mt.atan2
pi = mt.pi
'''以下为列表定义'''
X = []
Y = []
Z = []#单点定位结果
X1 = []
Y1 = []
Z1 = []#huber抗差定位结果
X2 = []
Y2 = []
Z2 = []#IGGIII定位结果
X3 = []
Y3 = []
Z3 = []#KF定位结果
X0 = []
Y0 = []
Z0 = []#参考真值
delX = []
delY = []
delZ = []#单点定位两者差值
delX1 = []
delY1 = []
delZ1 = []#Huber抗差定位两者差值
delX2 = []
delY2 = []
delZ2 = []#IGGIII抗差定位两者差值
delX3 = []
delY3 = []
delZ3 = []#KF定位两者差值
XT=[]
YT=[]
ZT=[]#将真值放到列表中
XT1=[]
YT1=[]
ZT1=[]#将真值放到列表中
XT2=[]
YT2=[]
ZT2=[]#将真值放到列表中
XT3=[]
YT3=[]
ZT3=[]#将真值放到列表中
E = []
N = []
U = []#单点定位ENU
E1 = []
N1 = []
U1 = []#Huber抗差定位ENU
E2 = []
N2 = []
U2 = []#IGGIII抗差定位ENU
E3 = []
N3 = []
U3 = []#KF定位ENU
'''以下为弧度转换角度函数'''
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, e2, a):
"""
该函数实现把某点在参心空间直角坐标系下的坐标(X, Y, Z)转为大地坐标(B, L, H).
:param X: X方向坐标,单位 m
:param Y: Y方向坐标, 单位 m
:param Z: Z方向坐标, 单位 m
:param e2: 椭球第一偏心率的平方
:param a: 地球短半轴,即大地坐标系原点到两级的距离, 单位 m
:return: B, L, H, 大地纬度、经度、海拔高度 (m)
"""
v = a
z = Z
zk = 0.0
r2 = X**2+Y**2
while abs(z - zk) > 1e-4:
zk = z
sinp = z / sqrt(r2 + z * z)
v = a / sqrt(1.0 - e2 * sinp * sinp)
z = Z + v * e2 * sinp
if r2 > 1e-12:
B = atan(z / sqrt(r2))
L = atan2(Y,X)
else:
L = 0.0
if Z > 0.0:
B = pi / 2.0
else:
B = -pi / 2.0
H = sqrt(r2+z*z)-v
return B, L, H # 返回大地纬度B、经度L、海拔高度H (m)
'''以下函数为大地坐标转站心地平坐标'''
def BLH2ENU(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: E ,N, U, 东方向,北方向,天顶方向(m)
"""
E = -sin(L)*(X-X0)+cos(L)*(Y-Y0)
N = -sin(B)*cos(L)*(X-X0)-sin(B)*sin(L)*(Y-Y0)+cos(B)*(Z-Z0)
U = cos(B)*cos(L)*(X-X0)+cos(B)*sin(L)*(Y-Y0)+sin(B)*(Z-Z0)
return E,N,U
if __name__ == '__main__':
path = input('请输入第一个单点定位结果文件:') # E://rtklib//rtklib-test1//不同方案对比数据//Huber权函数抗差最小二乘//MA36-x3-G+C-lsq.pos
path1 = input('请输入第二个Huber抗差定位结果文件:') # E://rtklib//rtklib-test1//不同方案对比数据//Huber权函数抗差最小二乘//MA36-x3G+C-huberls1.5.pos
path2 = input('请输入第三个IGGIII抗差定位结果文件:') # E://rtklib//rtklib-test1//不同方案对比数据//IGGIII权函数抗差最小二乘//MA36-x3-G+C-iggls1.5-6.pos
path3 = input('请输入第四个KF定位结果文件:') # E://rtklib//rtklib-test1//不同方案对比数据//静态Kalman滤波//MA36-x3-G+C-kf.pos
path0 = input('请输入pos参考真值文件:') # E://rtklib//rtklib-test1//data//MA36x3-ppp.pos
'''以下代码是读取单点定位结果所有行的XYZ'''
with open(path, 'r') as f:
lines = f.readlines()
for line in lines[0:len(lines)]:
if (line.strip('\n').split(' ')[0]) != '%':
X.append(float(line.split(' ')[1]))
Y.append(float(line.split(' ')[2]))
Z.append(float(line.split(' ')[3]))
'''以下代码是读取Huber抗差定位结果所有行的XYZ'''
with open(path1, 'r') as f:
lines = f.readlines()
for line in lines[0:len(lines)]:
if (line.strip('\n').split(' ')[0]) != '%':
X1.append(float(line.split(' ')[1]))
Y1.append(float(line.split(' ')[2]))
Z1.append(float(line.split(' ')[3]))
'''以下代码是读取IGGIII抗差定位结果所有行的XYZ'''
with open(path2, 'r') as f:
lines = f.readlines()
for line in lines[0:len(lines)]:
if (line.strip('\n').split(' ')[0]) != '%':
X2.append(float(line.split(' ')[1]))
Y2.append(float(line.split(' ')[2]))
Z2.append(float(line.split(' ')[3]))
'''以下代码是读取KF定位结果所有行的XYZ'''
with open(path3, 'r') as f:
lines = f.readlines()
for line in lines[0:len(lines)]:
if (line.strip('\n').split(' ')[0]) != '%':
X3.append(float(line.split(' ')[1]))
Y3.append(float(line.split(' ')[2]))
Z3.append(float(line.split(' ')[3]))
'''以下代码是读取参考真值所有行的XYZ'''
with open(path0, 'r') as f:
lines = f.readlines()
for line in lines[0:len(lines)]:
if (line.strip('\n').split(' ')[0]) != '%':
X0.append(float(line.split(' ')[1]))
Y0.append(float(line.split(' ')[2]))
Z0.append(float(line.split(' ')[3]))
'''以下行是将定位均值设为XYZ真值'''
Xt = np.mean(X0)
Yt = np.mean(Y0)
Zt = np.mean(Z0)
'''以下是计算真值对应的BLH'''
B0,L0,H0 = XYZ2BLH(Xt,Yt,Zt,e2,a)
'''下面为计算出各行对应的NEU'''
for l in range(len(X)):
E0,N0,U0 = BLH2ENU(B0,L0,X[l],Y[l],Z[l],Xt,Yt,Zt)
E.append(E0)
N.append(N0)
U.append(U0)
'''下面为计算出各行对应的NEU'''
for l in range(len(X1)):
E0, N0, U0 = BLH2ENU(B0, L0, X1[l], Y1[l], Z1[l], Xt, Yt, Zt)
E1.append(E0)
N1.append(N0)
U1.append(U0)
'''下面为计算出各行对应的NEU'''
for l in range(len(X2)):
E0, N0, U0 = BLH2ENU(B0, L0, X2[l], Y2[l], Z2[l], Xt, Yt, Zt)
E2.append(E0)
N2.append(N0)
U2.append(U0)
'''下面为计算出各行对应的NEU'''
for l in range(len(X3)):
E0, N0, U0 = BLH2ENU(B0, L0, X3[l], Y3[l], Z3[l], Xt, Yt, Zt)
E3.append(E0)
N3.append(N0)
U3.append(U0)
'''以下为定义单点定位NEU真值列表'''
NN = []
EE = []
UU = []
for k in range(len(X)):
NN.append(0)
EE.append(0)
UU.append(0)
'''以下为定义Huber抗差定位NEU真值列表'''
NN1 = []
EE1 = []
UU1 = []
for k in range(len(X1)):
NN1.append(0)
EE1.append(0)
UU1.append(0)
'''以下为定义IGGIII抗差定位NEU真值列表'''
NN2 = []
EE2 = []
UU2 = []
for k in range(len(X2)):
NN2.append(0)
EE2.append(0)
UU2.append(0)
'''以下为定义KF定位NEU真值列表'''
NN3 = []
EE3 = []
UU3 = []
for k in range(len(X3)):
NN3.append(0)
EE3.append(0)
UU3.append(0)
'''下面为计算ENU的均值、标准差和均方根误差'''
Emean = round(np.mean(E),4)
Emean1 = round(np.mean(E1),4)
Emean2 = round(np.mean(E2),4)
Emean3 = round(np.mean(E3),4)
Nmean = round(np.mean(N),4)
Nmean1 = round(np.mean(N1),4)
Nmean2 = round(np.mean(N2),4)
Nmean3 = round(np.mean(N3),4)
Umean = round(np.mean(U),4)
Umean1 = round(np.mean(U1),4)
Umean2 = round(np.mean(U2),4)
Umean3 = round(np.mean(U3),4)
Estd = round(np.std(E, ddof=1),4)
Estd1 = round(np.std(E1, ddof=1),4)
Estd2 = round(np.std(E2, ddof=1),4)
Estd3 = round(np.std(E3, ddof=1),4)
Nstd = round(np.std(N, ddof=1),4)
Nstd1 = round(np.std(N1, ddof=1),4)
Nstd2 = round(np.std(N2, ddof=1),4)
Nstd3 = round(np.std(N3, ddof=1),4)
Ustd = round(np.std(U, ddof=1),4)
Ustd1 = round(np.std(U1, ddof=1),4)
Ustd2 = round(np.std(U2, ddof=1),4)
Ustd3 = round(np.std(U3, ddof=1),4)
Ermse = round(sqrt(ms.mean_squared_error(EE,E)),4)
Ermse1 = round(sqrt(ms.mean_squared_error(EE1,E1)),4)
Ermse2 = round(sqrt(ms.mean_squared_error(EE2,E2)),4)
Ermse3 = round(sqrt(ms.mean_squared_error(EE3,E3)),4)
Nrmse = round(sqrt(ms.mean_squared_error(NN,N)),4)
Nrmse1 = round(sqrt(ms.mean_squared_error(NN1,N1)),4)
Nrmse2 = round(sqrt(ms.mean_squared_error(NN2,N2)),4)
Nrmse3 = round(sqrt(ms.mean_squared_error(NN3,N3)),4)
Urmse = round(sqrt(ms.mean_squared_error(UU,U)),4)
Urmse1 = round(sqrt(ms.mean_squared_error(UU1,U1)),4)
Urmse2 = round(sqrt(ms.mean_squared_error(UU2,U2)),4)
Urmse3 = round(sqrt(ms.mean_squared_error(UU3,U3)),4)
'''以下为定义收敛图的x坐标范围'''
xx = [i for i in range(1, len(X) + 1)]
'''以下为定义收敛图的x坐标范围'''
xx1 = [i for i in range(1, len(X1) + 1)]
'''以下为定义收敛图的x坐标范围'''
xx2 = [i for i in range(1, len(X2) + 1)]
'''以下为定义收敛图的x坐标范围'''
xx3 = [i for i in range(1, len(X3) + 1)]
'''以下为绘制收敛曲线图'''
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.errorbar(xx1, E1, yerr=E1, fmt='.b', ecolor='lightgray', elinewidth=0.05,
label='AVE={}m,STD={}m,RMSE={}m'.format(Emean1, Estd1, Ermse1), markersize=1)
plt.errorbar(xx2, E2, yerr=E2, fmt='.g', ecolor='lightgray', elinewidth=0.05,
label='AVE={}m,STD={}m,RMSE={}m'.format(Emean2, Estd2, Ermse2), markersize=1)
plt.errorbar(xx3, E3, yerr=E3, fmt='.k', ecolor='lightgray', elinewidth=0.05,
label='AVE={}m,STD={}m,RMSE={}m'.format(Emean3, Estd3, Ermse3), markersize=1)
plt.ylabel('E/m')
plt.legend(loc="upper right")
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.errorbar(xx1, N1, yerr=N1, fmt='.b', ecolor='lightgray', elinewidth=0.05,
label='AVE={}m,STD={}m,RMSE={}m'.format(Nmean1, Nstd1, Nrmse1), markersize=1)
plt.errorbar(xx2, N2, yerr=N2, fmt='.g', ecolor='lightgray', elinewidth=0.05,
label='AVE={}m,STD={}m,RMSE={}m'.format(Nmean2, Nstd2, Nrmse2), markersize=1)
plt.errorbar(xx3, N3, yerr=N3, fmt='.k', ecolor='lightgray', elinewidth=0.05,
label='AVE={}m,STD={}m,RMSE={}m'.format(Nmean3, Nstd3, Nrmse3), markersize=1)
plt.ylabel('N/m')
plt.legend(loc="upper right")
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.errorbar(xx1, U1, yerr=U1, fmt='.b', ecolor='lightgray', elinewidth=0.05,
label='AVE={}m,STD={}m,RMSE={}m'.format(Umean1, Ustd1, Urmse1), markersize=1)
plt.errorbar(xx2, U2, yerr=U2, fmt='.g', ecolor='lightgray', elinewidth=0.05,
label='AVE={}m,STD={}m,RMSE={}m'.format(Umean2, Ustd2, Urmse2), markersize=1)
plt.errorbar(xx3, U3, yerr=U3, fmt='.k', ecolor='lightgray', elinewidth=0.05,
label='AVE={}m,STD={}m,RMSE={}m'.format(Umean3, Ustd3, Urmse3), markersize=1)
plt.xlabel('历元/s')
plt.ylabel('U/m')
plt.legend(loc="upper right")
plt.legend()
plt.show()