commit ea65214b0c29659a132663f0eb832c58f94e5cc7 Author: haha020 Date: Fri Nov 5 15:07:12 2021 +0800 calculate r and v(not very precise) diff --git a/odpReader2.py b/odpReader2.py new file mode 100644 index 0000000..20c9b3f --- /dev/null +++ b/odpReader2.py @@ -0,0 +1,209 @@ +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() \ No newline at end of file diff --git a/pymap.py b/pymap.py new file mode 100644 index 0000000..212e76f --- /dev/null +++ b/pymap.py @@ -0,0 +1,15 @@ +from numpy.core.arrayprint import DatetimeFormat +import pymap3d.ecef +import pymap3d.eci +import pymap3d.aer +import pymap3d.ellipsoid +from datetime import datetime + +obs = (13.0344804066577, 77.5111003779841, 839.009756775573) +aer = (123.08, 50.06, 37350340.0) + +wgs84 = pymap3d.ellipsoid.Ellipsoid() +x,y,z = pymap3d.aer.aer2ecef(*aer,*obs, wgs84) +print(x,y,z) +x,y,z = pymap3d.eci.ecef2eci(x,y,z,time=datetime(1999,4,2,3,0,0)) +print(x,y,z) diff --git a/test.py b/test.py new file mode 100644 index 0000000..b091da2 --- /dev/null +++ b/test.py @@ -0,0 +1,65 @@ +from math import * +import numpy as np +import pymap3d.ecef + +EARTH_ECC = 0.01671022 +EARTH_R = 6385607.4105288265 # equator km + + +def LLA_to_ECEF(lat, lon, h): + deg2rad = pi / 180 + lat = lat * deg2rad + lon = lon * deg2rad + print((EARTH_R + h), cos(lat) ,cos(lon), (EARTH_R + h) * cos(lat) * cos(lon)) + X = (EARTH_R + h) * cos(lat) * cos(lon) + Y = (EARTH_R + h) * cos(lat) * sin(lon) + Z = (EARTH_R * (1 - EARTH_ECC**2) + h) * sin(lat) + print(X, Y, Z) + return (X, Y, Z) + + +def local_to_enu(aer): + A, E, R = aer + deg2rad = pi / 180 + A = A * deg2rad + E = E * deg2rad + e = R * cos(E) * sin(A) + n = R * cos(E) * cos(A) + u = R * sin(E) + return (e, n, u) + + +def enu_to_ECEF(p0_LLA, enu): + lat, lon, h = p0_LLA + e, n, u = enu + deg2rad = pi / 180 + lat = lat * deg2rad + lon = lon * deg2rad + mat = [[-sin(lon), -sin(lat) * cos(lon), cos(lat) * cos(lon)], + [cos(lon), -sin(lat) * sin(lon), cos(lat) * sin(lon)], + [0, cos(lat), sin(lat)]] + mat = np.array(mat) + # print(mat, enu) + delta = mat @ np.array([e, n, u]).reshape(-1, 1) + p0_xyz = np.array([*LLA_to_ECEF(*p0_LLA)]).reshape(-1, 1) + # print("--------", delta, p0_xyz) + return p0_xyz + delta + + +def observe_to_ECEF(station, aer): + enu = local_to_enu(aer) + r = enu_to_ECEF(station, enu) + return r + + +if __name__ == "__main__": + # lat, lon, h + # obs = (13.0344804066577, 77.5111003779841, 0.839009756775573) + # # aer = (132.67, 32.44, 16945.450) + # aer = (123.08, 50.06, 37350.340) + # # obs = (0, 0, 1000) + # # aer = (0, 90, 1000) + # ans = observe_to_ECEF(obs, aer) + # print(ans) + LLA_to_ECEF(36.2295894, 93.0661482, 746.049) + print(pymap3d.ecef.geodetic2ecef(36.2295894, 93.0661482, 746.049)) \ No newline at end of file