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
