mirror of
https://github.com/30hours/3lips.git
synced 2024-11-08 12:25:42 +00:00
Add extrap Delay
This commit is contained in:
parent
52a537f0c4
commit
33cc3574ee
4 changed files with 77 additions and 22 deletions
|
@ -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(
|
||||||
|
|
|
@ -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)]
|
|
@ -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:
|
||||||
|
|
||||||
|
|
|
@ -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 = [
|
||||||
|
|
Loading…
Reference in a new issue