diff --git a/api/main.py b/api/main.py index b4e8336..ac9791e 100644 --- a/api/main.py +++ b/api/main.py @@ -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": ""} diff --git a/event/algorithm/localisation/EllipseParametric.py b/event/algorithm/localisation/EllipseParametric.py index e69de29..9e0bb3e 100644 --- a/event/algorithm/localisation/EllipseParametric.py +++ b/event/algorithm/localisation/EllipseParametric.py @@ -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 \ No newline at end of file diff --git a/event/algorithm/localisation/EllipsoidParametric.py b/event/algorithm/localisation/EllipsoidParametric.py index 2eec4ac..5057701 100644 --- a/event/algorithm/localisation/EllipsoidParametric.py +++ b/event/algorithm/localisation/EllipsoidParametric.py @@ -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 = [] diff --git a/event/event.py b/event/event.py index d28974a..999f4f4 100644 --- a/event/event.py +++ b/event/event.py @@ -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