Add extrap Delay

This commit is contained in:
30hours 2024-03-11 05:57:37 +00:00
parent 52a537f0c4
commit 33cc3574ee
4 changed files with 77 additions and 22 deletions

View file

@ -62,7 +62,7 @@ class AdsbAssociator:
# associate radar and truth # associate radar and truth
assoc_detections_radar.append(self.process_1_radar( assoc_detections_radar.append(self.process_1_radar(
radar, radar_data[radar]["detection"], radar, radar_data[radar]["detection"],
adsb_detections, timestamp)) adsb_detections, timestamp, radar_data[radar]["config"]["capture"]["fc"]))
# associate detections between radars # associate detections between radars
output = {} output = {}
@ -76,7 +76,7 @@ class AdsbAssociator:
return output 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. @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: if 'delay' in adsb_detections[aircraft] and len(radar_detections['delay']) >= 1:
# extrapolate delay/Doppler to current time # extrapolate delay to current time
# delta_t = (timestamp - adsb_detections[aircraft]['timestamp'])/1000 # TODO extrapolate Doppler too
# delay = 1000*adsb_detections[aircraft]['delay'] + \ 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 # distance from aircraft to all detections
closest_point, distance = self.closest_point( closest_point, distance = self.closest_point(

View file

@ -7,6 +7,9 @@ from data.Ellipsoid import Ellipsoid
from algorithm.geometry.Geometry import Geometry from algorithm.geometry.Geometry import Geometry
import numpy as np import numpy as np
import math import math
import itertools
from concurrent.futures import ThreadPoolExecutor
class EllipseParametric: class EllipseParametric:
@ -24,7 +27,7 @@ class EllipseParametric:
""" """
self.ellipsoids = [] self.ellipsoids = []
self.nSamples = 150 self.nSamples = 80
self.threshold = 800 self.threshold = 800
def process(self, assoc_detections, radar_data): def process(self, assoc_detections, radar_data):
@ -85,17 +88,24 @@ class EllipseParametric:
samples_intersect = [] samples_intersect = []
# loop points in master ellipsoid # loop points in master ellipsoid
for point1 in target_samples[target][radar_keys[0]]: # for point1 in target_samples[target][radar_keys[0]]:
valid_point = True # valid_point = True
# loop over each other list # # loop over each other list
for i in range(1, len(radar_keys)): # for i in range(1, len(radar_keys)):
# loop points in other list # # loop points in other list
if not any(Geometry.distance_ecef(point1, point2) < self.threshold # if not any(Geometry.distance_ecef(point1, point2) < self.threshold
for point2 in target_samples[target][radar_keys[i]]): # for point2 in target_samples[target][radar_keys[i]]):
valid_point = False # valid_point = False
break # break
if valid_point: # if valid_point:
samples_intersect.append(point1) # 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 # remove duplicates and convert to LLA
output[target] = {} output[target] = {}
@ -151,4 +161,38 @@ class EllipseParametric:
ellipsoid.midpoint_lla[2]) ellipsoid.midpoint_lla[2])
output.append([x, y, z]) output.append([x, y, z])
return output 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)]

View file

@ -42,10 +42,13 @@ class SphericalIntersection:
# pick first radar rx node as ENU reference (arbitrary) # pick first radar rx node as ENU reference (arbitrary)
radar = next(iter(radar_data)) radar = next(iter(radar_data))
print(radar_data)
print(radar)
print(radar_data[radar]["config"])
reference_lla = [ reference_lla = [
radar_data[radar]["config"][self.type]["latitude"], radar_data[radar]["config"]["location"][self.type]["latitude"],
radar_data[radar]["config"][self.type]["longitude"], radar_data[radar]["config"]["location"][self.type]["longitude"],
radar_data[radar]["config"][self.type]["altitude"]] radar_data[radar]["config"]["location"][self.type]["altitude"]]
for target in assoc_detections: for target in assoc_detections:

View file

@ -38,6 +38,8 @@ saveFile = '/app/save/' + str(int(time.time())) + '.ndjson'
async def event(): async def event():
start_time = time.time()
global api, save global api, save
timestamp = int(time.time()*1000) timestamp = int(time.time()*1000)
api_event = copy.copy(api) api_event = copy.copy(api)
@ -170,12 +172,15 @@ async def event():
points[i] = ([round(lat, 3), round(lon, 3), 0]) points[i] = ([round(lat, 3), round(lon, 3), 0])
ellipsoids[radar["radar"]] = points ellipsoids[radar["radar"]] = points
stop_time = time.time()
# output data to API # output data to API
item["timestamp_event"] = timestamp item["timestamp_event"] = timestamp
item["truth"] = truth_adsb[item["adsb"]] item["truth"] = truth_adsb[item["adsb"]]
item["detections_associated"] = associated_dets item["detections_associated"] = associated_dets
item["detections_localised"] = localised_dets item["detections_localised"] = localised_dets
item["ellipsoids"] = ellipsoids item["ellipsoids"] = ellipsoids
item["time"] = stop_time - start_time
# delete old API requests # delete old API requests
api_event = [ api_event = [