Various bug fixes and adding mean/min methods

api/main.py

@@ -25,8 +25,10 @@ associators = [
 ]
 # todo: ellipse conic int (analytic), SX, arc length
 localisations = [
-  {"name": "Ellipse Parametric", "id": "ellipse-parametric"},
+  {"name": "Ellipse Parametric (Mean)", "id": "ellipse-parametric-mean"},
-  {"name": "Ellipsoid Parametric", "id": "ellipsoid-parametric"},
+  {"name": "Ellipse Parametric (Min)", "id": "ellipse-parametric-min"},
+  {"name": "Ellipsoid Parametric (Mean)", "id": "ellipsoid-parametric-mean"},
+  {"name": "Ellipsoid Parametric (Min)", "id": "ellipsoid-parametric-min"},
   {"name": "Spherical Intersection", "id": "spherical-intersection"}
 ]
 adsbs = [

event/algorithm/localisation/EllipseParametric.py

@@ -142,6 +142,8 @@ class EllipseParametric:
             output[target] = {}
             output[target]["points"] = []
             for i in range(len(samples_intersect)):
+              print('err??', flush=True)
+              print(samples_intersect, flush=True)
               samples_intersect[i] = Geometry.ecef2lla(
                 samples_intersect[i][0], 
                 samples_intersect[i][1], 

event/algorithm/localisation/EllipsoidParametric.py

@@ -103,6 +103,9 @@ class EllipsoidParametric:
                 average_point = Geometry.average_points(samples_intersect)
                 samples_intersect = [average_point]
 
+                if len(samples_intersect) == 0:
+                    return output 
+
             elif self.method == "minimum":
 
                 min_distance = self.threshold

event/algorithm/localisation/SphericalIntersection.py

@@ -13,7 +13,7 @@ class SphericalIntersection:
     @class SphericalIntersection
     @brief A class for intersecting ellipsoids using SX method.
     @details Uses associated detections from multiple radars.
-    @see blah2 at https://github.com/30hours/blah2.
+    @see https://ieeexplore.ieee.org/document/6129656
     """
 
     def __init__(self):
@@ -42,9 +42,6 @@ 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"]["location"][self.not_type]["latitude"], 
           radar_data[radar]["config"]["location"][self.not_type]["longitude"],
@@ -84,24 +81,11 @@ class SphericalIntersection:
                   [x, y, z], [reference_ecef[0], 
                   reference_ecef[1], reference_ecef[2]])
                 R_i = (radar["delay"]*1000) + distance
-                # print('R_i', flush=True)
-                # print(R_i, flush=True)
-                # print(radar["delay"]*1000, flush=True)
                 z_vec[index, :] = (x_enu**2 + y_enu**2 + z_enu**2 - R_i**2)/2
 
                 # add to r
                 r[index, :] = R_i
 
-            # print first to check
-            print('start printing SX:', flush=True)
-            print(S, flush=True)
-            print(S.size, flush=True)
-            print(z_vec, flush=True)
-            print(z_vec.size, flush=True)
-            print(r, flush=True)
-            print(r.size, flush=True)
-
-
             # now compute matrix math
             S_star = np.linalg.inv(S.T @ S) @ S.T
             a = S_star @ z_vec
@@ -112,7 +96,6 @@ class SphericalIntersection:
                 R_t[0] = (-2*(a.T @ b) - np.sqrt(discriminant))/(2*((b.T @ b)-1))
                 R_t[1] = (-2*(a.T @ b) + np.sqrt(discriminant))/(2*((b.T @ b)-1))
             else:
-                print('@@@ discriminant < 0', flush=True)
                 R_t[0] = np.real((-2*(a.T @ b) - np.sqrt(discriminant + 0j))/(2*((b.T @ b)-1)))
                 R_t[1] = np.real((-2*(a.T @ b) + np.sqrt(discriminant + 0j))/(2*((b.T @ b)-1)))
             x_t = [0, 0]
@@ -123,8 +106,6 @@ class SphericalIntersection:
             output[target] = {}
             output[target]["points"] = []
             x_t_list = [np.squeeze(arr).tolist() for arr in x_t]
-            print('x_t in ENU?')
-            print(x_t_list)
 
             # convert points back to LLA
             for index in range(len(x_t_list)):
@@ -141,8 +122,5 @@ class SphericalIntersection:
                 output[target]["points"].append(x_t_list[0])
             else:
                 output[target]["points"].append(x_t_list[1])
-
-            print('SX points:')
-            print(x_t_list)
                 
         return output

event/event.py

@@ -29,8 +29,10 @@ api = []
 # init config
 tDelete = 60
 adsbAssociator = AdsbAssociator()
-ellipseParametric = EllipseParametric("mean", 200, 500)
+ellipseParametricMean = EllipseParametric("mean", 150, 500)
-ellipsoidParametric = EllipsoidParametric("mean", 100, 500)
+ellipseParametricMin = EllipseParametric("min", 150, 500)
+ellipsoidParametricMean = EllipsoidParametric("mean", 120, 1000)
+ellipsoidParametricMin = EllipsoidParametric("min", 120, 1000)
 sphericalIntersection = SphericalIntersection()
 adsbTruth = AdsbTruth(5)
 save = True
@@ -114,10 +116,14 @@ async def event():
         return
 
       # localisation selection
-      if item["localisation"] == "ellipse-parametric":
+      if item["localisation"] == "ellipse-parametric-mean":
-        localisation = ellipseParametric
+        localisation = ellipseParametricMean
-      elif item["localisation"] == "ellipsoid-parametric":
+      elif item["localisation"] == "ellipse-parametric-min":
-        localisation = ellipsoidParametric
+        localisation = ellipseParametricMin
+      elif item["localisation"] == "ellipsoid-parametric-mean":
+        localisation = ellipsoidParametricMean
+      elif item["localisation"] == "ellipsoid-parametric-min":
+        localisation = ellipsoidParametricMin
       elif item["localisation"] == "spherical-intersection":
         localisation = sphericalIntersection
       else:
@@ -143,8 +149,10 @@ async def event():
 
       # show ellipsoids of associated detections for 1 target
       ellipsoids = {}
-      if item["localisation"] == "ellipse-parametric" or \
+      if item["localisation"] == "ellipse-parametric-mean" or \
-      item["localisation"] == "ellipsoid-parametric":
+      item["localisation"] == "ellipsoid-parametric-mean" or \
+      item["localisation"] == "ellipse-parametric-min" or \
+      item["localisation"] == "ellipsoid-parametric-min":
           if associated_dets_2_radars:
             # get first target key
             key = next(iter(associated_dets_2_radars))
@@ -169,7 +177,13 @@ async def event():
               points = localisation.sample(ellipsoid, radar["delay"]*1000, 50)
               for i in range(len(points)):
                 lat, lon, alt = Geometry.ecef2lla(points[i][0], points[i][1], points[i][2])
-                points[i] = ([round(lat, 3), round(lon, 3), 0])
+                if item["localisation"] == "ellipsoid-parametric-mean" or \
+                item["localisation"] == "ellipsoid-parametric-min":
+                  alt = round(alt)
+                if item["localisation"] == "ellipse-parametric-mean" or \
+                item["localisation"] == "ellipse-parametric-min":
+                  alt = 0
+                points[i] = ([round(lat, 3), round(lon, 3), alt])
               ellipsoids[radar["radar"]] = points
 
       stop_time = time.time()

script/plot_accuracy.py

@@ -36,6 +36,22 @@ def interpolate_positions(timestamp_vector, truth_timestamp, truth_position):
 
     return interpolated_positions
 
+def calculate_rmse(actual_values, predicted_values):
+    # Convert lists to NumPy arrays for easy calculations
+    actual_values = np.array(actual_values)
+    predicted_values = np.array(predicted_values)
+
+    # Calculate the squared differences
+    squared_diff = (actual_values - predicted_values) ** 2
+
+    # Calculate the mean squared error
+    mean_squared_error = np.mean(squared_diff)
+
+    # Calculate the root mean squared error
+    rmse = np.sqrt(mean_squared_error)
+
+    return rmse
+
 def main():
   
     # input handling
@@ -82,6 +98,7 @@ def main():
             # store target data
             method_localisation = method["localisation"]
 
+            # override skip a method
             if method_localisation == "spherical-intersection":
               continue
 
@@ -146,13 +163,18 @@ def main():
           radar4_lla[0], radar4_lla[1], radar4_lla[2]))
 
     # plot x, y, z
-    plt.figure(figsize=(5,7))
+    #plt.figure(figsize=(5,7))
+    fig, axes = plt.subplots(3, 1, figsize=(5, 7), sharex=True)
     for i in range(3):
         yaxis_truth = [pos[i] for pos in truth_position_resampled_enu]
         plt.subplot(3, 1, i+1)
         plt.plot(timestamp, yaxis_truth, label="ADS-B Truth")
     for method in position:
+        print(position[method])
+        if "detections_enu" not in position[method]:
+          continue
         for i in range(3):
+            print(position)
             yaxis_target = [pos[i] for pos in position[method]["detections_enu"]]
             plt.subplot(3, 1, i+1)
             plt.plot(position[method]["timestamp"], yaxis_target, 'x', label=method)
@@ -170,5 +192,25 @@ def main():
     filename = 'plot_accuracy_' + args.target_name + '.png'
     plt.savefig('save/' + filename, bbox_inches='tight', pad_inches=0.01)
 
+    # save tabular data
+    table = {}
+    for method in position:
+        if "detections_enu" not in position[method]:
+          continue
+        table[method] = {}
+        for i in range(3):
+
+            yaxis_truth = np.array([pos[i] for pos in truth_position_resampled_enu])
+            matching_indices = np.isin(np.array(timestamp), np.array(position[method]["timestamp"]))
+            yaxis_truth_target = yaxis_truth[matching_indices]
+            
+            yaxis_target = [pos[i] for pos in position[method]["detections_enu"]]
+            table[method][str(i)] = calculate_rmse(yaxis_target, yaxis_truth_target)
+            print('test')
+            print(yaxis_target)
+            print(yaxis_truth_target)
+
+    print(table)
+
 if __name__ == "__main__":
     main()

script/plot_associate.py

@@ -103,7 +103,7 @@ def main():
     img = plt.imshow(data, aspect='auto', interpolation='none')
     y_extent = plt.gca().get_ylim()
     img.set_extent([start_time/1000, stop_time/1000, y_extent[1], y_extent[0]])
-    plt.yticks(np.arange(len(radar_label)), radar_label, rotation='vertical')
+    plt.yticks(np.arange(len(radar_label)), radar_label[::-1], rotation='vertical')
     plt.xlabel('Timestamp')
     plt.ylabel('Radar')
     plt.tight_layout()