def unpack_deltas_cm( self, dx: Sequence[int], dy: Sequence[int], dz: Sequence[int], frame: GeoFrame ) -> np.ndarray: """ Get coords in world reference system (local ENU->ECEF->world). See the protobuf annotations for additional information about how coordinates are stored Args: dx (Sequence[int]): X displacement in centimeters in local ENU dy (Sequence[int]): Y displacement in centimeters in local ENU dz (Sequence[int]): Z displacement in centimeters in local ENU frame (GeoFrame): geo-location information for the local ENU. It contains lat and long origin of the frame Returns: np.ndarray: array of shape (Nx3) with XYZ coordinates in world ref system """ x = np.cumsum(np.asarray(dx) / 100) y = np.cumsum(np.asarray(dy) / 100) z = np.cumsum(np.asarray(dz) / 100) frame_lat, frame_lng = self._undo_e7(frame.origin.lat_e7), self._undo_e7( frame.origin.lng_e7 ) xyz = np.stack(pm.enu2ecef(x, y, z, frame_lat, frame_lng, 0), axis=-1) xyz = transform_points(xyz, self.ecef_to_world) return xyz
def test_aer_enu(): xyz = pm.aer2ecef(*aer0, *lla0) enu = pm.aer2enu(*aer0) assert enu == approx(enu0) assert pm.aer2enu(*raer0, deg=False) == approx(enu0) with pytest.raises(ValueError): pm.aer2enu(aer0[0], aer0[1], -1) assert pm.enu2ecef(*enu, *lla0) == approx(xyz) assert pm.enu2ecef(*enu, *rlla0, deg=False) == approx(xyz) assert pm.ecef2enu(*xyz, *lla0) == approx(enu) assert pm.ecef2enu(*xyz, *rlla0, deg=False) == approx(enu)
def c(points): x, y, z = p3d.enu2ecef(points[:, 0], points[:, 1], points[:, 2], fromCrs.lat, fromCrs.lon, fromCrs.alt) x, y, z = p3d.ecef2enu(x, y, z, toCrs.lat, toCrs.lon, toCrs.alt) return np.column_stack((x, y, z))
def test_scalar_aer_enu(xyz): """ verify we can handle the wide variety of input data type users might use """ enu = pm.ecef2enu(*xyz, 0, 90, -100) assert pm.enu2ecef(*enu, 0, 90, -100) == approx([0, A, 50])
def test_enu_ecef(enu, lla, xyz): x, y, z = pm.enu2ecef(*enu, *lla) assert x == approx(xyz[0]) assert y == approx(xyz[1]) assert z == approx(xyz[2]) rlla = (radians(lla[0]), radians(lla[1]), lla[2]) assert pm.enu2ecef(*enu, *rlla, deg=False) == approx(xyz) e, n, u = pm.ecef2enu(*xyz, *lla) assert e == approx(enu[0]) assert n == approx(enu[1]) assert u == approx(enu[2]) e, n, u = pm.ecef2enu(*xyz, *rlla, deg=False) assert e == approx(enu[0]) assert n == approx(enu[1]) assert u == approx(enu[2])
def test_scalar_enu(xyz): """ verify we can handle the wide variety of input data type users might use """ if isinstance(xyz[0], list): pytest.importorskip("numpy") enu = pm.ecef2enu(*xyz, 0, 90, -100) assert pm.enu2ecef(*enu, 0, 90, -100) == approx([0, A, 50])
def unpack_deltas_cm( dx: Sequence[int], dy: Sequence[int], dz: Sequence[int], g: GeoFrame # type: ignore ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: x = np.cumsum(np.asarray(dx) / 100) y = np.cumsum(np.asarray(dy) / 100) z = np.cumsum(np.asarray(dz) / 100) x, y, z = pm.enu2ecef(x, y, z, g.origin.lat_e7 * 1e-7, g.origin.lng_e7 * 1e-7, 0) # type: ignore return pm.ecef2enu(x, y, z, lat, lon, 0) # type:ignore
def test_enu_ecef(enu, lla, xyz): x, y, z = pm.enu2ecef(*enu, *lla) assert x == approx(xyz[0]) assert y == approx(xyz[1]) assert z == approx(xyz[2]) assert isinstance(x, float) assert isinstance(y, float) assert isinstance(z, float) rlla = (radians(lla[0]), radians(lla[1]), lla[2]) assert pm.enu2ecef(*enu, *rlla, deg=False) == approx(xyz) e, n, u = pm.ecef2enu(*xyz, *lla) assert e == approx(enu[0]) assert n == approx(enu[1]) assert u == approx(enu[2]) assert isinstance(e, float) assert isinstance(n, float) assert isinstance(u, float) e, n, u = pm.ecef2enu(*xyz, *rlla, deg=False) assert e == approx(enu[0]) assert n == approx(enu[1]) assert u == approx(enu[2])
def extend_flight_path_earth(self, launch_point_LLH): lat, lon, h = pm.enu2geodetic(self.pos_ENU_log[:self.index_landing, 0], self.pos_ENU_log[:self.index_landing, 1], self.pos_ENU_log[:self.index_landing, 2], launch_point_LLH[0], launch_point_LLH[1], launch_point_LLH[2]) # lat, lon, h self.pos_LLH_log = np.c_[lat, lon, h] ecef_x, ecef_y, ecef_z = pm.enu2ecef( self.pos_ENU_log[:self.index_landing, 0], self.pos_ENU_log[:self.index_landing, 1], self.pos_ENU_log[:self.index_landing, 2], launch_point_LLH[0], launch_point_LLH[1], launch_point_LLH[2]) self.pos_ECEF_log = np.c_[ecef_x, ecef_y, ecef_z] plt.figure() plt.plot(self.time_log[:self.index_landing], self.pos_ECEF_log[:self.index_landing, 0] / 1e3, label='X') plt.plot(self.time_log[:self.index_landing], self.pos_ECEF_log[:self.index_landing, 1] / 1e3, label='Y') plt.plot(self.time_log[:self.index_landing], self.pos_ECEF_log[:self.index_landing, 2] / 1e3, label='Z') plt.xlabel('Time [s]') plt.ylabel('Position ECEF [km]') plt.grid() plt.legend() plt.savefig(self.result_name + '_Pos_ECEF.png') kml = simplekml.Kml(open=1) Log_LLH = [] for i in range(len(self.pos_LLH_log[:, 0])): if 0 == i % 10: Log_LLH.append([ self.pos_LLH_log[i, 1], self.pos_LLH_log[i, 0], self.pos_LLH_log[i, 2] ]) line = kml.newlinestring() line.style.linestyle.width = 4 line.style.linestyle.color = simplekml.Color.red line.extrude = 1 line.altitudemode = simplekml.AltitudeMode.absolute line.coords = Log_LLH line.style.linestyle.colormode = simplekml.ColorMode.random kml.save(self.result_name + '_trajectory.kml')
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_3d_enu(): np = pytest.importorskip("numpy") xyz = (np.atleast_3d(0), np.atleast_3d(A), np.atleast_3d(50)) enu = pm.ecef2enu(*xyz, 0, 90, -100) assert pm.enu2ecef(*enu, 0, 90, -100) == approx([0, A, 50])
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')
-2 * np.cross(omega,dx) \ - B2_ref * (T_0 / T) ** alpha * (1 - a * (r - R_E) / T_0) ** alpha * dr * dx / m)) def terminate(t, X): return pm.ecef2enu(*X[:3], phi0, lambda0, h0)[2] < 0.0 # ## Setup # In[178]: t = np.linspace(0, 200, 20000) pos0 = np.asarray(pm.geodetic2ecef(phi0, lambda0, h0, deg=True)) local = pm.utils.sph2cart(A, E, v0) dpos0 = np.asarray(pm.enu2ecef(*local, phi0, lambda0, h0, deg=True)) - pos0 state0 = np.concatenate((pos0, dpos0)) # ## Part A # In[179]: t_magnus, soln_magnus = dsolve(partial(rhs, T=T_0), t, state0, terminate=terminate) t, soln = dsolve(partial(rhs, T=T_0, omega=np.zeros_like(Omega)), t, state0, terminate=terminate)