# -*- coding: utf-8 -*-
import numpy as np
import math
from datetime import datetime, timedelta
# 获取文件夹内的所有文件名
import os
import re
# WGS84椭球参数
a = 6378137.0 # 参考椭球的长半轴, 单位 m
f = 1 / 298.257223563 # 参考椭球的扁率
b = 6356752.31414 # 参考椭球的短半轴, 单位 m
'''以下为三角函数调用'''
sqrt = math.sqrt
sin = math.sin
cos = math.cos
atan = math.atan
atan2 = math.atan2
fabs = math.fabs
# 求列表平均值
def avg(list):
sum = 0
if len(list) > 0:
for i in list:
sum += i
return sum / len(list)
else:
return 0
# 度分格式转度格式
def dm2dd(B):
B1 = int(B[:B.find('.') - 2]) + round(float(B[B.find('.') - 2:]), 10) / 60
return round(B1, 10)
def XYZ2BLH(X, Y, Z):
"""
该函数实现把某点在参心空间直角坐标系下的坐标（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)
"""
R = sqrt(X * X + Y * Y)
B0 = atan2(Z, R)
while 1:
N = a / sqrt(1 - f * (2 - f) * sin(B0) * sin(B0))
B_ = atan2(Z + N * f * (2 - f) * sin(B0), R)
if (fabs(B_ - B0) < 1.0e-7):
break
B0 = B_
L_ = atan2(Y, X)
H = R / cos(B_) - N
B = math.degrees(B_)
L = math.degrees(L_)
return round(B, 10), round(L, 10), round(H, 3)
'''该函数把某点的大地坐标(B, L, H)转换为空间直角坐标（X, Y, Z)'''
def BLH2XYZ(B, L, H):
"""
该函数把某点的大地坐标(B, L, H)转换为空间直角坐标（X, Y, Z).
:param B: 大地纬度, 角度制, 规定南纬为负，范围为[-90, 90]
:param L: 大地经度, 角度制, 规定西经为负, 范围为[-180, 180]
:param H: 海拔，大地高, 单位 m
:param a: 地球长半轴，即赤道半径，单位 m
:param b: 地球短半轴，即大地坐标系原点到两级的距离, 单位 m
:return: X, Y, Z, 空间直角坐标, 单位 m
"""
B = math.radians(B) # 角度转为弧度
L = math.radians(L) # 角度转为弧度
e = sqrt((a ** 2 - b ** 2) / (a ** 2)) # 参考椭球的第一偏心率
N = a / sqrt(1 - e * e * sin(B) * sin(B)) # 卯酉圈半径, 单位 m
X = (N + H) * cos(B) * cos(L)
Y = (N + H) * cos(B) * sin(L)
Z = (N * (1 - e * e) + H) * sin(B)
return round(X, 4), round(Y, 4), round(Z, 4) # 返回空间直角坐标(X, Y, Z)
'''以下函数为大地坐标转站心地平坐标'''
def XYZ2NEU(X, Y, Z, X1, Y1, Z1):
"""
该函数实现把某点在参心空间直角坐标系下的坐标（X, Y, Z)转为站心地平坐标(E,N,U).
X,Y,Z 定位结果的XYZ值
X1,Y1,Z1 参考坐标的XYZ值
:return: N, E ,U, 东方向，北方向，天顶方向(m)
"""
dx = X - X1
dy = Y - Y1
dz = Z - Z1
B, L, H = XYZ2BLH(X1, Y1, Z1)
B = math.radians(B)
L = math.radians(L)
N = -sin(B) * cos(L) * dx - sin(B) * sin(L) * dy + cos(B) * dz
E = -sin(L) * dx + cos(L) * dy
U = cos(B) * cos(L) * dx + cos(B) * sin(L) * dy + sin(B) * dz
H = sqrt(pow(N, 2) + pow(E, 2)) # 平面方向高程
error = sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))
return round(E, 4), round(N, 4), round(U, 4)
# 获取所有XYZ文件
def get_files():
rtucppp_posfiles = []
# path = 'D:\算法终端定位工具\RTUCPPP批量采集\配置文件'
path = r'E:\DPI\安庆周边基站NRTK&中移服务对比测试\11.18-\test'
for root, dirs, files in os.walk(path):
print(root)
if 'out' in root:
for i in files:
# 删除历史数据
# if '_DPI_' in i:
# os.remove(root+'\\'+i)
if '.pos' in i and int(i[10:15]) >= 22432:
# print(root+'\\'+i)
rtucppp_posfiles.append(root + '\\' + i)
print(rtucppp_posfiles)
return rtucppp_posfiles
file = get_files()
keyonglv = []
t = datetime.now().strftime("%m%d%H%M")
for posfile in file:
filename = posfile.split("\\")
station_id = re.sub(r'[\u4e00-\u9fa5]', '', filename[-1])[:4]
error_nei_h = []
error_nei_u = []
X = []
Y = []
Z = []
nei_e = []
nei_n = []
nei_u = []
wai_e = []
wai_n = []
wai_u = []
error_wai_H = []
error_wai_U = []
if 'pos' in posfile:
with open(posfile, 'r') as XYZfile:
countdabiao = 0
for line in XYZfile:
# if 'POS:' in line and line[:5]!='POS:5':
# print(line)
temp = line.split()
if len(temp) >= 23 and float(temp[9]) != 0:
X.append(float(temp[23]))
Y.append(float(temp[24]))
Z.append(float(temp[25]))
wai_e.append(float(temp[9]))
wai_n.append(float(temp[10]))
wai_u.append(float(temp[11]))
# 用于计算平均值坐标
t_X1 = sorted(X)
t_Y1 = sorted(Y)
t_Z1 = sorted(Z)
t_X = [i for i in t_X1[int(len(t_X1) / 2) - int(len(t_X1) / 4):int(len(t_X1) / 2) + int(len(t_X1) / 4)]]
t_Y = [i for i in t_Y1[int(len(t_Y1) / 2) - int(len(t_Y1) / 4):int(len(t_Y1) / 2) + int(len(t_Y1) / 4)]]
t_Z = [i for i in t_Z1[int(len(t_Z1) / 2) - int(len(t_Z1) / 4):int(len(t_Z1) / 2) + int(len(t_Z1) / 4)]]
# 平均值坐标
av_X = round(avg(t_X), 4)
av_Y = round(avg(t_Y), 4)
av_Z = round(avg(t_Z), 4)
# 计算内符合精度
Convergence_time1 = 0 # 用于计算收敛时间
Convergence_time = [] # 用于计算收敛时间
for i in range(len(X)):
nfh_e = abs(XYZ2NEU(X[i], Y[i], Z[i], av_X, av_Y, av_Z)[0])
nfh_n = abs(XYZ2NEU(X[i], Y[i], Z[i], av_X, av_Y, av_Z)[1])
nfh_u = abs(XYZ2NEU(X[i], Y[i], Z[i], av_X, av_Y, av_Z)[2])
if nfh_e <= 0.03 and nfh_n <= 0.03 and nfh_u <= 0.05:
countdabiao = countdabiao + 1
Convergence_time1 = Convergence_time1 + 1
else:
Convergence_time1 = 0
if Convergence_time1 == 100:
Convergence_time.append(i - 99)
if nfh_e <= 0.03 * 100000 and nfh_n <= 0.03 * 100000 and nfh_u <= 0.05 * 100000: # 统计满足平面3cm，高程5cm的定位点
nei_e.append(XYZ2NEU(X[i], Y[i], Z[i], av_X, av_Y, av_Z)[0])
nei_n.append(XYZ2NEU(X[i], Y[i], Z[i], av_X, av_Y, av_Z)[1])
nei_u.append(XYZ2NEU(X[i], Y[i], Z[i], av_X, av_Y, av_Z)[2])
error_wai_H.append(sqrt(pow(wai_e[i], 2) + pow(wai_n[i], 2)))
if 'CM100' in posfile:
error_wai_U.append(abs(wai_u[i] - 0.121))
elif 'STHCR3-G3' in posfile:
error_wai_U.append(abs(wai_u[i] - 0.123))
elif 'HITAT45101CP' in posfile:
error_wai_U.append(abs(wai_u[i] - 0.144))
elif 'CHCAT661' in posfile:
error_wai_U.append(abs(wai_u[i] - 0.134))
elif 'CHCC220GR' in posfile:
error_wai_U.append(abs(wai_u[i] - 0.115))
elif 'HXCCGX601A' in posfile:
error_wai_U.append(abs(wai_u[i] - 0.21))
else:
error_wai_U.append(abs(wai_u[i]))
for i in range(len(nei_e)):
# 计算水平精度和垂直精度
error_nei_h.append(sqrt(pow(nei_e[i], 2) + pow(nei_n[i], 2)))
error_nei_u.append(abs(nei_u[i]))
error_nei_h.sort()
error_nei_u.sort()
error_wai_H.sort()
error_wai_U.sort()
if Convergence_time == []:
Convergence_time.append(9999)
# 精度达标时间
if len(X) >= 1:
dabiao = round(countdabiao / len(X) * 100, 2)
else:
dabiao = 0
if len(error_nei_h) > 0:
av_B = XYZ2BLH(av_X, av_Y, av_Z)[0]
av_L = XYZ2BLH(av_X, av_Y, av_Z)[1]
resu = filename[-1][:-4] + ',定位可用率,' + str(dabiao) + '%,' + \
'总点数,' + str(len(X)) + ',满足精度点数,' + str(countdabiao) + ',收敛时间,' + str(Convergence_time[0]) + \
',内:H(95%),' + str(round(error_nei_h[int(len(error_nei_h) * 0.95)] * 100, 2)) + \
',内:V(95%),' + str(round(error_nei_u[int(len(error_nei_u) * 0.95)] * 100, 2)) + \
',外:H(95%),' + str(round(error_wai_H[int(len(error_wai_H) * 0.95)] * 100, 2)) + \
',外:V(95%),' + str(round(error_wai_U[int(len(error_wai_U) * 0.95)] * 100, 2)) + \
',' + str(dabiao) + ',' + filename[-1][:4] + '_' + str(dabiao) + ',' + str(av_L)[:9] + ',' + str(
av_B)[:9]
print(resu)
with open('D:\算法终端定位工具\RTUCPPP批量采集\\精度计算结果' + t + '.txt', 'a+') as resultfile:
resultfile.writelines(resu + '\n')本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
