def convertMapData(lat, lon, hgt, lat0=32.775776, lon0=35.024963, alt0=229): """Convert lat/lon/height data to grid data.""" n, e, d = pymap3d.geodetic2ned(lat, lon, hgt, lat0=lat0, lon0=lon0, h0=alt0) x, y, z = e, n, -d xi = np.linspace(-10000, 10000, 300) yi = np.linspace(-10000, 10000, 300) X, Y = np.meshgrid(xi, yi) Z = ml.griddata(y.flatten(), x.flatten(), z.flatten(), yi, xi, interp='linear') Z_mask = calcSeaMask(Z) return X, Y, Z, Z_mask
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 fix_rear_callback(self, data): lat = data.latitude lon = data.longitude alt = data.altitude self.lat_rear = data.latitude self.lon_rear = data.longitude self.alt_rear = data.altitude self.pos_rear_ned = np.asarray( pm.geodetic2ned(lat, lon, alt, self.lat0, self.lon0, self.alt0))
def distanceFromGoal(self): geodetic_coord = (self.current_pose.latitude, self.current_pose.longitude, self.current_pose.altitude) x, y, z = pm.geodetic2ned(*geodetic_coord, *self.reference_pose) d_x = float(self.goal_pose[0]) - x d_y = float(self.goal_pose[1]) - y d_z = float(self.goal_pose[2]) + z distance = math.sqrt(d_x * d_x + d_y * d_y + d_z * d_z) self.info_msg('Distance from goal is: ' + str(distance)) return distance
def convertMapData(lat, lon, hgt, lat0=32.775776, lon0=35.024963, alt0=229): """Convert lat/lon/height data to grid data.""" n, e, d = pymap3d.geodetic2ned(lat, lon, hgt, lat0=lat0, lon0=lon0, h0=alt0) x, y, z = e, n, -d return x, y
def load_path(flight_path=FLIGHT_PATH, lat0=32.775776, lon0=35.024963, alt0=229): """Load the flight path.""" file_paths = sorted(glob.glob(os.path.join(flight_path, '*.json'))) data = [] indices = [] lat = [] lon = [] alt = [] relative_alt = [] for file_path in file_paths: with open(file_path, 'rb') as f: d = json.load(f) if len(d['data'] ) == 0 or d['coords'] is None or d['coords']['lat'] == 0: # # ignore corrupt data. # continue t = datetime.datetime(*[ int(i) for i in os.path.split(file_path)[-1].split('.')[0].split('_') ]) indices.append(t) data.append(d['data']) lat.append(d['coords']['lat'] * 1e-7) lon.append(d['coords']['lon'] * 1e-7) alt.append(d['coords']['alt'] * 1e-3) relative_alt.append(d['coords']['relative_alt'] * 1e-3) data = np.array(data)[..., :-1] df = pd.DataFrame(data=data, index=indices, columns=COLUMNS) # # Convert lat/lon/height data to grid data # n, e, d = pymap3d.geodetic2ned(lat, lon, alt, lat0=lat0, lon0=lon0, h0=alt0) x, y, z = e, n, -d return df, x, y, z
def draw_catalog(self, show_all=True): if self.core.project is None: return scaling = self.ui.scaleSpinBox.value() t_max = datetime.max if show_all else self.core.project.project_time events = [ e for e in self.core.project.seismic_catalog.seismic_events if e.date_time < t_max ] loc = np.array([(e.lat, e.lon, e.depth) for e in events]) mag = np.array([e.magnitude for e in events]) * scaling ref = self.core.project.reference_point lat, lon, h = loc[:, 0], loc[:, 1], -loc[:, 2] n, e, d = geodetic2ned(lat, lon, h, ref['lat'], ref['lon'], ref['h']) self.ui.viewerWidget.show_events(np.array([n, e, d]).T, size=mag)
def test_ned_geodetic(): lla = pm.aer2geodetic(*aer0, *lla0) enu3 = pm.geodetic2enu(*lla, *lla0) ned3 = (enu3[1], enu3[0], -enu3[2]) assert pm.geodetic2ned(*lla, *lla0) == approx(ned3) lat, lon, alt = pm.enu2geodetic(*enu3, *lla0) assert lat == approx(lla[0]) assert lon == approx(lla[1]) assert alt == approx(lla[2]) lat, lon, alt = pm.ned2geodetic(*ned3, *lla0) assert lat == approx(lla[0]) assert lon == approx(lla[1]) assert alt == approx(lla[2])
def on_global_geodetic_setpoint_msg (self, msg): self.target_lat = msg.lat self.target_lon = msg.lon self.target_alt = msg.alt # convert to local target north, east, up = pm.geodetic2ned(self.target_lat, self.target_lon, self.target_alt, self.lat0, self.lon0, self.alt ) # publish new target self.setpoint_msg.x = north self.setpoint_msg.y = east self.setpoint_msg.z = -self.target_alt self.local_setpoint_pub.publish(self.setpoint_msg)
def calc_LLH2NED(*args): for value in args: nav = value #print(nav) #refLat = -35.360799697 #refLon = 149.207155405 #refAlt = 638.635 refLat = -35.36081064 refLon = 149.20715053 refAlt = 638.598 errN, errE, errD = pm.geodetic2ned(nav['Lat [deg]'], nav['Lon [deg]'], nav['Alt Ellips [m]'], refLat, refLon, refAlt) return ([errN, errE, errD])
def convert(lat, lon, hgt, lat0=32.775776, lon0=35.024963, alt0=229): n, e, d = pymap3d.geodetic2ned(lat, lon, hgt, lat0=lat0, lon0=lon0, h0=alt0) x, y, z = e, n, -d xi = np.linspace(-10000, 10000, 100) yi = np.linspace(-10000, 10000, 100) X, Y = np.meshgrid(xi, yi) Z = ml.griddata(y.flatten(), x.flatten(), z.flatten(), yi, xi, interp='linear') return X, Y, Z
def to_ned(self, lat, lon, height): """ Convert all the points to NED relative to passed llh """ ned = [] act = [] for ins in self.points: if ins.point: n, e, d = geodetic2ned(lat=ins.point.lat, lon=ins.point.lon, h=ins.point.alt, lat0=lat, lon0=lon, h0=height) ned.append([n * 100, e * 100, d * 100]) # Multiplication is for meters -> CM if ins.actions: act.append(ins.actions) else: act.append("") return ned, act
def test_ned_geodetic(): lat1, lon1, alt1 = pm.aer2geodetic(*aer0, *lla0) enu3 = pm.geodetic2enu(lat1, lon1, alt1, *lla0) ned3 = (enu3[1], enu3[0], -enu3[2]) assert pm.geodetic2ned(lat1, lon1, alt1, *lla0) == approx(ned3) lat, lon, alt = pm.enu2geodetic(*enu3, *lla0) assert lat == approx(lat1) assert lon == approx(lon1) assert alt == approx(alt1) assert isinstance(lat, float) assert isinstance(lon, float) assert isinstance(alt, float) lat, lon, alt = pm.ned2geodetic(*ned3, *lla0) assert lat == approx(lat1) assert lon == approx(lon1) assert alt == approx(alt1) assert isinstance(lat, float) assert isinstance(lon, float) assert isinstance(alt, float)
def from_gps_to_ned(target_position: Vector3r, home_gps: Vector3r): n, e, d = pymap3d.geodetic2ned(target_position.x_val, target_position.y_val, target_position.z_val, home_gps.x_val, home_gps.y_val, home_gps.z_val) return Vector3r(n, e, -d)
[-np.sin(gps_lla[0])*np.cos(gps_lla[1]), -np.sin(gps_lla[1]), -np.cos(gps_lla[0])*np.cos(gps_lla[1])], [-np.sin(gps_lla[0])*np.sin(gps_lla[1]), np.cos(gps_lla[1]), -np.cos(gps_lla[0])*np.sin(gps_lla[1])], [np.cos(gps_lla[0]), 0, -np.sin(gps_lla[0])] ]).transpose() # get pos of GPS, INS in NED gps_pos_ned = C_NLLA@(gps_lla) ins_pos_ned = C_NLLA@(ins_lla) ''' # ref pos is current INS pos estimate gps_pos_ned = np.array( geodetic2ned(gps_lla[0] * (180 / np.pi), gps_lla[1] * (180 / np.pi), gps_lla[2], ins_lla[0] * (180 / np.pi), ins_lla[1] * (180 / np.pi), ins_lla[2])) ins_pos_ned = np.array( geodetic2ned(ins_lla[0] * (180 / np.pi), ins_lla[1] * (180 / np.pi), ins_lla[2], ins_lla[0] * (180 / np.pi), ins_lla[1] * (180 / np.pi), ins_lla[2])) ''' gps_pos_ned = np.array(geodetic2ned(gps_lla[0]*(180/np.pi), gps_lla[1]*(180/np.pi), gps_lla[2], init_lla[0]*(180/np.pi), init_lla[1]*(180/np.pi), init_lla[2])) ins_pos_ned = np.array(geodetic2ned(ins_lla[0]*(180/np.pi), ins_lla[1]*(180/np.pi), ins_lla[2], init_lla[0]*(180/np.pi), init_lla[1]*(180/np.pi), init_lla[2])) '''
if len(arg) < 3: print("Not enough arguments\n") exit(0) directory = str(arg[1]) filename = str(arg[2]) fullpath = directory + filename # Read data into numpy array fuse = open(fullpath, 'rb') data = np.genfromtxt(fuse, delimiter=' ') num_points = np.shape(data)[0] # Create new file and add ply header ply = open(directory + filename.split('.')[0] + '_ned.ply', 'w+') header = 'ply\nformat ascii 1.0\nelement vertex ' + str(num_points) \ + '\nproperty float x\nproperty float y\nproperty float z\n' \ + 'property uchar intensity\nelement face 0\n' \ + 'property list uchar int vertex_indices\nend_header\n' ply.write(header) # Convert each point from WGS48 to NED and store in file lat0 = 48.858858 lon0 = 2.299525 h0 = 76.995310 for i in range(num_points): point = data[i] x, y, z = pm.geodetic2ned(point[0], point[1], point[2], lat0, lon0, h0) # x, y , z = pm.geodetic2ecef(point[0], point[1], point[2]) row = str(x) + ' ' + str(y) + ' ' + str(z) + ' ' + str(int( point[3])) + '\n' ply.write(row) ply.close() fuse.close()
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')
def main( lidar_lat, lidar_lg, lidar_h, delphib, cadastraldir, *nrbds ): ''' Follow these steps to perform azimuthal calibration of the lidar 1. Use .geojson_trimmer to create a suitable sized geojson file for plotting 2. specify the data set used for calibration check Plots out the horizontal sweep of the nrb against the cadastral map the geojson can be created and trimmed in /.cadastral_trimmer. Note that the coordinates provided by nrb_calc are in spherical coordinates, which map x->N, y->W. But here we plot on a 2D-plane, which maps y->N(latitude), x->E(longitude). This is taken care in the coordinate transform of the azimuthal array from nrb_calc. which converts phi to psi, which is the angle starting from the E axis, aka the longitude axis Parameters lidar_lat (float): [deg] lidar latitude lidar_lg (float): [deg] lidar longitude lidar_h (float): [m] lidar heigh above geodetic ellipsoid delphib (float): [rad] uncert of phib cadastraldir (str): directory of cadastral geojson nrbds (list): list of output from product_calc.nrb_calc ''' # reading files sg_df = gpd.read_file(cadastraldir) # plot creation fig, ax = plt.subplots() ## plotting map sg_df.plot(ax=ax) # computing coordinates for i, nrbd in enumerate(nrbds): color = f'C{i+1}' if i == 0: markeralpha = _reference_markeralpha markercolor = _reference_markercolor else: markeralpha = _markeralpha markercolor = color NRB_tra = nrbd['NRB_tra'] r_tra = nrbd['r_tra'] * 1000 r_trm = nrbd['r_trm'] phi_ta = nrbd['phi_ta'] psi_ta = nrbd['phi_ta'] + np.pi/2 # psi is the angle inthe 2D projection # anti clockwise from E(longitude) axis # scan lines; coverting local NE to lat long, referenced from the lidar # coordinates print('computing lidar point') point1lat_ta = lidar_lat*np.ones_like(psi_ta) point1lg_ta = lidar_lg*np.ones_like(psi_ta) point1n_ta, point1e_ta, point1d_ta = geodetic2ned( point1lat_ta, point1lg_ta, lidar_h, lidar_lat, lidar_lg, lidar_h ) print('computing scan points') point2n_ta = point1n_ta + _D*np.sin(psi_ta) point2e_ta = point1e_ta + _D*np.cos(psi_ta) point2lat_ta, point2lg_ta, _ = ned2geodetic( point2n_ta, point2e_ta, point1d_ta, lidar_lat, lidar_lg, lidar_h, ) # threshold for nrb r_trm *= NRB_tra > _nrbthres # range (time, bins) print('computing range points') rn_tra = point1n_ta[:, None] + r_tra * np.sin(psi_ta)[:, None] re_tra = point1e_ta[:, None] + r_tra * np.cos(psi_ta)[:, None] rlat_tra, rlg_tra, _ = ned2geodetic( rn_tra, re_tra, point1d_ta[:, None], lidar_lat, lidar_lg, lidar_h, ) # error bars ## errorbar for range; (time, bins, 2(lower limit, upper limit)) print('computing range error bar') deln_ta = _delr/2 * np.sin(psi_ta) dele_ta = _delr/2 * np.cos(psi_ta) delrn_tr2a = np.stack([ rn_tra - deln_ta[:, None], rn_tra + deln_ta[:, None] ], axis=2) delre_tr2a = np.stack([ re_tra - dele_ta[:, None], re_tra + dele_ta[:, None] ], axis=2) delrlat_tr2a, delrlg_tr2a, _ = ned2geodetic( delrn_tr2a, delre_tr2a, point1d_ta[:, None, None], lidar_lat, lidar_lg, lidar_h, ) ## error bar for azimuth; (time, bins, _psinum), 'n' represents _psinum print('computing azimuthal error bar') delpsi_na = np.linspace(-delphib, delphib, _psinum) delpsi_tna = psi_ta[:, None] + delpsi_na delpsin_trna = point1n_ta[:, None, None] + r_tra[:, :, None] * \ np.sin(delpsi_tna)[:, None, :] delpsie_trna = point1e_ta[:, None, None] + r_tra[:, :, None] * \ np.cos(delpsi_tna)[:, None, :] delpsilat_trna, delpsilg_trna, _ = ned2geodetic( delpsin_trna, delpsie_trna, point1d_ta[:, None, None], lidar_lat, lidar_lg, lidar_h ) # plotting ## lidar ax.plot(lidar_lg, lidar_lat, 'ko') ## plotting threshold nrb for j in range(len(phi_ta)): # indexing time # scan lines ax.plot( [point1lg_ta[j], point2lg_ta[j]], [point1lat_ta[j], point2lat_ta[j]], '-', alpha=0.3, color=color ) # nrb threshold points r_rm = r_trm[j] # (bins) ax.plot( rlg_tra[j][r_rm], rlat_tra[j][r_rm], 'o', color=markercolor, alpha=markeralpha ) # plotting error bars for k in range(np.sum(r_rm)): # plotting range error bars ax.plot( delrlg_tr2a[j][r_rm][k], delrlat_tr2a[j][r_rm][k], '-', color='k' ) # plotting azimuth error bars ax.plot( delpsilg_trna[j][r_rm][k], delpsilat_trna[j][r_rm][k], '-', color='k' ) # showing plt.xlabel('Long [deg]') plt.ylabel('Lat [deg]') plt.show()
def run(self, scenario, run_info): """ Run the model on the remote worker using the settings specified in model_input. The worker will return a job ID corresponding to the row ID of the final results stored in the remote database. :param Scenario scenario: Scenario for which to run the model :param dict run_info: Supplementary info for this run: 'reference_point': (lat, lon, depth) reference for coord. conversion 'injection_point': (lat, lon, depth) of current injection point """ forecast = scenario.forecast_input.forecast forecast_schema = ForecastSchema() serialized = forecast_schema.dump(forecast).data data = { 'forecast': serialized, 'parameters': self.model_config['parameters'], 'scenario id': scenario.id } # Add cartesian coordinates ref = forecast.forecast_set.project.reference_point try: catalog = data['forecast']['input']['input_catalog'] for e in catalog['seismic_events']: x, y, z = geodetic2ned(e['lat'], e['lon'], e['depth'], ref['lat'], ref['lon'], ref['h']) e['x'], e['y'], e['z'] = x, y, z except TypeError: self.logger.info('No seismic events') # Request model run self.results = None self.logger.info('Starting remote worker for {}'.format(self.model_id)) notification = ErrorNotification(calc_id=self.model_id) try: r = requests.post(self.url, json=data, timeout=5) except (ConnectionError, Timeout) as ex: self.logger.error('Can' 't connect to worker: {}'.format(repr(ex))) else: notification.response = r if r.status_code == requests.codes.accepted: notification = RunningNotification(self.model_id, response=r) QTimer.singleShot(self.poll_interval, self._get_results) elif r.status_code == requests.codes.bad_request: self.logger.error( 'The worker did not accept our request: {}'.format( r.content)) elif r.status_code == requests.codes.server_error: self.logger.error('The worker reported an error: {}'.format( r.content)) elif r.status_code == requests.codes.unavailable: self.logger.error( 'The worker did not accept our job: {}'.format(r.content)) else: self.logger.error( 'Unexpected response received: [{}] {}'.format( r.status_code, r.content)) self.client_notification.emit(notification)
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')