calculate r and v(not very precise)

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

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)

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