Add Ellipse method

This commit is contained in:
30hours 2024-03-05 12:22:30 +00:00
parent 88c99ed45e
commit 885019ab08
4 changed files with 167 additions and 9 deletions

View file

@ -23,17 +23,11 @@ servers = [
associators = [
{"name": "ADSB Associator", "id": "adsb-associator"}
]
# coordregs = [
# {"name": "Ellipse Analytic Intersection", "id": "ellipse-conic-int"},
# {"name": "Ellipse Parametric", "id": "ellipse-parametric"},
# {"name": "Ellipse Parametric (Arc Length)", "id": "ellipse-parametric-arc"},
# {"name": "Ellipsoid Parametric", "id": "ellipsoid-parametric"},
# {"name": "Ellipsoid Parametric (Arc Length)", "id": "ellipsoid-parametric-arc"}
# ]
# todo: ellipse conic int (analytic), SX, arc length
localisations = [
{"name": "Ellipse Parametric", "id": "ellipse-parametric"},
{"name": "Ellipsoid Parametric", "id": "ellipsoid-parametric"}
]
adsbs = [
{"name": "adsb.30hours.dev", "url": "adsb.30hours.dev"},
{"name": "None", "url": ""}

View file

@ -0,0 +1,160 @@
"""
@file EllipseParametric.py
@author 30hours
"""
from data.Ellipsoid import Ellipsoid
from algorithm.geometry.Geometry import Geometry
import numpy as np
import math
class EllipseParametric:
"""
@class EllipseParametric
@brief A class for intersecting ellipses using a parametric approx.
@details Uses associated detections from multiple radars.
@see blah2 at https://github.com/30hours/blah2.
"""
def __init__(self):
"""
@brief Constructor for the EllipseParametric class.
"""
self.ellipsoids = []
self.nSamples = 150
self.threshold = 800
def process(self, assoc_detections, radar_data):
"""
@brief Perform target localisation using the ellipse parametric method.
@details Generate a (non arc-length) parametric ellipse for each node.
@param assoc_detections (dict): JSON of blah2 radar detections.
@param radar_data (dict): JSON of adsb2dd truth detections.
@return dict: Dict of associated detections.
"""
output = {}
# return if no detections
if not assoc_detections:
return output
for target in assoc_detections:
print(target, flush=True)
target_samples = {}
target_samples[target] = {}
for radar in assoc_detections[target]:
print(radar["radar"], flush=True)
print(radar["delay"], flush=True)
# create ellipsoid for radar
ellipsoid = next((
item for item in self.ellipsoids
if item.name == radar["radar"]), None)
if ellipsoid is None:
config = radar_data[radar["radar"]]["config"]
x_tx, y_tx, z_tx = Geometry.lla2ecef(
config['location']['tx']['latitude'],
config['location']['tx']['longitude'],
config['location']['tx']['altitude']
)
x_rx, y_rx, z_rx = Geometry.lla2ecef(
config['location']['rx']['latitude'],
config['location']['rx']['longitude'],
config['location']['rx']['altitude']
)
ellipsoid = Ellipsoid(
[x_tx, y_tx, z_tx],
[x_rx, y_rx, z_rx],
radar["radar"]
)
samples = self.sample(ellipsoid, radar["delay"]*1000, self.nSamples)
target_samples[target][radar["radar"]] = samples
# find close points, ellipse 1 is master
radar_keys = list(target_samples[target].keys())
samples_intersect = []
# loop points in master ellipsoid
for point1 in target_samples[target][radar_keys[0]]:
valid_point = True
# loop over each other list
for i in range(1, len(radar_keys)):
# loop points in other list
if not any(Geometry.distance_ecef(point1, point2) < self.threshold
for point2 in target_samples[target][radar_keys[i]]):
valid_point = False
break
if valid_point:
samples_intersect.append(point1)
# remove duplicates and convert to LLA
output[target] = {}
output[target]["points"] = []
for i in range(len(samples_intersect)):
samples_intersect[i] = Geometry.ecef2lla(
samples_intersect[i][0],
samples_intersect[i][1],
0)
output[target]["points"].append([
round(samples_intersect[i][0], 3),
round(samples_intersect[i][1], 3),
0])
return output
def sample(self, ellipsoid, bistatic_range, n):
"""
@brief Generate a set of ECEF points for the ellipse.
@details No arc length parametrisation.
@details Use ECEF because distance measure is simple over LLA.
@param ellipsoid (Ellipsoid): The ellipsoid object to use.
@param bistatic_range (float): Bistatic range for ellipse.
@param n (int): Number of points to generate.
@return list: Samples with size [n, 3].
"""
# rotation matrix
theta = ellipsoid.yaw
R = np.array([
[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]
])
# compute samples vectorised
a = (bistatic_range+ellipsoid.distance)/2
b = np.sqrt(a**2 - (ellipsoid.distance/2)**2)
u = np.linspace(0, 2 * np.pi, n)
x = a * np.cos(u)
y = b * np.sin(u)
r = np.stack([x, y], axis=-1).reshape(-1, 2)
r_1 = np.dot(r, R)
output = []
for i in range(len(r_1)):
# points to ECEF
x, y, z = Geometry.enu2ecef(
r_1[i][0], r_1[i][1], 0,
ellipsoid.midpoint_lla[0],
ellipsoid.midpoint_lla[1],
ellipsoid.midpoint_lla[2])
# points to LLA
[x, y, z] = Geometry.ecef2lla(x, y, z)
# only store points above ground
if z > 0:
# convert back to ECEF for simple distance measurements
[x, y, z] = Geometry.lla2ecef(x, y, z)
output.append([round(x, 3), round(y, 3), 0])
return output

View file

@ -80,7 +80,7 @@ class EllipsoidParametric:
samples = self.sample(ellipsoid, radar["delay"]*1000, self.nSamples)
target_samples[target][radar["radar"]] = samples
# find close points - ellipsoid 1 = master
# find close points, ellipsoid 1 is master
radar_keys = list(target_samples[target].keys())
samples_intersect = []

View file

@ -14,6 +14,7 @@ import json
import hashlib
from algorithm.associator.AdsbAssociator import AdsbAssociator
from algorithm.localisation.EllipseParametric import EllipseParametric
from algorithm.localisation.EllipsoidParametric import EllipsoidParametric
from common.Message import Message
@ -26,6 +27,7 @@ api = []
# init config
tDelete = 60
adsbAssociator = AdsbAssociator()
ellipseParametric = EllipseParametric()
ellipsoidParametric = EllipsoidParametric()
async def event():
@ -97,6 +99,8 @@ async def event():
# localisation selection
if item["localisation"] == "ellipsoid-parametric":
localisation = ellipsoidParametric
elif item["localisation"] == "ellipse-parametric":
localisation = ellipseParametric
else:
print("Error: Localisation invalid.")
return