def geo_carla2xyz_carla(lat, lon, alt): # transforms geodetic location from carla.GnssMeasurement to location in cartesian Carla map frame. However, # transformed location and ground truth deviate from each other (see above). # after this transformation of the latitude, GNSS projection and ego_vehicle.get_transform().location are "closer" lat = pm.geocentric2geodetic(lat, alt, pm.Ellipsoid('wgs84'), deg=True) x_enu, y_enu, z_enu = pm.geodetic2enu(lat, lon, alt, map_carla_origin_geo.latitude, map_carla_origin_geo.longitude, map_carla_origin_geo.altitude, pm.Ellipsoid('wgs84'), deg=True) # y-coordinate in Carla coordinate system is flipped (see https://github.com/carla-simulator/carla/issues/2737) return x_enu, -y_enu, z_enu
def enu2geo(self, x, y): lat, lon, _ = p3d.enu2geodetic(x, y, 0, self.lat0, self.lon0, 0, deg=True, ell=p3d.Ellipsoid('wgs84')) return lat, lon
def geo2enu(self, lat, lon): x, y, _ = p3d.geodetic2enu(lat, lon, 0, self.lat0, self.lon0, 0, deg=True, ell=p3d.Ellipsoid('wgs84')) return x, y
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 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 unscented_transform(A, max_height, pixels, origin, rx, ry, rz, r_az, r_el): points, weights_av, weights_cov = sigma_points( [10, 10, 10, 0.01745, 0.01745, 3 * 0.01745, 0.01745, 0.01745]) n = points.shape[1] N = len(pixels) media_geodetic = np.zeros([N, 3]) P_xyz = np.zeros([N, 3]) pos_intersection = np.zeros([n, 3]) media = np.zeros([ 3, ]) for j in range(0, N): for i in range(0, n): sigma_point = points[:, i] pos_intersection[i, :] = irt_project(A, max_height, pixels[j], origin, rx, ry, rz, r_az, r_el, sigma_point) #detecao de erro if np.isnan(pos_intersection).any(): return np.nan, np.nan media = np.dot(weights_av, pos_intersection) media_geodetic[j, :] = ned2geodetic(media[0, 0], media[0, 1], media[0, 2], origin[0], origin[1], origin[2], pymap3d.Ellipsoid('wgs84')) P = np.zeros([3, 3]) for i in range(0, n): diff = np.subtract(pos_intersection[i, :], media) P_new = np.matmul(np.transpose(diff), diff) * weights_cov[0, i] P = np.add(P, P_new) P_xyz[j, :] = np.sqrt(np.diag(P)) return media_geodetic, P_xyz
import argparse import xml.etree.ElementTree as ET import zipfile from pathlib import Path import sys import pymap3d import shapely.geometry as SHP from descartes.patch import PolygonPatch from matplotlib import pyplot as plt from matplotlib.collections import PatchCollection, LineCollection from shapely.geometry import LineString, JOIN_STYLE, MultiLineString import math from isoxmlviz.LineStringUtil import extract_lines_within ell_wgs84 = pymap3d.Ellipsoid('wgs84') default_propagation_num = 100 def pnt_to_pair(element: ET): # C=lat return float(element.attrib.get("C")), float(element.attrib.get("D")) def totuple(a): try: return tuple(totuple(i) for i in a) except TypeError: return a
def map_target(tx, rx, az, el, rf): """ Find the scatter location given tx location, rx, location, total rf distance, and target angle-of-arrival. Parameters ---------- tx : float np.array [latitude, longitude, altitude] of tx array in degrees and kilometers rx : float np.array [latitude, longitude, altitude] of rx array in degrees and kilometers az : float angle-of-arrival azimuth in degrees el : float angle-of-arrival elevation in degrees rf : float np.array total rf path distance rf = c * tau Returns ------- sx : float np.array [latitude, longitude, altitude] of scatter in degrees and kilometers r : float bistatic slant range in kilometers """ # Setup givens in correct units rf = rf * 1.0e3 * 1.5 - 230e3 # Assumed rf propagation correction in km az = np.where(az < 0, np.deg2rad(az + 367.0), np.deg2rad(az + 7.0)) el = np.deg2rad(np.abs(el)) sx = np.zeros((3, len(rf))) uv = np.zeros((3, len(rf))) us = np.zeros((3, len(rf))) # Determine the slant range, r bx1, by1, bz1 = pm.geodetic2ecef(rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) ur = np.array([bx1, by1, bz1]) / np.linalg.norm([bx1, by1, bz1]) bx2, by2, bz2 = pm.geodetic2ecef(tx[0], tx[1], tx[2], ell=pm.Ellipsoid("wgs84"), deg=True) ut = np.array([bx2, by2, bz2]) / np.linalg.norm([bx2, by2, bz2]) bx = bx2 - bx1 by = by2 - by1 bz = bz2 - bz1 b = np.linalg.norm([bx, by, bz]) ub = np.array([bx, by, bz]) / b dr = np.ones(1000) dtheta = np.ones(1000) dr[0] = 1e6 dtheta[0] = 1e6 r2 = np.copy(rf) theta2 = np.ones(len(az)) * np.pi cnt = 0 while np.max(dtheta[cnt]) >= 0.002 and dr[cnt] >= 1500: ua = np.array([np.sin(az) * np.cos(el), np.cos(az) * np.cos(el), np.sin(el)]) theta = np.arccos(ua[0, :] * ub[0] + ua[1, :] * ub[1] + ua[2, :] * ub[2]) r = (rf ** 2 - b ** 2) / (2 * (rf - b * np.cos(theta))) # Correct elevation using geocentric angle gamma and first order ranges, find scatter lat, long, alt for i in range(len(rf)): bx3, by3, bz3 = pm.aer2ecef(np.rad2deg(az[i]), np.rad2deg(el[i]), np.abs(r[i]), rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) us[:, i] = np.array([bx3, by3, bz3]) / np.linalg.norm([bx3, by3, bz3]) el[i] -= np.arccos(ur[0] * us[0, i] + ur[1] * us[1, i] + ur[2] * us[2, i]) sx[:, i] = pm.aer2geodetic(np.rad2deg(az[i]), np.rad2deg(el[i]), np.abs(r[i]), rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) dr[cnt + 1] = np.max(np.abs(r - r2)) dtheta[cnt + 1] = np.max(np.abs(theta - theta2)) cnt += 1 r2 = r theta2 = theta # Find the bistatic bisector velocity unit vector uv = (us + ua) / 2.0 uv = uv / np.linalg.norm(uv) # Set units to degrees and kilometers sx[2, :] /= 1.0e3 r /= 1.0e3 return sx[2, :], r, dr[0:cnt], dtheta[0:cnt]
#!/usr/bin/env python3 from math import radians import pytest from pytest import approx import pymap3d as pm ELL = pm.Ellipsoid() A = ELL.semimajor_axis B = ELL.semiminor_axis @pytest.mark.parametrize("aer,lla,xyz", [((33, 70, 1000), (42, -82, 200), (660930.2, -4701424.0, 4246579.6))]) def test_aer2ecef(aer, lla, xyz): x, y, z = pm.aer2ecef(*aer, *lla) assert x == approx(xyz[0]) assert y == approx(xyz[1]) assert z == approx(xyz[2]) raer = (radians(aer[0]), radians(aer[1]), aer[2]) rlla = (radians(lla[0]), radians(lla[1]), lla[2]) assert pm.aer2ecef(*raer, *rlla, deg=False) == approx(xyz) with pytest.raises(ValueError): pm.aer2ecef(aer[0], aer[1], -1, *lla) @pytest.mark.parametrize( "xyz, lla, aer", [ ((A - 1, 0, 0), (0, 0, 0), (0, -90, 1)),
def map_target(tx, rx, az, el, rf, dop, wavelength): """ Find the scatter location given tx location, rx location, total rf distance, and target angle-of-arrival using the 'WGS84' Earth model. Also determines the bistatic velocity vector and bistatic radar wavelength. Parameters ---------- tx : float np.array [latitude, longitude, altitude] of tx array in degrees and kilometers rx : float np.array [latitude, longitude, altitude] of rx array in degrees and kilometers az : float np.array angle-of-arrival azimuth in degrees el : float np.array angle-of-arrival elevation in degrees rf : float np.array total rf path distance rf = c * tau in kilometers dop : float np.array doppler shift in hertz wavelength : float radar signal center wavelength Returns ------- sx : float np.array [latitude, longitude, altitude] of scatter in degrees and kilometers sa : float np.array [azimuth, elevation, slant range] of scatter in degrees and kilometers sv : float np.array [azimuth, elevation, velocity] the bistatic Doppler velocity vector in degrees and kilometers. Coordinates given in the scattering targets local frame (azimuth from North, elevation up from the plane normal to zenith, Doppler [Hz] * lambda / (2 cos(e/2)) ) Notes ----- tx : transmitter location rx : receiver location sx : scatter location gx : geometric center of Earth, origin u_rt : unit vector rx to tx u_rs : unit vector rx to sx u_gt : unit vector gx to tx u_gr : unit vector gx to rx u_gs : unit vector gx to sx """ # Initialize output arrays sx = np.zeros((3, len(rf)), dtype=float) sa = np.zeros((3, len(rf)), dtype=float) sv = np.zeros((3, len(rf)), dtype=float) # Setup variables in correct units for pymap3d rf = rf * 1.0e3 az = np.where(az < 0.0, az + 360.0, az) az = np.deg2rad(az) el = np.deg2rad(np.abs(el)) # Determine the slant range, r bx1, by1, bz1 = pm.geodetic2ecef(rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) v_gr = np.array([bx1, by1, bz1]) bx2, by2, bz2 = pm.geodetic2ecef(tx[0], tx[1], tx[2], ell=pm.Ellipsoid("wgs84"), deg=True) v_gt = np.array([bx2, by2, bz2]) raz, rel, b = pm.ecef2aer(bx2, by2, bz2, rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) u_rt = np.array([ np.sin(np.deg2rad(raz)) * np.cos(np.deg2rad(rel)), np.cos(np.deg2rad(raz)) * np.cos(np.deg2rad(rel)), np.sin(np.deg2rad(rel)) ]) el -= relaxation_elevation(el, rf, az, b, u_rt) u_rs = np.array( [np.sin(az) * np.cos(el), np.cos(az) * np.cos(el), np.sin(el)]) r = (rf**2 - b**2) / (2 * (rf - b * np.dot(u_rt, u_rs))) # WGS84 Model for lat, long, alt sx[:, :] = pm.aer2geodetic(np.rad2deg(az), np.rad2deg(el), np.abs(r), np.repeat(rx[0], len(az)), np.repeat(rx[1], len(az)), np.repeat(rx[2], len(az)), ell=pm.Ellipsoid("wgs84"), deg=True) # Determine the bistatic Doppler velocity vector x, y, z = pm.geodetic2ecef(sx[0, :], sx[1, :], sx[2, :], ell=pm.Ellipsoid('wgs84'), deg=True) v_gs = np.array([x, y, z]) v_bi = (-1 * v_gs.T + v_gt / 2.0 + v_gr / 2.0).T u_bi = v_bi / np.linalg.norm(v_bi, axis=0) v_sr = (v_gr - v_gs.T).T u_sr = v_sr / np.linalg.norm(v_sr, axis=0) radar_wavelength = wavelength / np.abs( 2.0 * np.einsum('ij,ij->j', u_sr, u_bi)) # doppler_sign = np.sign(dop) # 1 for positive, -1 for negative, and 0 for zero doppler_sign = np.where( dop >= 0, 1, -1) # 1 for positive, -1 for negative, and 0 for zero vaz, vel, _ = pm.ecef2aer(doppler_sign * u_bi[0, :] + x, doppler_sign * u_bi[1, :] + y, doppler_sign * u_bi[2, :] + z, sx[0, :], sx[1, :], sx[2, :], ell=pm.Ellipsoid("wgs84"), deg=True) # Convert back to conventional units sx[2, :] /= 1.0e3 az = np.rad2deg(az) el = np.rad2deg(el) sa[:, :] = np.array([az, el, r / 1.0e3]) sv[:, :] = np.array([vaz, vel, dop * radar_wavelength]) return sx, sa, sv
def publish_topics(dictionary_observables: dict) -> object: name_stages = [] people_distrib_per_stage = [] lat_stages = [] lon_stages = [] cov_stages = [] if not PermanentSettings.containerized: device_number = Settings.device_number stage_number = Settings.stage_number name_stages = Settings.name_stages people_distrib_per_stage = Settings.people_distrib_per_stage lat_stages = Settings.lat_stages lon_stages = Settings.lon_stages cov_stages = Settings.cov_stages else: stage_number = 4 try: device_number = int(os.environ[constants.DEVICE_NUMBER_KEY]) for i in range(1, stage_number + 1): stage_id = "_" + str(i) name_stages.append(os.environ[constants.STAGE_NAME_KEY + stage_id]) people_distrib_per_stage.append( float(os.environ[constants.DISTR_STAGE_KEY + stage_id])) lat_stages.append( float(os.environ[constants.LAT_STAGE_KEY + stage_id])) lon_stages.append( float(os.environ[constants.LON_STAGE_KEY + stage_id])) cov_stages.append([[ int(os.environ[constants.SIGMA_N_S_KEY + stage_id]), 0 ], [ 0, int(os.environ[constants.SIGMA_E_O_KEY + stage_id]) ]]) except KeyError as ke: logging.critical("Missing environmental variable: " + str(ke)) exit(1) try: if not dictionary_observables: return # create an ellipsoid object ell_wgs84 = pymap3d.Ellipsoid('wgs84') counter_message_sent = 0 num_people = [] for percentage in people_distrib_per_stage: num_people.append(round(percentage / 100 * device_number)) logging.debug("Total number of people: " + str(sum(num_people))) count = num_people[0] thresholds = [num_people[0]] for i in range(1, stage_number): thresholds.append(num_people[i] + count) count = thresholds[i] index_stage = 0 current_threshold = thresholds[index_stage] for iot_id in dictionary_observables: list_topic_tag_id = dictionary_observables[iot_id] if len(list_topic_tag_id) < 2: logging.debug("Element ignored: " + str(list_topic_tag_id)) continue topic = list_topic_tag_id[0] tag_id = list_topic_tag_id[1] cov = cov_stages[index_stage] # cov = [[100, 0], [0, 100]] # diagonal covariance mean = [0, 0] num_samples = 1 e1, n1 = np.random.multivariate_normal(mean, cov, num_samples).T u1 = 0 lat0 = lat_stages[index_stage] lon0 = lon_stages[index_stage] h0 = 0 # lat0, lon0, h0 = 5.0, 48.0, 10.0 # origin of ENU, (h is height above ellipsoid) # e1, n1, u1 = 0.0, 0.0, 0.0 # ENU coordinates of test point, `point_1` # From ENU to geodetic computation lat1, lon1, h1 = pymap3d.enu2geodetic( e1[0], n1[0], u1, lat0, lon0, h0, ell=ell_wgs84, deg=True) # use wgs84 ellisoid localization = Localization(tag_id=tag_id, iot_id=iot_id, lat=lat1, lon=lon1, area_id=name_stages[index_stage]) payload = localization.to_dictionary() correctly_sent = ServerMQTT.publish(topic=topic, dictionary=payload) if correctly_sent: logging.debug('On topic: "' + topic + '" was sent payload: \n' + str(payload)) else: logging.error("Error sending on topic: '" + topic + "' payload: \n" + str(payload)) counter_message_sent += 1 if counter_message_sent >= current_threshold: index_stage = index_stage + 1 if index_stage < stage_number: current_threshold = thresholds[index_stage] else: index_stage = index_stage - 1 # go back -- do not increase this index larger than index_stage - 1 if (counter_message_sent % 100) == 0: logging.info('MQTT Publish Messages: {}'.format( counter_message_sent)) logging.info('MQTT Publish Messages Completed: {}'.format( counter_message_sent)) except Exception as ex: logging.error('Exception publish_topics: {}'.format(ex))
def test_reference(model, f): assert pm.Ellipsoid(model).flattening == approx(f)
def map_target(tx, rx, az, el, rf): """ Find the scatter location given tx location, rx, location, total rf distance, and target angle-of-arrival. Parameters ---------- tx : float np.array [latitude, longitude, altitude] of tx array in degrees and kilometers rx : float np.array [latitude, longitude, altitude] of rx array in degrees and kilometers az : float angle-of-arrival azimuth in degrees el : float angle-of-arrival elevation in degrees rf : float np.array total rf path distance rf = c * tau Returns ------- sx : float np.array [latitude, longitude, altitude] of scatter in degrees and kilometers r : float bistatic slant range in kilometers """ # Setup givens in correct units rf = rf * 1.0e3 * 1.5 - 230e3 az = np.where(az < 0, np.deg2rad(az + 367.0), np.deg2rad(az + 7.0)) el = np.deg2rad(np.abs(el)) sx = np.zeros((3, len(rf))) uv = np.zeros((3, len(rf))) us = np.zeros((3, len(rf))) # Determine the slant range, r bx1, by1, bz1 = pm.geodetic2ecef(rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) ur = np.array([bx1, by1, bz1]) / np.linalg.norm([bx1, by1, bz1]) bx2, by2, bz2 = pm.geodetic2ecef(tx[0], tx[1], tx[2], ell=pm.Ellipsoid("wgs84"), deg=True) ut = np.array([bx2, by2, bz2]) / np.linalg.norm([bx2, by2, bz2]) bx = bx2 - bx1 by = by2 - by1 bz = bz2 - bz1 b = np.linalg.norm([bx, by, bz]) ub = np.array([bx, by, bz]) / b ua = np.array( [np.sin(az) * np.cos(el), np.cos(az) * np.cos(el), np.sin(el)]) theta = np.arccos(ua[0, :] * ub[0] + ua[1, :] * ub[1] + ua[2, :] * ub[2]) r = (rf**2 - b**2) / (2 * (rf - b * np.cos(theta))) # Correct elevation using geocentric angle gamma and first order ranges, find scatter lat, long, alt for i in range(len(rf)): bx3, by3, bz3 = pm.aer2ecef(np.rad2deg(az[i]), np.rad2deg(el[i]), np.abs(r[i]), rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) us[:, i] = np.array([bx3, by3, bz3]) / np.linalg.norm([bx3, by3, bz3]) #el[i] -= np.arccos(ut[0]*us[0, i] + ut[1]*us[1, i] + ut[2]*us[2, i]) el[i] -= np.arccos(ur[0] * us[0, i] + ur[1] * us[1, i] + ur[2] * us[2, i]) sx[:, i] = pm.aer2geodetic(np.rad2deg(az[i]), np.rad2deg(el[i]), np.abs(r[i]), rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) # Second order slant range, r ua = np.array( [np.sin(az) * np.cos(el), np.cos(az) * np.cos(el), np.sin(el)]) theta = np.arccos(ua[0, :] * ub[0] + ua[1, :] * ub[1] + ua[2, :] * ub[2]) r = (rf**2 - b**2) / (2 * (rf - b * np.cos(theta))) for i in range(len(rf)): sx[:, i] = pm.aer2geodetic(np.rad2deg(az[i]), np.rad2deg(el[i]), np.abs(r[i]), rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) # Find the bistatic bisector velocity unit vector uv = (us + ua) / 2.0 uv = uv / np.linalg.norm(uv) # Set units to degrees and kilometers sx[2, :] /= 1.0e3 r /= 1.0e3 # d = rf/2 - 200e3 # r1 = np.sqrt((6378.1370e3 * np.cos(np.deg2rad(52.1579))) ** 2 + (6356.7523e3 * np.sin(np.deg2rad(52.1579))) ** 2) # pre_alt = np.sqrt(r1 ** 2 + (d) ** 2 - 2 * r1 * (d) * np.cos(np.pi/2 + el)) # el -= np.arccos(((d) ** 2 - (r1 ** 2) - (pre_alt ** 2)) / (-2 * r1 * pre_alt)) # Find lat, long, alt of target # sx = np.zeros((3, len(rf))) # for i in range(len(rf)): # sx[:, i] = pm.aer2geodetic(np.rad2deg(az[i]), np.rad2deg(el[i]), np.abs(r[i]), # rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) #return sx, r, uv vaz, vel, _ = pm.ecef2aer(uv[0, :] + bx3, uv[1, :] + by3, uv[2, :] + bz3, sx[0, :], sx[1, :], sx[2, :], ell=pm.Ellipsoid("wgs84"), deg=True) return sx[2, :], r, vaz, vel
def map_target(tx, rx, az, el, rf, mode='ellipsoidal'): """ Find the scatter location given tx location, rx, location, total rf distance, and target angle-of-arrival. Parameters ---------- tx : float np.array [latitude, longitude, altitude] of tx array in degrees and kilometers rx : float np.array [latitude, longitude, altitude] of rx array in degrees and kilometers az : float np.array angle-of-arrival azimuth in degrees el : float np.array angle-of-arrival elevation in degrees rf : float np.array total rf path distance rf = c * tau in kilometers mode : string earth model option; 'ellipsoidal' for WGS84 or 'spherical' for simple sphere Returns ------- sx : float np.array [latitude, longitude, altitude] of scatter in degrees and kilometers r : float bistatic corrected slant range in kilometers el : float np.array bistatic low elevation corrected elevation angle-of-arrival in degrees """ # Setup givens in correct units rf = rf * 1.0e3 az = np.where(az < 0.0, az + 360.0, az) az = np.deg2rad(az) el = np.deg2rad(np.abs(el)) sx = np.zeros((3, len(rf))) uv = np.zeros((3, len(rf))) us = np.zeros((3, len(rf))) # Determine the slant range, r bx1, by1, bz1 = pm.geodetic2ecef(rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) ur = np.array([bx1, by1, bz1]) / np.linalg.norm([bx1, by1, bz1]) bx2, by2, bz2 = pm.geodetic2ecef(tx[0], tx[1], tx[2], ell=pm.Ellipsoid("wgs84"), deg=True) ut = np.array([bx2, by2, bz2]) / np.linalg.norm([bx2, by2, bz2]) bx = bx2 - bx1 by = by2 - by1 bz = bz2 - bz1 b = np.linalg.norm([bx, by, bz]) ub = np.array([bx, by, bz]) / b el -= relaxation_elevation(el, rf, az, b, ub) ua = np.array( [np.sin(az) * np.cos(el), np.cos(az) * np.cos(el), np.sin(el)]) r = (rf**2 - b**2) / (2 * (rf - b * np.dot(ub, ua))) # WGS84 Model for lat, long, alt. if mode == 'ellipsoidal': sx[:, :] = pm.aer2geodetic(np.rad2deg(az), np.rad2deg(el), np.abs(r), np.repeat(rx[0], len(az)), np.repeat(rx[1], len(az)), np.repeat(rx[2], len(az)), ell=pm.Ellipsoid("wgs84"), deg=True) # Spherical Earth approximation if mode == 'spherical': re = 6378.0e3 # Radius of earth in [m] sx[2, :] = -re + np.sqrt(re**2 + r**2 + 2 * re * r * np.sin(el)) return sx[2, :] / 1.0e3, r / 1.0e3, np.rad2deg(el)
# requires pymap3d # from __future__ import absolute_import import pymap3d as pm # print(pm.__file__) # from pm import geodetic2ecef # from pymap3d import geodetic2ecef # import pymap3d x, y, z = pm.geodetic2ecef(-25.1, -49.5, 978.2) wgs = pm.Ellipsoid() e, n, u = pm.ecef2enu(x, y, z, -25.0, -49.0, wgs, True) # print([x,y,z]) print([e, n, u]) # def main(): # x,y,z = pm.geodetic2ecef(-25.1,-49.5,978.2) # # x,y,z = pymap3d.geodetic2ecef(-25.1,-49.5,978.2) # print([x,y,z])
# %% outcomes from matlab xyz0 = (660.6753e3, -4700.9487e3, 4245.738e3) # geodetic2ecef lla1 = (42.002582, -81.997752, 1.1397018e3) # aer2geodetic rlla1 = (np.radians(lla1[0]), np.radians(lla1[1]), lla1[2]) axyz0 = 660930.2, -4701424, 4246579.6 # aer2ecef enu0 = (186.277521, 286.842228, 939.692621) # aer2enu ned0 = (enu0[1], enu0[0], -enu0[2]) # vector vx, vy, vz = (5, 3, 2) ve, vn, vu = (5.368859646588048, 3.008520763668120, -0.352347711524077) E = pm.Ellipsoid() def test_losint(): az = [0., 10., 125.] lat, lon, sr = pm.lookAtSpheroid(*lla0, az, tilt=0.) assert (lat[0] == lat).all() and (lon[0] == lon).all() and (sr[0] == sr).all() assert (lat[0], lon[0], sr[0]) == approx(lla0) with pytest.raises(ValueError): pm.lookAtSpheroid(lla0[0], lla0[1], -1, az, 0)
longitude = csv['Lon (deg)'] altitude = csv['Alt (km)'] + altitude_correction sel = np.where(np.abs(altitude) < 10) return xarray.DataArray(altitude[sel], coords={ 'time': time[sel], 'lat': ('time', latitude[sel]), 'lon': ('time', longitude[sel]) }, dims=['time']) reference_lla = [16.6858, 159.5218, -1561.12] moon_ellipsoid = pymap3d.Ellipsoid(model='moon') def track_comparison(track1, track2): plt.figure(figsize=(12, 12), facecolor='w') plt.imshow(dem, extent=dem_extent, origin='bottom', cmap='gray') plt.plot(track1.coords['lon'], track1.coords['lat'], 'C0') plt.plot(track2.coords['lon'], track2.coords['lat'], 'C1') plt.title('Tracks over lunar surface') plt.xlabel('Longitude (deg)') plt.ylabel('Latitude (deg)') plt.figure(figsize=(12, 6), facecolor='w') track1.coords['lat'].plot() track2.coords['lat'].plot() plt.title('Track comparison')
#!/usr/bin/env python3 import pytest from pytest import approx import pymap3d as pm ell = pm.Ellipsoid() A = ell.semimajor_axis @pytest.mark.parametrize("lat,curvature", [(0, A), (90, 0), (-90, 0), (45.0, 4517590.87884893), (-45, 4517590.87884893)]) def test_rcurve_parallel(lat, curvature): assert pm.rcurve_parallel(lat) == approx(curvature, abs=1e-9) def test_numpy_parallel(): pytest.importorskip("numpy") assert pm.rcurve_parallel([0, 90]) == approx([A, 0], abs=1e-9) @pytest.mark.parametrize("lat,curvature", [(0, 6335439.327), (90, 6399593.6258), (-90, 6399593.6258), (45.0, 6367381.8156), (-45, 6367381.8156)]) def test_rcurve_meridian(lat, curvature): assert pm.rcurve_meridian(lat) == approx(curvature) def test_numpy_meridian():
def irt_project(A, max_height, pixels, origin, rx, ry, rz, r_az, r_el, sigma_point): pos_intersection = np.zeros([1, 3]) # parametros da camara cx = 319.5 cy = 239.5 fx = 481.2 fy = 480.0 # orientacao Rxyz = R.from_euler( 'xyz', [rx + sigma_point[3], ry + sigma_point[4], rz + sigma_point[5]]) R_gimbal = R.from_euler('xyz', [0, r_el + sigma_point[6], r_az + sigma_point[7]]) R_camera_to_world = R.from_matrix([[0, 0, 1], [1, 0, 0], [0, 1, 0]]) #calcula o raio optico no referencial inercial dir_ray_camara = [pixels[0] - cx, pixels[1] - cy, (fx + fy) / 2] dir_ray_inertial = R_camera_to_world.apply(dir_ray_camara) dir_ray_inertial = dir_ray_inertial / np.linalg.norm(dir_ray_inertial) ray_gimbal = R_gimbal.apply(dir_ray_inertial) ray = Rxyz.apply(ray_gimbal) #comeca a iterar com Z igual a altura maxima do mapa if origin[2] > max_height and ray[2] != 0: scale_factor = (origin[2] - max_height) / ray[2] new_origin_x = scale_factor * ray[0] new_origin_y = scale_factor * ray[1] origin_loop = np.array( [new_origin_x, new_origin_y, origin[2] - max_height]) else: origin_loop = [0, 0, 0] origin_loop = origin_loop + sigma_point[0:3] step = 100 step_divider = 10 step_thresh = 1 intersection = 0 pos_ray = origin_loop i = 0 while intersection != 1: i = i + 1 pos_ray = pos_ray + step * ray #calcula a elevacao do ponto por interpolação pos_geodetic = ned2geodetic(pos_ray[0], pos_ray[1], pos_ray[2], origin[0], origin[1], origin[2], pymap3d.Ellipsoid('wgs84')) z = interp_map(A, pos_geodetic[0], pos_geodetic[1], pos_geodetic[2]) if np.isnan(z) or i > 200: return np.nan #caso a elevacao do mapa seja superior a elevacao do raio, houve intersecao #ativa-se o step pequeno para melhorar a estimativa elif z > pos_geodetic[2] and step > step_thresh: pos_ray = pos_ray - step * ray step = step / step_divider continue #conclui o algoritmo, foi detetada a intersecao com o step pequeno elif z > pos_geodetic[2] and step <= step_thresh: pos_intersection = pos_ray intersection = 1 return pos_intersection