def test_ned():
xyz = pm.aer2ecef(*aer0, *lla0)
enu = pm.aer2enu(*aer0)
ned = (enu[1], enu[0], -enu[2])
lla = pm.aer2geodetic(*aer0, *lla0)
assert pm.aer2ned(*aer0) == approx(ned0)
with pytest.raises(ValueError):
pm.aer2ned(aer0[0], aer0[1], -1)
assert pm.enu2aer(*enu) == approx(aer0)
assert pm.enu2aer(*enu, deg=False) == approx(raer0)
assert pm.ned2aer(*ned) == approx(aer0)
assert pm.ecef2ned(*xyz, *lla0) == approx(ned)
assert pm.ned2ecef(*ned, *lla0) == approx(xyz)
# %%
assert pm.ecef2enuv(vx, vy, vz, *lla0[:2]) == approx((ve, vn, vu))
assert pm.ecef2nedv(vx, vy, vz, *lla0[:2]) == approx((vn, ve, -vu))
# %%
enu3 = pm.geodetic2enu(*lla, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert pm.geodetic2ned(*lla, *lla0) == approx(ned3)
assert pm.enu2geodetic(*enu3, *lla0) == approx(lla)
assert pm.ned2geodetic(*ned3, *lla0) == approx(lla)
def test_ned():
xyz = pm.aer2ecef(*aer0, *lla0)
enu = pm.aer2enu(*aer0)
ned = (enu[1], enu[0], -enu[2])
lla = pm.aer2geodetic(*aer0, *lla0)
assert pm.aer2ned(*aer0) == approx(ned0)
with pytest.raises(ValueError):
pm.aer2ned(aer0[0], aer0[1], -1)
assert pm.enu2aer(*enu) == approx(aer0)
assert pm.enu2aer(*enu, deg=False) == approx(raer0)
assert pm.ned2aer(*ned) == approx(aer0)
assert pm.ecef2ned(*xyz, *lla0) == approx(ned)
assert pm.ned2ecef(*ned, *lla0) == approx(xyz)
# %%
assert pm.ecef2enuv(vx, vy, vz, *lla0[:2]) == approx((ve, vn, vu))
assert pm.ecef2nedv(vx, vy, vz, *lla0[:2]) == approx((vn, ve, -vu))
# %%
enu3 = pm.geodetic2enu(*lla, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert pm.geodetic2ned(*lla, *lla0) == approx(ned3)
assert pm.enu2geodetic(*enu3, *lla0) == approx(lla)
assert pm.ned2geodetic(*ned3, *lla0) == approx(lla)
def test_ecef_ned():
enu = pm.aer2enu(*aer0)
ned = (enu[1], enu[0], -enu[2])
xyz = pm.aer2ecef(*aer0, *lla0)
n, e, d = pm.ecef2ned(*xyz, *lla0)
assert n == approx(ned[0])
assert e == approx(ned[1])
assert d == approx(ned[2])
assert pm.ned2ecef(*ned, *lla0) == approx(xyz)
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_ecef2ned():
dst_ecef = my_blh2ecef(lat, lon, hig)
obs_ecef = my_blh2ecef(lat_o, lon_o, hig_o)
ned_lib_ans = pm.ecef2ned(dst_ecef[0], dst_ecef[1], dst_ecef[2],
lat_o, lon_o, hig_o)
my_ned_ans = my_ecef2ned(dst_ecef[0], dst_ecef[1], dst_ecef[2],
obs_ecef[0], obs_ecef[1], obs_ecef[2])
print("result_ned : ", my_ned_ans)
print("answer_ned : ", ned_lib_ans)
diff = [abs(j - i) for (i, j) in zip(my_ned_ans, ned_lib_ans)]
print("ned diff : ", diff)
return my_ned_ans
def test_ecefenu():
assert_allclose(pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt),
(a2x, a2y, a2z),
rtol=0.01,
err_msg='aer2ecef: {}'.format(
pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt)))
assert_allclose(pm.aer2enu(taz, tel, tsrange), (a2e, a2n, a2u),
rtol=0.01,
err_msg='aer2enu: ' + str(pm.aer2enu(taz, tel, tsrange)))
assert_allclose(pm.aer2ned(taz, tel, tsrange), (a2n, a2e, -a2u),
rtol=0.01,
err_msg='aer2ned: ' + str(pm.aer2ned(taz, tel, tsrange)))
assert_allclose(pm.ecef2enu(tx, ty, tz, tlat, tlon, talt), (e2e, e2n, e2u),
rtol=0.01,
err_msg='ecef2enu: {}'.format(
pm.ecef2enu(tx, ty, tz, tlat, tlon, talt)))
assert_allclose(pm.ecef2enuv(vx, vy, vz, tlat, tlon), (ve, vn, vu))
assert_allclose(pm.ecef2ned(tx, ty, tz, tlat, tlon, talt),
(e2n, e2e, -e2u),
rtol=0.01,
err_msg='ecef2ned: {}'.format(
pm.ecef2enu(tx, ty, tz, tlat, tlon, talt)))
assert_allclose(pm.ecef2nedv(vx, vy, vz, tlat, tlon), (vn, ve, -vu))
assert_allclose(pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt),
(a2la, a2lo, a2a),
rtol=0.01,
err_msg='aer2geodetic {}'.format(
pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt)))
assert_allclose(pm.ecef2aer(tx, ty, tz, tlat, tlon, talt),
(ec2az, ec2el, ec2rn),
rtol=0.01,
err_msg='ecef2aer {}'.format(
pm.ecef2aer(a2x, a2y, a2z, tlat, tlon, talt)))
#%%
assert_allclose(pm.enu2aer(te, tn, tu), (e2az, e2el, e2rn),
rtol=0.01,
err_msg='enu2aer: ' + str(pm.enu2aer(te, tn, tu)))
assert_allclose(pm.ned2aer(tn, te, -tu), (e2az, e2el, e2rn),
rtol=0.01,
err_msg='enu2aer: ' + str(pm.enu2aer(te, tn, tu)))
assert_allclose(pm.enu2geodetic(te, tn, tu, tlat, tlon,
talt), (lat2, lon2, alt2),
rtol=0.01,
err_msg='enu2geodetic: ' +
str(pm.enu2geodetic(te, tn, tu, tlat, tlon, talt)))
assert_allclose(pm.ned2geodetic(tn, te, -tu, tlat, tlon,
talt), (lat2, lon2, alt2),
rtol=0.01,
err_msg='enu2geodetic: ' +
str(pm.enu2geodetic(te, tn, tu, tlat, tlon, talt)))
assert_allclose(pm.enu2ecef(te, tn, tu, tlat, tlon, talt), (e2x, e2y, e2z),
rtol=0.01,
err_msg='enu2ecef: ' +
str(pm.enu2ecef(te, tn, tu, tlat, tlon, talt)))
assert_allclose(
pm.ned2ecef(tn, te, -tu, tlat, tlon, talt), (e2x, e2y, e2z),
rtol=0.01,
err_msg='ned2ecef: ' + str(pm.ned2ecef(tn, te, -tu, tlat, tlon, talt)))
def test_geodetic(self):
if pyproj:
ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
xyz1 = pm.geodetic2ecef(*lla0)
assert_allclose(pm.geodetic2ecef(*rlla0, deg=False),
xyz1,
err_msg='geodetic2ecef: rad')
assert_allclose(xyz1, xyz0, err_msg='geodetic2ecef: deg')
assert_allclose(pm.ecef2geodetic(*xyz1),
lla0,
err_msg='ecef2geodetic: deg')
assert_allclose(pm.ecef2geodetic(*xyz1, deg=False),
rlla0,
err_msg='ecef2geodetic: rad')
if pyproj:
assert_allclose(
pyproj.transform(lla, ecef, lla0[1], lla0[0], lla0[2]), xyz1)
assert_allclose(pyproj.transform(ecef, lla, *xyz1),
(lla0[1], lla0[0], lla0[2]))
lla2 = pm.aer2geodetic(*aer0, *lla0)
rlla2 = pm.aer2geodetic(*raer0, *rlla0, deg=False)
assert_allclose(lla2, lla1, err_msg='aer2geodetic: deg')
assert_allclose(rlla2, rlla1, err_msg='aer2geodetic:rad')
assert_allclose(pm.geodetic2aer(*lla2, *lla0),
aer0,
err_msg='geodetic2aer: deg')
assert_allclose(pm.geodetic2aer(*rlla2, *rlla0, deg=False),
raer0,
err_msg='geodetic2aer: rad')
# %% aer-ecef
xyz2 = pm.aer2ecef(*aer0, *lla0)
assert_allclose(pm.aer2ecef(*raer0, *rlla0, deg=False),
axyz0,
err_msg='aer2ecef:rad')
assert_allclose(xyz2, axyz0, err_msg='aer2ecef: deg')
assert_allclose(pm.ecef2aer(*xyz2, *lla0),
aer0,
err_msg='ecef2aer:deg')
assert_allclose(pm.ecef2aer(*xyz2, *rlla0, deg=False),
raer0,
err_msg='ecef2aer:rad')
# %% aer-enu
enu1 = pm.aer2enu(*aer0)
ned1 = (enu1[1], enu1[0], -enu1[2])
assert_allclose(enu1, enu0, err_msg='aer2enu: deg')
assert_allclose(pm.aer2enu(*raer0, deg=False),
enu0,
err_msg='aer2enu: rad')
assert_allclose(pm.aer2ned(*aer0), ned0, err_msg='aer2ned')
assert_allclose(pm.enu2aer(*enu1), aer0, err_msg='enu2aer: deg')
assert_allclose(pm.enu2aer(*enu1, deg=False),
raer0,
err_msg='enu2aer: rad')
assert_allclose(pm.ned2aer(*ned1), aer0, err_msg='ned2aer')
# %% enu-ecef
assert_allclose(pm.enu2ecef(*enu1, *lla0),
xyz2,
err_msg='enu2ecef: deg')
assert_allclose(pm.enu2ecef(*enu1, *rlla0, deg=False),
xyz2,
err_msg='enu2ecef: rad')
assert_allclose(pm.ecef2enu(*xyz2, *lla0),
enu1,
err_msg='ecef2enu:deg')
assert_allclose(pm.ecef2enu(*xyz2, *rlla0, deg=False),
enu1,
err_msg='ecef2enu:rad')
assert_allclose(pm.ecef2ned(*xyz2, *lla0), ned1, err_msg='ecef2ned')
assert_allclose(pm.ned2ecef(*ned1, *lla0), xyz2, err_msg='ned2ecef')
# %%
assert_allclose(pm.ecef2enuv(vx, vy, vz, *lla0[:2]), (ve, vn, vu))
assert_allclose(pm.ecef2nedv(vx, vy, vz, *lla0[:2]), (vn, ve, -vu))
# %%
enu3 = pm.geodetic2enu(*lla2, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert_allclose(pm.geodetic2ned(*lla2, *lla0),
ned3,
err_msg='geodetic2ned: deg')
assert_allclose(pm.enu2geodetic(*enu3, *lla0),
lla2,
err_msg='enu2geodetic')
assert_allclose(pm.ned2geodetic(*ned3, *lla0),
lla2,
err_msg='ned2geodetic')
def test_geodetic(self):
if pyproj:
ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
x1, y1, z1 = pm.geodetic2ecef(tlat, tlon, talt)
assert_allclose(
pm.geodetic2ecef(radians(tlat), radians(tlon), talt, deg=False),
(x1, y1, z1))
assert_allclose((x1, y1, z1), (x0, y0, z0), err_msg='geodetic2ecef')
assert_allclose(pm.ecef2geodetic(x1, y1, z1), (tlat, tlon, talt),
err_msg='ecef2geodetic')
if pyproj:
assert_allclose(pyproj.transform(lla, ecef, tlon, tlat, talt),
(x1, y1, z1))
assert_allclose(pyproj.transform(ecef, lla, x1, y1, z1),
(tlon, tlat, talt))
lat2, lon2, alt2 = pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt)
assert_allclose((lat2, lon2, alt2), (lat1, lon1, alt1),
err_msg='aer2geodetic')
assert_allclose(pm.geodetic2aer(lat2, lon2, alt2, tlat, tlon, talt),
(taz, tel, tsrange),
err_msg='geodetic2aer')
x2, y2, z2 = pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt)
assert_allclose(
pm.aer2ecef(radians(taz),
radians(tel),
tsrange,
radians(tlat),
radians(tlon),
talt,
deg=False), (a2x, a2y, a2z))
assert_allclose((x2, y2, z2), (a2x, a2y, a2z), err_msg='aer2ecef')
assert_allclose(pm.ecef2aer(x2, y2, z2, tlat, tlon, talt),
(taz, tel, tsrange),
err_msg='ecef2aer')
e1, n1, u1 = pm.aer2enu(taz, tel, tsrange)
assert_allclose((e1, n1, u1), (e0, n0, u0), err_msg='aer2enu')
assert_allclose(pm.aer2ned(taz, tel, tsrange), (n0, e0, -u0),
err_msg='aer2ned')
assert_allclose(pm.enu2aer(e1, n1, u1), (taz, tel, tsrange),
err_msg='enu2aer')
assert_allclose(pm.ned2aer(n1, e1, -u1), (taz, tel, tsrange),
err_msg='ned2aer')
assert_allclose(pm.enu2ecef(e1, n1, u1, tlat, tlon, talt),
(x2, y2, z2),
err_msg='enu2ecef')
assert_allclose(pm.ecef2enu(x2, y2, z2, tlat, tlon, talt),
(e1, n1, u1),
err_msg='ecef2enu')
assert_allclose(pm.ecef2ned(x2, y2, z2, tlat, tlon, talt),
(n1, e1, -u1),
err_msg='ecef2ned')
assert_allclose(pm.ned2ecef(n1, e1, -u1, tlat, tlon, talt),
(x2, y2, z2),
err_msg='ned2ecef')
# %%
assert_allclose(pm.ecef2enuv(vx, vy, vz, tlat, tlon), (ve, vn, vu))
assert_allclose(pm.ecef2nedv(vx, vy, vz, tlat, tlon), (vn, ve, -vu))
#%%
e3, n3, u3 = pm.geodetic2enu(lat2, lon2, alt2, tlat, tlon, talt)
assert_allclose(pm.geodetic2ned(lat2, lon2, alt2, tlat, tlon, talt),
(n3, e3, -u3))
assert_allclose(pm.enu2geodetic(e3, n3, u3, tlat, tlon, talt),
(lat2, lon2, alt2),
err_msg='enu2geodetic')
assert_allclose(pm.ned2geodetic(n3, e3, -u3, tlat, tlon, talt),
(lat2, lon2, alt2),
err_msg='ned2geodetic')