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()