From 33cc3574ee215a0af56aa8720ae039346d01eadd Mon Sep 17 00:00:00 2001 From: 30hours Date: Mon, 11 Mar 2024 05:57:37 +0000 Subject: [PATCH] Add extrap Delay --- event/algorithm/associator/AdsbAssociator.py | 15 ++-- .../localisation/EllipseParametric.py | 70 +++++++++++++++---- .../localisation/SphericalIntersection.py | 9 ++- event/event.py | 5 ++ 4 files changed, 77 insertions(+), 22 deletions(-) diff --git a/event/algorithm/associator/AdsbAssociator.py b/event/algorithm/associator/AdsbAssociator.py index ff21c4f..39411ba 100644 --- a/event/algorithm/associator/AdsbAssociator.py +++ b/event/algorithm/associator/AdsbAssociator.py @@ -62,7 +62,7 @@ class AdsbAssociator: # associate radar and truth assoc_detections_radar.append(self.process_1_radar( radar, radar_data[radar]["detection"], - adsb_detections, timestamp)) + adsb_detections, timestamp, radar_data[radar]["config"]["capture"]["fc"])) # associate detections between radars output = {} @@ -76,7 +76,7 @@ class AdsbAssociator: return output - def process_1_radar(self, radar, radar_detections, adsb_detections, timestamp): + def process_1_radar(self, radar, radar_detections, adsb_detections, timestamp, fc): """ @brief Associate detections between 1 radar/truth pair. @@ -96,10 +96,13 @@ class AdsbAssociator: if 'delay' in adsb_detections[aircraft] and len(radar_detections['delay']) >= 1: - # extrapolate delay/Doppler to current time - # delta_t = (timestamp - adsb_detections[aircraft]['timestamp'])/1000 - # delay = 1000*adsb_detections[aircraft]['delay'] + \ - + # extrapolate delay to current time + # TODO extrapolate Doppler too + for i in range(len(radar_detections['delay'])): + delta_t = (timestamp - radar_detections['timestamp'])/1000 + delay = (1000*radar_detections['delay'][i] + \ + (radar_detections['doppler'][i]*(299792458/fc))*delta_t)/1000 + radar_detections['delay'][i] = delay # distance from aircraft to all detections closest_point, distance = self.closest_point( diff --git a/event/algorithm/localisation/EllipseParametric.py b/event/algorithm/localisation/EllipseParametric.py index 292e07f..1da29fb 100644 --- a/event/algorithm/localisation/EllipseParametric.py +++ b/event/algorithm/localisation/EllipseParametric.py @@ -7,6 +7,9 @@ from data.Ellipsoid import Ellipsoid from algorithm.geometry.Geometry import Geometry import numpy as np import math +import itertools + +from concurrent.futures import ThreadPoolExecutor class EllipseParametric: @@ -24,7 +27,7 @@ class EllipseParametric: """ self.ellipsoids = [] - self.nSamples = 150 + self.nSamples = 80 self.threshold = 800 def process(self, assoc_detections, radar_data): @@ -85,17 +88,24 @@ class EllipseParametric: 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) + # 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) + + # find closest points bruteforce + points = list(target_samples[target].values()) + result_points, result_distance = self.closest_points_bruteforce(points) + average_point = self.average_points(result_points) + if result_distance < self.threshold: + samples_intersect.append(average_point) # remove duplicates and convert to LLA output[target] = {} @@ -151,4 +161,38 @@ class EllipseParametric: ellipsoid.midpoint_lla[2]) output.append([x, y, z]) - return output \ No newline at end of file + return output + + def euclidean_distance(self, point1, point2): + return np.linalg.norm(np.array(point1) - np.array(point2)) + + # def closest_points_bruteforce(self, point_sets): + # closest_distance = float('inf') + # closest_points = None + + # for combination in itertools.product(*point_sets): + # distance = sum(self.euclidean_distance(combination[i], combination[i+1]) for i in range(len(point_sets)-1)) + # if distance < closest_distance: + # closest_distance = distance + # closest_points = combination + + # return closest_points, closest_distance + + def closest_points_bruteforce(point_sets): + closest_distance = float('inf') + closest_points = None + + def calculate_distance(combination): + nonlocal closest_distance, closest_points + distance = sum(euclidean_distance(combination[i], combination[i+1]) for i in range(len(point_sets)-1)) + if distance < closest_distance: + closest_distance = distance + closest_points = combination + + with ThreadPoolExecutor() as executor: + executor.map(calculate_distance, itertools.product(*point_sets)) + + return closest_points, closest_distance + + def average_points(self, points): + return [sum(coord) / len(coord) for coord in zip(*points)] \ No newline at end of file diff --git a/event/algorithm/localisation/SphericalIntersection.py b/event/algorithm/localisation/SphericalIntersection.py index 20ce84f..e3fe70d 100644 --- a/event/algorithm/localisation/SphericalIntersection.py +++ b/event/algorithm/localisation/SphericalIntersection.py @@ -42,10 +42,13 @@ class SphericalIntersection: # pick first radar rx node as ENU reference (arbitrary) radar = next(iter(radar_data)) + print(radar_data) + print(radar) + print(radar_data[radar]["config"]) reference_lla = [ - radar_data[radar]["config"][self.type]["latitude"], - radar_data[radar]["config"][self.type]["longitude"], - radar_data[radar]["config"][self.type]["altitude"]] + radar_data[radar]["config"]["location"][self.type]["latitude"], + radar_data[radar]["config"]["location"][self.type]["longitude"], + radar_data[radar]["config"]["location"][self.type]["altitude"]] for target in assoc_detections: diff --git a/event/event.py b/event/event.py index bbe21bd..4804ffe 100644 --- a/event/event.py +++ b/event/event.py @@ -38,6 +38,8 @@ saveFile = '/app/save/' + str(int(time.time())) + '.ndjson' async def event(): + start_time = time.time() + global api, save timestamp = int(time.time()*1000) api_event = copy.copy(api) @@ -170,12 +172,15 @@ async def event(): points[i] = ([round(lat, 3), round(lon, 3), 0]) ellipsoids[radar["radar"]] = points + stop_time = time.time() + # output data to API item["timestamp_event"] = timestamp item["truth"] = truth_adsb[item["adsb"]] item["detections_associated"] = associated_dets item["detections_localised"] = localised_dets item["ellipsoids"] = ellipsoids + item["time"] = stop_time - start_time # delete old API requests api_event = [