calculate r and v(not very precise)
haha020
4 years ago
commit
ea65214b0c
@ -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()
|
||||
@ -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)
|
||||
@ -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))
|
||||
Loading…
Reference in new issue