def test_array_ecef2geodetic():
"""
tests ecef2geodetic can handle numpy array data in addition to singular floats
"""
np = pytest.importorskip("numpy")
# test values with no points inside ellipsoid
lla0_array = (
np.array([lla0[0], lla0[0]]),
np.array([lla0[1], lla0[1]]),
np.array([lla0[2], lla0[2]]),
)
xyz = pm.geodetic2ecef(*lla0_array)
lats, lons, alts = pm.ecef2geodetic(*xyz)
assert lats == approx(lla0_array[0])
assert lons == approx(lla0_array[1])
assert alts == approx(lla0_array[2])
# test values with some (but not all) points inside ellipsoid
lla0_array_inside = (
np.array([lla0[0], lla0[0]]),
np.array([lla0[1], lla0[1]]),
np.array([lla0[2], -lla0[2]]),
)
xyz = pm.geodetic2ecef(*lla0_array_inside)
lats, lons, alts = pm.ecef2geodetic(*xyz)
assert lats == approx(lla0_array_inside[0])
assert lons == approx(lla0_array_inside[1])
assert alts == approx(lla0_array_inside[2])
def test_geodetic():
xyz = pm.geodetic2ecef(*lla0)
assert xyz == approx(xyz0)
assert pm.geodetic2ecef(*rlla0, deg=False) == approx(xyz)
with pytest.raises(ValueError):
pm.geodetic2ecef(lla0[0], lla0[1], -1)
with pytest.raises(ValueError):
pm.geodetic2ecef(-100, lla0[1], lla0[2])
with pytest.raises(ValueError):
pm.geodetic2ecef(lla0[0], -200, lla0[2])
assert pm.ecef2geodetic(*xyz) == approx(lla0)
assert pm.ecef2geodetic(*xyz, deg=False) == approx(rlla0)
lla2 = pm.aer2geodetic(*aer0, *lla0)
rlla2 = pm.aer2geodetic(*raer0, *rlla0, deg=False)
with pytest.raises(ValueError):
pm.aer2geodetic(aer0[0], aer0[1], -1, *lla0)
assert lla2 == approx(lla1)
assert rlla2 == approx(rlla1)
assert pm.geodetic2aer(*lla2, *lla0) == approx(aer0)
assert pm.geodetic2aer(*rlla2, *rlla0, deg=False) == approx(raer0)
anan = np.empty((10, 10))
anan.fill(np.nan)
assert np.isnan(pm.geodetic2aer(anan, anan, anan, *lla0)).all()
assert np.isnan(pm.aer2geodetic(anan, anan, anan, *lla0)).all()
def test_ecef():
xyz = pm.geodetic2ecef(*lla0)
assert xyz == approx(xyz0)
assert pm.geodetic2ecef(*rlla0, deg=False) == approx(xyz)
with pytest.raises(ValueError):
pm.geodetic2ecef(-100, lla0[1], lla0[2])
with pytest.raises(ValueError):
pm.geodetic2ecef(lla0[0], -200, lla0[2])
assert pm.ecef2geodetic(*xyz) == approx(lla0)
assert pm.ecef2geodetic(*xyz, deg=False) == approx(rlla0)
def test_ecef():
xyz = pm.geodetic2ecef(*lla0)
assert xyz == approx(xyz0)
assert pm.geodetic2ecef(*rlla0, deg=False) == approx(xyz)
with pytest.raises(ValueError):
pm.geodetic2ecef(-100, lla0[1], lla0[2])
assert pm.ecef2geodetic(*xyz) == approx(lla0)
assert pm.ecef2geodetic(*xyz, deg=False) == approx(rlla0)
assert pm.ecef2geodetic((A - 1) / np.sqrt(2),
(A - 1) / np.sqrt(2), 0) == approx([0, 45, -1])
def dataFromNC(fnc,fnav,sv,
el_mask=30,tlim=None,
satpos=False,
ipp=False,
ipp_alt=None):
leap_seconds = uf.getLeapSeconds(fnav)
D = gr.load(fnc, useindicators=True).sel(sv=sv)
if tlim is not None:
if len(tlim) == 2:
D = D.where(np.logical_and(D.time >= np.datetime64(tlim[0]), D.time <= np.datetime64(tlim[1])), drop=True)
obstimes64 = D.time.values
dt = np.array([Timestamp(t).to_pydatetime() for t in obstimes64]) - \
datetime.timedelta(seconds = leap_seconds)
rx_xyz = D.position
aer = gpsSatPosition(fnav,dt,sv=sv, rx_position=rx_xyz, coords='aer')
idel = (aer[1] >= el_mask)
aer = aer[:, idel]
dt = dt[idel]
if satpos:
D['az'] = aer[0]
D['el'] = aer[1]
if ipp:
if ipp_alt is None:
print ('Auto assigned altitude of the IPP: 250 km')
ipp_alt = 250e3
else:
ipp_alt *= 1e3
rec_lat, rec_lon, rec_alt = ecef2geodetic(rx_xyz[0], rx_xyz[1], rx_xyz[2])
fm = np.sin(np.radians(aer[1]))
r_new = ipp_alt / fm
lla_vector = np.array(aer2geodetic(aer[0], aer[1], r_new, rec_lat, rec_lon, rec_alt))
D['ipp_lon'] = lla_vector[1]
D['ipp_lat'] = lla_vector[0]
D['ipp_alt'] = ipp_alt
return dt, D, idel
def test_3d_ecef2geodetic():
np = pytest.importorskip("numpy")
xyz = (np.atleast_3d(xyz0[0]), np.atleast_3d(xyz0[1]), np.atleast_3d(xyz0[2]))
lat, lon, alt = pm.ecef2geodetic(*xyz)
assert [lat, lon, alt] == approx(lla0, rel=1e-4)
def test_scalar_ecef2geodetic():
"""
verify we can handle the wide variety of input data type users might use
"""
lat, lon, alt = pm.ecef2geodetic(xyz0[0], xyz0[1], xyz0[2])
assert [lat, lon, alt] == approx(lla0, rel=1e-4)
def payload_parachute_dynamics(x, t, rocket):
pos_ECI = x[0:3]
vel_ECI = x[3:6]
DCM_ECI2ECEF = coord.DCM_ECI2ECEF(t)
pos_ECEF = DCM_ECI2ECEF.dot(pos_ECI)
pos_LLH = pm.ecef2geodetic(pos_ECEF[0], pos_ECEF[1], pos_ECEF[2])
altitude = pos_LLH[2]
DCM_ECEF2NED = coord.DCM_ECEF2NED(rocket.pos0_LLH)
vel_ECEF = coord.vel_ECI2ECEF(vel_ECI, DCM_ECI2ECEF, pos_ECI)
vel_NED = DCM_ECEF2NED.dot(vel_ECEF)
vel_decent = vel_NED[2]
g_NED = np.array([0.0, 0.0, env.gravity(altitude)])
Ta, Pa, rho, Cs = env.std_atmo(altitude)
drag_NED = np.array([
0, 0, -0.5 * rho * vel_decent * np.abs(vel_decent) * rocket.payload.CdS
])
acc_NED = drag_NED / rocket.payload.mass + g_NED
acc_ECI = DCM_ECI2ECEF.transpose().dot(
DCM_ECEF2NED.transpose().dot(acc_NED))
wind_NED = env.Wind_NED(rocket.wind_speed(altitude),
rocket.wind_direction(altitude))
vel_NED = vel_NED + wind_NED
vel_ecef = DCM_ECEF2NED.transpose().dot(vel_NED)
vel_ECI = coord.vel_ECEF2ECI(vel_ecef, DCM_ECI2ECEF, pos_ECI)
dx = np.zeros(6)
dx[0:3] = vel_ECI
dx[3:6] = acc_ECI
return dx
def test_scalar_ecef2geodetic(xyz):
"""
verify we can handle the wide variety of input data type users might use
"""
lat, lon, alt = pm.ecef2geodetic(*xyz)
assert [lat, lon, alt] == approx(lla0, rel=1e-4)
def test_geodetic():
assert_allclose(pm.ecef2geodetic(tx, ty, tz), (ec2la, ec2lo, ec2a),
rtol=0.01,
err_msg='ecef2geodetic: ' +
str(pm.ecef2geodetic(tx, ty, tz)))
assert_allclose(pm.geodetic2aer(lat2, lon2, alt2, tlat, tlon, talt),
(g2az, g2el, g2rn),
rtol=0.05,
err_msg='geodetic2aer: {}'.format(
pm.geodetic2aer(lat2, lon2, alt2, tlat, tlon, talt)))
assert_allclose(pm.geodetic2ecef(tlat, tlon, talt), (g2x, g2y, g2z),
rtol=0.01,
err_msg='geodetic2ecef: {}'.format(
pm.geodetic2ecef(tlat, tlon, talt)))
def gpsSatPosition(fnav, dt, sv=None, rx_position=None, coords='xyz'):
navdata = gr.load(fnav).sel(sv=sv)
timesarray = np.asarray(dt,dtype='datetime64[ns]') #[datetime64 [ns]]
# Manipulate with times, epochs and crap like this
navtimes = navdata.time.values # [datetime64 [ns]]
idnan = np.isfinite(navdata['Toe'].values)
navtimes = navtimes[idnan]
bestephind = []
for t in timesarray:
idt = abs(navtimes - t).argmin() #if t>navtimes[abs(navtimes - t).argmin()] else abs(navtimes - t).argmin()-1
bestephind.append(idt)
# bestephind = np.array([np.argmin(abs(navtimes-t)) for t in timesarray])
gpstime = np.array([getGpsTime(t) for t in dt])
t = gpstime - navdata['Toe'][idnan][bestephind].values # [datetime.datetime]
# constants
GM = 3986005.0E8 # universal gravational constant
OeDOT = 7.2921151467E-5
# Elements
ecc = navdata['Eccentricity'][idnan][bestephind].values # Eccentricity
Mk = navdata['M0'][idnan][bestephind].values + \
t *(np.sqrt(GM / navdata['sqrtA'][idnan][bestephind].values**6) +
navdata['DeltaN'][idnan][bestephind].values)
Ek = solveIter(Mk,ecc)
Vk = np.arctan2(np.sqrt(1.0 - ecc**2) * np.sin(Ek), np.cos(Ek) - ecc)
PhiK = Vk + navdata['omega'][idnan][bestephind].values
# Perturbations
delta_uk = navdata['Cuc'][idnan][bestephind].values * np.cos(2.0*PhiK) + \
navdata['Cus'][idnan][bestephind].values * np.sin(2.0*PhiK)
Uk = PhiK + delta_uk
delta_rk = navdata['Crc'][idnan][bestephind].values * np.cos(2.0*PhiK) + \
navdata['Crs'][idnan][bestephind].values * np.sin(2.0*PhiK)
Rk = navdata['sqrtA'][idnan][bestephind].values**2 * (1.0 - ecc * np.cos(Ek)) + delta_rk
delta_ik = navdata['Cic'][idnan][bestephind].values * np.cos(2.0*PhiK) + \
navdata['Cis'][idnan][bestephind].values * np.sin(2.0*PhiK)
Ik = navdata['Io'][idnan][bestephind].values + \
navdata['IDOT'][idnan][bestephind].values * t + delta_ik
#Compute the right ascension
Omegak = navdata['Omega0'][idnan][bestephind].values + \
(navdata['OmegaDot'][idnan][bestephind].values - OeDOT) * t - \
(OeDOT * navdata['Toe'][idnan][bestephind].values)
#X,Y coordinate corrections
Xkprime = Rk * np.cos(Uk)
Ykprime = Rk * np.sin(Uk)
# ECEF XYZ
X = Xkprime * np.cos(Omegak) - (Ykprime * np.sin(Omegak) * np.cos(Ik))
Y = Xkprime * np.sin(Omegak) + (Ykprime * np.cos(Omegak) * np.cos(Ik))
Z = Ykprime * np.sin(Ik)
if coords == 'xyz':
return np.array([X,Y,Z])
elif coords == 'aer':
assert rx_position is not None
rec_lat, rec_lon, rec_alt = ecef2geodetic(rx_position[0], rx_position[1], rx_position[2])
A,E,R = ecef2aer(X, Y, Z, rec_lat, rec_lon, rec_alt)
return np.array([A,E,R])
def cartesian_to_spherical(vector): theta, phi = get_theta_phi(vector) ECEF = array(([1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0])) if not theta and not phi: phi, theta, _ = pm.ecef2geodetic(vector[0], vector[1], vector[2]) # print(theta, phi) phi = 90 - degrees(arccos(scal(ECEF[2], vector))) return radians(theta), radians(phi)
def ecef_to_geodetic(point: Union[np.ndarray, Sequence[float]]) -> np.ndarray:
"""Convert given ECEF coordinate into latitude, longitude, altitude.
Args:
point (Union[np.ndarray, Sequence[float]]): ECEF coordinate vector
Returns:
np.ndarray: latitude, altitude, longitude
"""
return np.array(pm.ecef2geodetic(point[0], point[1], point[2]))
def plotnav(nav: xarray.Dataset):
if nav is None:
return
svs = nav.sv.values
ax = figure().gca(projection=cartopy.crs.PlateCarree())
ax.add_feature(cpf.LAND)
ax.add_feature(cpf.OCEAN)
ax.add_feature(cpf.COASTLINE)
ax.add_feature(cpf.BORDERS, linestyle=':')
for sv in svs:
if sv[0] == 'S':
lat, lon, alt = ecef2geodetic(nav.sel(sv=sv)['X'].dropna(dim='time', how='all'),
nav.sel(sv=sv)['Y'].dropna(
dim='time', how='all'),
nav.sel(sv=sv)['Z'].dropna(dim='time', how='all'))
assert ((35.7e6 < alt) & (alt < 35.9e6)).all(
), 'unrealistic geostationary satellite altitudes'
assert ((-1 < lat) & (lat < 1)
).all(), 'unrealistic geostationary satellite latitudes'
elif sv[0] == 'R':
lat, lon, alt = ecef2geodetic(nav.sel(sv=sv)['X'].dropna(dim='time', how='all'),
nav.sel(sv=sv)['Y'].dropna(
dim='time', how='all'),
nav.sel(sv=sv)['Z'].dropna(dim='time', how='all'))
assert ((19.0e6 < alt) & (alt < 19.4e6)).all(
), 'unrealistic GLONASS satellite altitudes'
assert ((-67 < lat) & (lat < 67)
).all(), 'GPS inclination ~ 65 degrees'
elif sv[0] == 'G':
ecef = keplerian2ecef(nav.sel(sv=sv))
lat, lon, alt = ecef2geodetic(*ecef)
assert ((19.4e6 < alt) & (alt < 21.0e6)).all(
), 'unrealistic GPS satellite altitudes'
assert ((-57 < lat) & (lat < 57)
).all(), 'GPS inclination ~ 55 degrees'
else:
continue
ax.plot(lon, lat, label=sv)
def getTileChildren(pnts_task):
las_target = pnts_task.requires().output().load()
data = las_target.obj.points['point']
x_min = np.min(data['X'])
x_max = np.max(data['X'])
y_min = np.min(data['Y'])
y_max = np.max(data['Y'])
domain_min = np.dot(np.array([x_min, y_min, 0, 1]), mat_xyz)
domain_max = np.dot(np.array([x_max, x_max, 0, 1]), mat_xyz)
domain_latlon_min = pymap3d.ecef2geodetic(
*tuple(domain_min[0:3].tolist()))
domain_latlon_max = pymap3d.ecef2geodetic(
*tuple(domain_max[0:3].tolist()))
south, west, _ = domain_latlon_min
north, east, _ = domain_latlon_max
target_tile = os.path.basename(pnts_task.output().path)
tileset_def = {
'boundingVolume': {
'region': [
# TODO calcurate accurate bounding box
# west / 180.0 * math.pi, # lon_min
# south / 180.0 * math.pi, # lat_min
# east / 180.0 * math.pi, # lon_max
# north / 180.0 * math.pi, # lat_max
min(domain_lon) / 180.0 * math.pi, # lon_min
min(domain_lat) / 180.0 * math.pi, # lat_min
max(domain_lon) / 180.0 * math.pi, # lon_max
max(domain_lat) / 180.0 * math.pi, # lat_max
min(height_range),
max(height_range),
]
},
'geometricError': 0,
'refine': 'ADD',
'content': {
'url': target_tile,
},
}
return tileset_def
def test_scalar_ecef2geodetic(xyz):
"""
verify we can handle the wide variety of input data type users might use
"""
if isinstance(xyz[0], list):
pytest.importorskip("numpy")
lat, lon, alt = pm.ecef2geodetic(*xyz)
assert [lat, lon, alt] == approx(lla0, rel=1e-4)
def cartesian_to_galactic(system, vector):
vector = unit(vector)
system = normvec(system)
ECEF = array(([1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]))
R = transform_matrix(ECEF, system)
vector_in_system = R.dot(vector)
phi, theta, _ = pm.ecef2geodetic(vector_in_system[0], vector_in_system[1],
vector_in_system[2])
# print(theta, phi)
return theta, phi
def dump_for_hotspot(conn=conn):
c = conn.cursor()
sensors = []
for row in c.execute("SELECT X, Y, Z, Amplitude FROM Sensors;"):
lat, lon, _ = pm.ecef2geodetic(x, y, z)
row = (x, z, Amplitude)
sensors.append(row)
sensors = np.array(sensors)
sensors = latlon_to_fake_xy(sensors)
return sensors
def getIonosphericPiercingPoints(rx_xyz, sv, obstimes, ipp_alt, navfn,
cs='wsg84', rx_xyz_coords='xyz', el0=0):
"""
Sebastijan Mrak
Function returns a list of Ionospheric Piersing Point (IPP) trajectory in WSG84
coordinate system (CS). Function takes as parameter a receiver location in
ECEF CS, satellite number, times ob observation and desired altitude of a IPP
trajectory. You also have to specify a full path to th navigation data file.
It returns IPP location in either WSG84 or AER coordinate system.
"""
ipp_alt = ipp_alt * 1E3
if rx_xyz_coords == 'xyz':
rec_lat, rec_lon, rec_alt = ecef2geodetic(rx_xyz[0], rx_xyz[1], rx_xyz[2])
else:
if not isinstance(rx_xyz, list): rx_xyz = list(rx_xyz)
if len(rx_xyz) == 2: rx_xyz.append(0)
assert len(rx_xyz) == 3
rec_lat = rx_xyz[0]
rec_lon = rx_xyz[1]
rec_alt = rx_xyz[2]
rx_xyz = geodetic2ecef(lat = rec_lon, lon = rec_lat, alt = rec_alt)
if sv[0] == 'G':
if navfn.endswith('n'):
xyz = gpsSatPosition(navfn, obstimes, sv=sv, rx_position=rx_xyz, coords='xyz')
elif navfn.endswith('sp3'):
xyz = gpsSatPositionSP3(navfn, obstimes, sv=sv, rx_position=rx_xyz, coords='xyz')
az,el,r = ecef2aer(xyz[0,:],xyz[1,:],xyz[2,:],rec_lat, rec_lon, rec_alt)
aer_vector = np.array([az, el, r])
r_new = []
for i in range(len(el)):
if el[i] > el0:
fm = np.sin(np.radians(el[i]))
r_new.append(ipp_alt / fm)
else:
r_new.append(np.nan)
lla_vector = np.array(aer2geodetic(az, el, r_new, rec_lat, rec_lon, rec_alt))
elif sv[0] == 'R':
aer_vector = gloSatPosition(navfn=navfn, sv=sv, obstimes=obstimes, rx_position=[rec_lon, rec_lat, rec_alt], cs='aer')
fm = np.sin(np.radians(aer_vector[1]))
r_new = ipp_alt / fm
lla_vector = np.array(aer2geodetic(aer_vector[0], aer_vector[1], r_new, rec_lat, rec_lon, rec_alt))
else:
print ('Type in valid sattype initial. "G" for GPS and "R" for GLONASS')
if (cs == 'wsg84'):
return lla_vector
elif (cs == 'aer'):
return aer_vector
else:
print ('Enter either "wsg84" or "aer" as coordinate system. "wsg84" is default one.')
def RAY_INTERSECT_WGS84(point, ray_dir, t):
#-----WGS 84 --------------
dat_a = 6378137 #Meters
dat_f = 1 / 298.257223563 #flat scale for oblateness
dat_ep = 1 / ((1 - dat_f) * (1 - dat_f))
xd = ray_dir[0] #-31.46071792#-
yd = ray_dir[1] #58.59611618#-
zd = ray_dir[2] #27.47631664#-
xc = point[0]
yc = point[1]
zc = point[2]
qa = xd**2 + yd**2 + (zd**2) * dat_ep
qb = 2 * (xc * xd + yc * yd + zc * zd * dat_ep)
qc = xc**2 + yc**2 + ((zc**2) * dat_ep) - (dat_a**2)
ray_roots = np.roots([qa, qb, qc])
earth_point = []
earth_point.append([
xc + ray_roots[0] * xd, yc + ray_roots[0] * yd, zc + ray_roots[0] * zd
])
earth_point.append([
xc + ray_roots[1] * xd, yc + ray_roots[1] * yd, zc + ray_roots[1] * zd
])
#print(earth_point)
#6371e3 * np.vstack(ecef)
#print('--NORMS '+str(np.linalg.norm(earth_point[0]))+'--:--'+str(np.linalg.norm(earth_point[1]))+'\n')
#print('\n\n ---------- STATE VECTOR 2 NORMAL PROJECTION -------------- \n')
#print(earth_point)
#print(6371e3*np.vstack(earth_point[0]))
earth_point[0] = pm.eci2ecef((earth_point[1]), t)
#earth_point[0]=6371e3*np.array(earth_point[0])
# 6371e3 * np.vstack(ecef)
ecx = -1 * earth_point[0][0]
ecy = -1 * earth_point[0][1]
ecz = -1 * earth_point[0][2]
#print((ecx**2+ecy**2+ecz**2)**0.5)
#print("LAT(deg) LON(deg) HEIGHT(m) :"+str(pm.ecef2geodetic(ecx,ecy,ecz)))
#print("LAT(deg) LON(deg) HEIGHT(m) :"+str(ECI_TO_WGS84LLH(-1*np.array(earth_point[0]),t )))
#print("LAT(deg) LON(deg) HEIGHT(m) :"+str(ECEF_TO_WGS84LLH(-1*np.array(earth_point[0]))))
#earth_point[0]=np.array([ecx,ecy,ecz))/1000
#llh=pm.ecef2geodetic(ecx,ecy,ecz)
# ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
# lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
# lon, lat, alt = pyproj.transform(ecef, lla, ecx, ecy, ecz, radians=False)
llh = pm.ecef2geodetic(ecx, ecy, ecz)
#print("LAT(deg) LON(deg) HEIGHT(m) :"+str(llh))
#print('---------- ------------------------------- -------------- \n')
#print(llh)
return (llh)
def ecef2EnuMat(self, origin, scene_origin = np.array([4.40145e+06, 597011, 4.56599e+06])):
ell = pm3d.EarthEllipsoid(model='wgs84')
phi, lam, alt = pm3d.ecef2geodetic(origin[0] + scene_origin[0], origin[1] + scene_origin[1], origin[2] + scene_origin[2], ell)
""" phi is latitude, lam is longitude
transformation taken from:
https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates
"""
pi_2 = np.pi / 2.0
rx = self.Rx(pi_2 - phi)
rz = self.Rz(pi_2 + lam)
return rx.dot(rz)
def cartesian_to_galactic(system, vector): vector = unit(vector) system = normvec(system) ECEF = array(([1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0])) R = transform_matrix(ECEF, system) vector_in_system = around(R.dot(vector), decimals = 3) # print(vector_in_system) theta, phi = get_theta_phi(vector_in_system) if not theta and not phi: phi, theta, _ = pm.ecef2geodetic(vector_in_system[0], vector_in_system[1], vector_in_system[2]) # print(theta, phi) phi = 90 - degrees(arccos(scal(ECEF[2], vector_in_system))) return theta, phi
def ecef2enu(self, M, origin, scene_origin = np.array([4.40145e+06, 597011, 4.56599e+06])):
"""M is a row-wise atrix of 3D ecef points
origin is a vector in ecef defining the origin for ENU
scene_origin will be added to both origin and each point in M before
the transformation
"""
ell = pm3d.EarthEllipsoid(model='wgs84')
lat, lon, alt = pm3d.ecef2geodetic(origin[0] + scene_origin[0], origin[1] + scene_origin[1], origin[2] + scene_origin[2], ell)
res = []
for i in range(M.shape[0]):
e,n,u = pm3d.ecef2enu(M[i,0] + scene_origin[0], M[i,1] + scene_origin[1], M[i,2] + scene_origin[2], lat, lon, alt, ell)
res.append(np.array([e,n,u]))
return np.array(res)
def test_xarray():
xarray = pytest.importorskip("xarray")
xr_lla = xarray.DataArray(list(lla0))
xyz = pm.geodetic2ecef(*xr_lla)
assert xyz == approx(xyz0)
# %%
xr_xyz = xarray.DataArray(list(xyz0))
lla = pm.ecef2geodetic(*xr_xyz)
assert lla == approx(lla0)
def test_ellipsoid():
assert pm.ecef2geodetic(*xyz0, ell=pm.Ellipsoid('wgs84')) == approx([42.014670535, -82.0064785, 276.9136916])
assert pm.ecef2geodetic(*xyz0, ell=pm.Ellipsoid('grs80')) == approx([42.014670536, -82.0064785, 276.9137385])
assert pm.ecef2geodetic(*xyz0, ell=pm.Ellipsoid('clarke1866')) == approx([42.01680003, -82.0064785, 313.9026793])
assert pm.ecef2geodetic(*xyz0, ell=pm.Ellipsoid('mars')) == approx([42.009428417, -82.006479, 2.981246e6])
assert pm.ecef2geodetic(*xyz0, ell=pm.Ellipsoid('venus')) == approx([41.8233663, -82.0064785, 3.17878159e5])
assert pm.ecef2geodetic(*xyz0, ell=pm.Ellipsoid('moon')) == approx([41.8233663, -82.0064785, 4.630878e6])
def pPosition(self, h: str):
""" Write coordaintes in list and convert to geodetic
"""
try:
self.position = [float(x) for x in h.split()]
try:
from pymap3d import ecef2geodetic
except ImportError:
ecef2geodetic = None
if ecef2geodetic is not None:
self.positionGeodetic = ecef2geodetic(*self.position)
except (KeyError, ValueError):
self.position = None
def plotLOSecef(cam,odir):
fg = figure()
if odir and skml is not None:
kml1d = skml.Kml()
for C in cam:
if not C.usecam:
continue
ax = fg.gca(projection='3d')
ax.plot(xs=C.x2mz, ys=C.y2mz, zs=C.z2mz, zdir='z', label=str(C.name))
ax.set_title('LOS to magnetic zenith in ECEF')
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
ax.set_zlabel('z [m]')
if odir and skml is not None: #Write KML
#convert LOS ECEF -> LLA
loslat,loslon,losalt = ecef2geodetic(C.x2mz, C.y2mz, C.z2mz)
kclr = ['ff5c5ccd','ffff0000']
#camera location points
bpnt = kml1d.newpoint(name='HST {}'.format(C.name),
description='camera {} location'.format(C.name),
coords=[(C.lon, C.lat)])
bpnt.altitudemode = skml.AltitudeMode.clamptoground
bpnt.style.iconstyle.icon.href = 'http://maps.google.com/mapfiles/kml/paddle/pink-blank.png'
bpnt.style.iconstyle.scale = 2.0
#show cam to mag zenith los
linestr = kml1d.newlinestring(name='')
#TODO this is only first and last point without middle!
linestr.coords = [(loslon[0], loslat[0], losalt[0]),
(loslon[-1], loslat[-1], losalt[-1])]
linestr.altitudemode = skml.AltitudeMode.relativetoground
linestr.style.linestyle.color = kclr[C.name]
ax.legend()
if C.verbose:
show()
if odir and skml is not None:
kmlfn = odir / 'debug1dcut.kmz'
print(f'saving {kmlfn}')
kml1d.savekmz(str(kmlfn))
if odir:
ofn = odir / 'ecef_cameras.eps'
print(f'saving {ofn}')
fg.savefig(ofn,bbox_inches='tight')
def __make_log(self, rocket):
self.pos_onlauncer_LLH_log = np.array([
pm.ned2geodetic(pos_NED[0], pos_NED[1], pos_NED[2],
rocket.pos0_LLH[0], rocket.pos0_LLH[1],
rocket.pos0_LLH[2])
for pos_NED in self.pos_onlauncher_NED_log
])
self.downrange_log = np.array([
pm.vincenty.vdist(rocket.pos0_LLH[0], rocket.pos0_LLH[1], pos[0],
pos[1]) for pos in self.pos_LLH_log
])[:, 0]
self.vel_AIR_BODYframe_abs_log = np.array(
[np.linalg.norm(vel) for vel in self.vel_AIR_BODYframe_log])
DCM_NED2BODY_log = np.array(
[coord.DCM_NED2BODY_quat(quat) for quat in self.quat_log])
self.attitude_log = np.array(
[coord.quat2euler(DCM) for DCM in DCM_NED2BODY_log])
self.pos_NED_log = np.array([
pm.ecef2ned(pos_ECEF[0], pos_ECEF[1], pos_ECEF[2],
rocket.pos0_LLH[0], rocket.pos0_LLH[1],
rocket.pos0_LLH[2]) for pos_ECEF in self.pos_ECEF_log
])
self.pos_decent_ECEF_log = np.array([
coord.DCM_ECI2ECEF(t).dot(pos_ECI) for pos_ECI, t in zip(
self.pos_decent_ECI_log, self.time_decent_log)
])
self.pos_decent_LLH_log = np.array([
pm.ecef2geodetic(pos[0], pos[1], pos[2])
for pos in self.pos_decent_ECEF_log
])
self.vel_decent_NED_log = np.array([
coord.DCM_ECEF2NED(rocket.pos0_LLH).dot(
coord.vel_ECI2ECEF(vel_eci, coord.DCM_ECI2ECEF(t), pos_eci))
for vel_eci, t, pos_eci in
zip(self.vel_decent_ECI_log, self.time_decent_log,
self.pos_decent_ECI_log)
])
self.downrange_decent_log = np.array([
pm.vincenty.vdist(rocket.pos0_LLH[0], rocket.pos0_LLH[1], pos[0],
pos[1]) for pos in self.pos_decent_LLH_log
])[:, 0]
self.pos_hard_LLH_log = np.r_[self.pos_onlauncer_LLH_log,
self.pos_LLH_log]
index = np.argmax(self.time_log > self.time_decent_log[0])
self.pos_soft_LLH_log = np.r_[self.pos_onlauncer_LLH_log,
self.pos_LLH_log[:index, :],
self.pos_decent_LLH_log]
def test_xarray():
xarray = pytest.importorskip('xarray')
xr_lla = xarray.DataArray(list(lla0))
xyz = pm.geodetic2ecef(*xr_lla)
assert xyz == approx(xyz0)
assert isinstance(xyz[0], xarray.DataArray)
# %%
xr_xyz = xarray.DataArray(list(xyz0))
lla = pm.ecef2geodetic(*xr_xyz)
assert lla == approx(lla0)
assert isinstance(lla[0], float) # xarrayness is lost, possibly expensive to keep due to isinstance()
def __get_point(lines):
m = numpy.zeros([3, 3])
left = numpy.zeros([3, 3])
right = numpy.zeros([3])
for origin, direction in lines:
m = numpy.identity(3) - \
numpy.column_stack([numpy.array([direction * direction[0]]).T,
numpy.array([direction * direction[1]]).T,
numpy.array([direction * direction[2]]).T])
left += m
right += m.dot(origin)
output = numpy.linalg.inv(left).dot(right)
return numpy.array(pymap3d.ecef2geodetic(*output))
def test_ellipsoid():
assert pm.ecef2geodetic(*xyz0, ell=pm.Ellipsoid('wgs84')) == approx(
[42., -82., 200.24339])
assert pm.ecef2geodetic(*xyz0, ell=pm.Ellipsoid('grs80')) == approx(
[42., -82., 200.24344])
assert pm.ecef2geodetic(*xyz0, ell=pm.Ellipsoid('clrk66')) == approx(
[42.00213, -82., 237.17182])
assert pm.ecef2geodetic(*xyz0, ell=pm.Ellipsoid('mars')) == approx(
[41.99476, -82., 2.981169e6])
assert pm.ecef2geodetic(*xyz0, ell=pm.Ellipsoid('venus')) == approx(
[41.808706, -82., 3.178069e5])
assert pm.ecef2geodetic(*xyz0, ell=pm.Ellipsoid('moon')) == approx(
[41.808706, -82., 4.630807e6])
def parachute_dynamics(x, t, rocket):
pos_ECI = x[0:3]
vel_ECI = x[3:6]
DCM_ECI2ECEF = coord.DCM_ECI2ECEF(t)
pos_ECEF = DCM_ECI2ECEF.dot(pos_ECI)
pos_LLH = pm.ecef2geodetic(pos_ECEF[0], pos_ECEF[1], pos_ECEF[2])
altitude = pos_LLH[2]
DCM_ECEF2NED = coord.DCM_ECEF2NED(rocket.pos0_LLH)
vel_ECEF = coord.vel_ECI2ECEF(vel_ECI, DCM_ECI2ECEF, pos_ECI)
vel_NED = DCM_ECEF2NED.dot(vel_ECEF)
vel_decent = vel_NED[2]
g_NED = np.array([0.0, 0.0, env.gravity(altitude)])
Ta, Pa, rho, Cs = env.std_atmo(altitude)
if rocket.timer_mode:
if t > rocket.t_2nd_max or (altitude <= rocket.alt_sepa2
and t > rocket.t_2nd_min):
CdS = rocket.CdS1 + rocket.CdS2
else:
CdS = rocket.CdS1
else:
CdS = rocket.CdS1 + rocket.CdS2 if altitude <= rocket.alt_sepa2 else rocket.CdS1
drag_NED = np.array(
[0, 0, -0.5 * rho * vel_decent * np.abs(vel_decent) * CdS])
acc_NED = drag_NED / rocket.m_burnout + g_NED
acc_ECI = DCM_ECI2ECEF.transpose().dot(
DCM_ECEF2NED.transpose().dot(acc_NED))
wind_NED = env.Wind_NED(rocket.wind_speed(altitude),
rocket.wind_direction(altitude))
vel_NED = vel_NED + wind_NED
vel_ecef = DCM_ECEF2NED.transpose().dot(vel_NED)
vel_ECI = coord.vel_ECEF2ECI(vel_ecef, DCM_ECI2ECEF, pos_ECI)
dx = np.zeros(6)
dx[0:3] = vel_ECI
dx[3:6] = acc_ECI
return dx
def test_ecef2geodetic(xyz, lla):
assert pm.ecef2geodetic(*xyz) == approx(lla)
def test_somenan():
xyz = np.stack((xyz0, (nan, nan, nan)))
lat, lon, alt = pm.ecef2geodetic(xyz[:, 0], xyz[:, 1], xyz[:, 2])
assert (lat[0], lon[0], alt[0]) == approx(lla0)
def obsheader2(f: TextIO,
useindicators: bool = False,
meas: Sequence[str] = None) -> Dict[str, Any]:
"""
End users should use rinexheader()
"""
if isinstance(f, (str, Path)):
with opener(f, header=True) as h:
return obsheader2(h, useindicators, meas)
f.seek(0)
# %% selection
if isinstance(meas, str):
meas = [meas]
if not meas or not meas[0].strip():
meas = None
hdr = rinexinfo(f)
Nobs = 0 # not None due to type checking
for ln in f:
if "END OF HEADER" in ln:
break
h = ln[60:80].strip()
c = ln[:60]
# %% measurement types
if '# / TYPES OF OBSERV' in h:
if Nobs == 0:
Nobs = int(c[:6])
hdr[h] = c[6:].split()
else:
hdr[h] += c[6:].split()
elif h not in hdr: # Header label
hdr[h] = c # string with info
else: # concatenate
hdr[h] += " " + c
# %% useful values
try:
hdr['systems'] = hdr['RINEX VERSION / TYPE'][40]
except KeyError:
pass
hdr['Nobs'] = Nobs
# 5 observations per line (incorporating LLI, SSI)
hdr['Nl_sv'] = ceil(hdr['Nobs'] / 5)
# %% list with receiver location in x,y,z cartesian ECEF (OPTIONAL)
try:
hdr['position'] = [float(j) for j in hdr['APPROX POSITION XYZ'].split()]
if ecef2geodetic is not None:
hdr['position_geodetic'] = ecef2geodetic(*hdr['position'])
except (KeyError, ValueError):
pass
# %% observation types
try:
hdr['fields'] = hdr['# / TYPES OF OBSERV']
if Nobs != len(hdr['fields']):
raise ValueError(f'{f.name} header read incorrectly')
if isinstance(meas, (tuple, list, np.ndarray)):
ind = np.zeros(len(hdr['fields']), dtype=bool)
for m in meas:
for i, f in enumerate(hdr['fields']):
if f.startswith(m):
ind[i] = True
hdr['fields_ind'] = np.nonzero(ind)[0]
else:
ind = slice(None)
hdr['fields_ind'] = np.arange(Nobs)
hdr['fields'] = np.array(hdr['fields'])[ind].tolist()
except KeyError:
pass
hdr['Nobsused'] = hdr['Nobs']
if useindicators:
hdr['Nobsused'] *= 3
# %%
try:
hdr['# OF SATELLITES'] = int(hdr['# OF SATELLITES'][:6])
except (KeyError, ValueError):
pass
# %% time
try:
hdr['t0'] = _timehdr(hdr['TIME OF FIRST OBS'])
except (KeyError, ValueError):
pass
try:
hdr['t1'] = _timehdr(hdr['TIME OF LAST OBS'])
except (KeyError, ValueError):
pass
try: # This key is OPTIONAL
hdr['interval'] = float(hdr['INTERVAL'][:10])
except (KeyError, ValueError):
pass
return hdr
def navtimeseries(nav: xarray.Dataset):
if not isinstance(nav, xarray.Dataset):
return
svs = nav.sv.values
if cartopy is not None:
ax = figure().gca(projection=cartopy.crs.PlateCarree())
ax.add_feature(cpf.LAND)
ax.add_feature(cpf.OCEAN)
ax.add_feature(cpf.COASTLINE)
ax.add_feature(cpf.BORDERS, linestyle=':')
else:
ax = figure().gca()
for sv in svs:
if sv[0] == 'S':
lat, lon, alt = pm.ecef2geodetic(nav.sel(sv=sv)['X'].dropna(dim='time', how='all'),
nav.sel(sv=sv)['Y'].dropna(
dim='time', how='all'),
nav.sel(sv=sv)['Z'].dropna(dim='time', how='all'))
if ((alt < 35.7e6) | (alt > 35.9e6)).any():
logging.warning('unrealistic geostationary satellite altitudes')
if ((lat < -1) | (lat > 1)).any():
logging.warning('unrealistic geostationary satellite latitudes')
elif sv[0] == 'R':
lat, lon, alt = pm.ecef2geodetic(nav.sel(sv=sv)['X'].dropna(dim='time', how='all'),
nav.sel(sv=sv)['Y'].dropna(
dim='time', how='all'),
nav.sel(sv=sv)['Z'].dropna(dim='time', how='all'))
if ((alt < 19.0e6) | (alt > 19.4e6)).any():
logging.warning('unrealistic GLONASS satellite altitudes')
if ((lat < -67) | (lat > 67)).any():
logging.warning('GLONASS inclination ~ 65 degrees')
elif sv[0] == 'G':
ecef = keplerian2ecef(nav.sel(sv=sv))
lat, lon, alt = pm.ecef2geodetic(*ecef)
if ((alt < 19.4e6) | (alt > 21.0e6)).any():
logging.warning('unrealistic GPS satellite altitudes')
if ((lat < -57) | (lat > 57)).any():
logging.warning('GPS inclination ~ 55 degrees')
elif sv[0] == 'E':
ecef = keplerian2ecef(nav.sel(sv=sv))
lat, lon, alt = pm.ecef2geodetic(*ecef)
if ((alt < 23e6) | (alt > 24e6)).any():
logging.warning('unrealistic Galileo satellite altitudes')
if ((lat < -57) | (lat > 57)).any():
logging.warning('Galileo inclination ~ 56 degrees')
else:
continue
ax.plot(lon, lat, label=sv)
def obsheader3(f: TextIO,
use: Sequence[str] = None,
meas: Sequence[str] = None) -> Dict[str, Any]:
"""
get RINEX 3 OBS types, for each system type
optionally, select system type and/or measurement type to greatly
speed reading and save memory (RAM, disk)
"""
if isinstance(f, (str, Path)):
with opener(f, header=True) as h:
return obsheader3(h, use, meas)
fields = {}
Fmax = 0
# %% first line
hdr = rinexinfo(f)
for ln in f:
if "END OF HEADER" in ln:
break
h = ln[60:80]
c = ln[:60]
if 'SYS / # / OBS TYPES' in h:
k = c[0]
fields[k] = c[6:60].split()
N = int(c[3:6])
# %% maximum number of fields in a file, to allow fast Numpy parse.
Fmax = max(N, Fmax)
n = N-13
while n > 0: # Rinex 3.03, pg. A6, A7
ln = f.readline()
assert 'SYS / # / OBS TYPES' in ln[60:]
fields[k] += ln[6:60].split()
n -= 13
assert len(fields[k]) == N
continue
if h.strip() not in hdr: # Header label
hdr[h.strip()] = c # don't strip for fixed-width parsers
# string with info
else: # concatenate to the existing string
hdr[h.strip()] += " " + c
# %% list with x,y,z cartesian (OPTIONAL)
try:
hdr['position'] = [float(j) for j in hdr['APPROX POSITION XYZ'].split()]
if ecef2geodetic is not None:
hdr['position_geodetic'] = ecef2geodetic(*hdr['position'])
except (KeyError, ValueError):
pass
# %% time
try:
t0s = hdr['TIME OF FIRST OBS']
# NOTE: must do second=int(float()) due to non-conforming files
hdr['t0'] = datetime(year=int(t0s[:6]), month=int(t0s[6:12]), day=int(t0s[12:18]),
hour=int(t0s[18:24]), minute=int(t0s[24:30]), second=int(float(t0s[30:36])),
microsecond=int(float(t0s[30:43]) % 1 * 1000000))
except (KeyError, ValueError):
pass
try:
hdr['interval'] = float(hdr['INTERVAL'][:10])
except (KeyError, ValueError):
pass
# %% select specific satellite systems only (optional)
if use is not None:
if not set(fields.keys()).intersection(use):
raise KeyError(f'system type {use} not found in RINEX file')
fields = {k: fields[k] for k in use if k in fields}
# perhaps this could be done more efficiently, but it's probably low impact on overall program.
# simple set and frozenset operations do NOT preserve order, which would completely mess up reading!
sysind = {}
if isinstance(meas, (tuple, list, np.ndarray)):
for sk in fields: # iterate over each system
# ind = np.isin(fields[sk], meas) # boolean vector
ind = np.zeros(len(fields[sk]), dtype=bool)
for m in meas:
for i, f in enumerate(fields[sk]):
if f.startswith(m):
ind[i] = True
fields[sk] = np.array(fields[sk])[ind].tolist()
sysind[sk] = np.empty(Fmax*3, dtype=bool) # *3 due to LLI, SSI
for j, i in enumerate(ind):
sysind[sk][j*3:j*3+3] = i
else:
sysind = {k: slice(None) for k in fields}
hdr['fields'] = fields
hdr['fields_ind'] = sysind
hdr['Fmax'] = Fmax
return hdr
