You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
209 lines
7.4 KiB
Python
209 lines
7.4 KiB
Python
import math
|
|
import pymap3d.ecef
|
|
import pymap3d.eci
|
|
import pymap3d.aer
|
|
import pymap3d.ellipsoid
|
|
from datetime import datetime, timedelta
|
|
from dateutil.relativedelta import relativedelta
|
|
import pandas as pd
|
|
|
|
|
|
class StandardDistance():
|
|
def __init__(self, strline):
|
|
s = "#" + strline
|
|
self.satelliteID = s[1:8]
|
|
self.measureType = s[8:10]
|
|
self.flagTime10 = s[10:11]
|
|
self.flagTime11 = s[11:12]
|
|
self.surveyStation = s[12:17].strip()
|
|
self.timeMeasureYear = int(s[17:19])
|
|
self.timeMeasureDay = int(s[19:22])
|
|
self.timeMeasureSecond = int(s[22:27])
|
|
self.timeMeasureMicroSecond = int(s[27:33])
|
|
self.flagFixedIonosphere = s[33:34]
|
|
self.flagFixedTroposphere = s[34:35]
|
|
self.flagDataEdition = s[35:36]
|
|
self.flagFixedCentroidShift = s[36:37]
|
|
self.flagPhaseContinuity = s[37:38]
|
|
self.flagFixedPhaseCenter = s[38:39]
|
|
# space in 39
|
|
self.UHFdistance_mm = float(s[40:52]) # important
|
|
self.dataCompressionPoint = s[52:55]
|
|
self.standardPointsWindow = s[55:56]
|
|
# space in 56
|
|
self.weatherSurfacePressure = s[57:61]
|
|
self.weatherSurfaceTemperature = s[61:64]
|
|
self.weatherRelativeHumidity = s[64:67]
|
|
self.measurementStandardDeviation_mm = s[67:73]
|
|
self.ionosphericReflectionCorrection_mm = s[73:80]
|
|
self.convectiveReflectionCorrection_mm = s[80:86]
|
|
self.centroidOffsetCorrection_mm = s[86:91]
|
|
self.phaseCenterCorrection_mm = s[91:97]
|
|
self.launchStation = s[97:101].strip()
|
|
# c, light speed
|
|
self.lightSpeed_m_per_s = 299792458
|
|
|
|
|
|
class AzimuthAndElevationAngle():
|
|
def __init__(self, strline):
|
|
s = "#" + strline
|
|
self.satelliteID = s[1:8]
|
|
self.measureType = s[8:10]
|
|
self.flagTime10 = s[10:11]
|
|
self.flagTime11 = s[11:12]
|
|
self.surveyStation = s[12:17].strip()
|
|
self.timeMeasureYear = int(s[17:19])
|
|
self.timeMeasureDay = int(s[19:22])
|
|
self.timeMeasureSecond = int(s[22:27])
|
|
self.timeMeasureMicroSecond = int(s[27:33])
|
|
self.flagAngle = s[33:34]
|
|
self.flagFixedTroposphere = s[34:35]
|
|
self.flagDataEdition = s[35:36]
|
|
self.angleA_degree = float(s[36:39])
|
|
self.angleA_minute = float(s[39:41])
|
|
self.angleA_second = float(s[41:46])
|
|
self.angleE_degree = float(s[46:49])
|
|
self.angleE_minute = float(s[49:51])
|
|
self.angleE_second = float(s[51:56])
|
|
# space in 56-57
|
|
self.angleA_StandardDeviation = s[58:62]
|
|
self.angleE_StandardDeviation = s[62:66]
|
|
# space in 66
|
|
self.angleA_TroposphereReflectionCorrection = s[67:72]
|
|
self.angleE_TroposphereReflectionCorrection = s[72:77]
|
|
self.CDP_LaserSystemID = s[77:79]
|
|
self.CDP_SN = s[79:81]
|
|
# space in 81-90
|
|
|
|
|
|
class OpticalData():
|
|
def __init__(self, strline):
|
|
s = "#" + strline
|
|
self.satelliteID = s[1:8]
|
|
self.measureType = s[8:10]
|
|
self.flagTime10 = s[10:11]
|
|
self.flagTime11 = s[11:12]
|
|
self.surveyStation = s[12:17].strip()
|
|
self.timeMeasureYear = int(s[17:19])
|
|
self.timeMeasureDay = int(s[19:22])
|
|
self.timeMeasureSecond = int(s[22:27])
|
|
self.timeMeasureMicroSecond = int(s[27:33])
|
|
# space in 33,34
|
|
self.flagDataEdition = s[35:36]
|
|
self.IDEGA = s[36:39]
|
|
self.IMINA = s[39:41]
|
|
self.SECA = s[41:46]
|
|
self.IDEGE = s[46:49]
|
|
self.IMINE = s[49:51]
|
|
self.SECE = s[51:56]
|
|
# space in 56,57
|
|
self.SIGMAA = s[58:62]
|
|
self.SIGMAE = s[62:66]
|
|
|
|
|
|
station_position = {}
|
|
|
|
|
|
# ID = (lat, lon, h)
|
|
def station_reader(filename):
|
|
with open(filename, 'r') as f:
|
|
for line in f.readlines()[1:]:
|
|
strline = line.split()
|
|
if len(strline) > 0:
|
|
station_position[strline[0]] = (float(strline[3]),
|
|
float(strline[2]),
|
|
float(strline[1]))
|
|
|
|
|
|
def distance_3D(vec1, vec2):
|
|
return math.sqrt(sum((x - y)**2 for x, y in zip(vec1, vec2)))
|
|
|
|
|
|
def read_vector_R(str_dist, str_AE):
|
|
dist_obj = StandardDistance(str_dist)
|
|
AE_obj = AzimuthAndElevationAngle(str_AE)
|
|
angleA = AE_obj.angleA_degree + AE_obj.angleA_minute / 60 + AE_obj.angleA_second / 3600
|
|
angleE = AE_obj.angleE_degree + AE_obj.angleE_minute / 60 + AE_obj.angleE_second / 3600
|
|
distance = dist_obj.UHFdistance_mm / 1000 # mm to m
|
|
aer = (angleA, angleE, distance)
|
|
obs = station_position[dist_obj.surveyStation]
|
|
launch = station_position[dist_obj.launchStation]
|
|
|
|
wgs84 = pymap3d.ellipsoid.Ellipsoid()
|
|
p_star_fake = pymap3d.aer.aer2ecef(*aer, *obs, wgs84) # fake
|
|
p_obs = pymap3d.ecef.geodetic2ecef(*obs, wgs84)
|
|
p_lau = pymap3d.ecef.geodetic2ecef(*launch, wgs84)
|
|
|
|
a, b, c = distance_3D(p_star_fake,
|
|
p_obs), distance_3D(p_lau, p_obs), distance_3D(
|
|
p_star_fake, p_lau),
|
|
cos_theta = (a * a + b * b - c * c) / (2 * a * b)
|
|
delta_x = (b * b - distance * distance) / (2 * b * cos_theta -
|
|
2 * distance)
|
|
|
|
aer2 = (aer[0], aer[1], delta_x)
|
|
x, y, z = pymap3d.aer.aer2ecef(*aer2, *obs, wgs84) # real
|
|
obs_time = datetime(2000, 1, 1) + relativedelta(
|
|
years=dist_obj.timeMeasureYear) + timedelta(
|
|
days=dist_obj.timeMeasureDay,
|
|
seconds=dist_obj.timeMeasureSecond,
|
|
microseconds=dist_obj.timeMeasureMicroSecond)
|
|
x, y, z = pymap3d.eci.ecef2eci(x, y, z, time=obs_time)
|
|
return (x[0], y[0], z[0]), obs_time
|
|
|
|
|
|
def diff_v(pos_list, time_h):
|
|
'''
|
|
pos_list = [(x1, y1, z1), (x2, y2, z2)...]
|
|
time_h = time gap, such as 1s
|
|
'''
|
|
# n = len(pos_list)//2
|
|
# down = time_h * n * (n + 1) * (2 * n + 1)
|
|
# sumX, sumY, sumZ = 0, 0, 0
|
|
# for (x, y, z) in pos_list:
|
|
# sumX += x
|
|
# sumY += y
|
|
# sumZ += z
|
|
# # n = 2m+1 , return v_(m+1) if start count form 1
|
|
# return (sumX * 3 / down, sumY * 3 / down, sumZ * 3 / down)
|
|
return [(y - x) / (2 * time_h) for x, y in zip(pos_list[0], pos_list[2])]
|
|
|
|
|
|
if __name__ == "__main__":
|
|
R_list = []
|
|
T_list = []
|
|
V_list = []
|
|
Vpos_list = []
|
|
VT_list = []
|
|
station_reader(r"C:\Users\ASUS\Desktop\天智杯科目一测试数据\01输入文件示例\站点坐标.dat")
|
|
with open(
|
|
r"C:\Users\ASUS\Desktop\天智杯科目一测试数据\01输入文件示例\data\20200720\20200720120000_000877_1376.odp"
|
|
) as f:
|
|
odplist = f.readlines()
|
|
for distline, aerline in zip(odplist[0::2], odplist[1::2]):
|
|
R, T = read_vector_R(distline, aerline)
|
|
R_list.append(R)
|
|
T_list.append(T)
|
|
if len(R_list) == 3:
|
|
V = diff_v(R_list, time_h=1)
|
|
V_list.append(V)
|
|
VT_list.append(T_list[1])
|
|
Vpos_list.append(R_list[1])
|
|
T_list.clear()
|
|
R_list.clear()
|
|
|
|
aso_datas = []
|
|
ID = 1
|
|
for v, vp, t in zip(V_list, Vpos_list, VT_list):
|
|
print(v,vp,t)
|
|
aso_data = {}
|
|
aso_data['aso_name'] = ID
|
|
aso_data['aso_id'] = ID
|
|
aso_data['epoch'] = t
|
|
aso_data['r_x'], aso_data['r_y'], aso_data['r_z'] = vp
|
|
aso_data['v_x'], aso_data['v_y'], aso_data['v_z'] = v
|
|
aso_data['object_type'] = 'xs'
|
|
aso_datas.append(aso_data)
|
|
res = pd.DataFrame(aso_datas)
|
|
print(res)
|
|
# res.to_parquet() |