Add extrap Delay

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(

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] = {}
@@ -152,3 +162,37 @@ class EllipseParametric:
           output.append([x, y, z])
 
         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)]

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:
 

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 = [