From 129bf2fcc8cbe2a26ea68a36ffab70508b719196 Mon Sep 17 00:00:00 2001 From: 30hours Date: Thu, 14 Mar 2024 01:05:15 +0000 Subject: [PATCH] Various bug fixes and adding mean/min methods --- api/main.py | 6 ++- .../localisation/EllipseParametric.py | 2 + .../localisation/EllipsoidParametric.py | 3 ++ .../localisation/SphericalIntersection.py | 24 +--------- event/event.py | 32 ++++++++++---- script/plot_accuracy.py | 44 ++++++++++++++++++- script/plot_associate.py | 2 +- 7 files changed, 77 insertions(+), 36 deletions(-) diff --git a/api/main.py b/api/main.py index 8a66ab5..d708ac1 100644 --- a/api/main.py +++ b/api/main.py @@ -25,8 +25,10 @@ associators = [ ] # todo: ellipse conic int (analytic), SX, arc length localisations = [ - {"name": "Ellipse Parametric", "id": "ellipse-parametric"}, - {"name": "Ellipsoid Parametric", "id": "ellipsoid-parametric"}, + {"name": "Ellipse Parametric (Mean)", "id": "ellipse-parametric-mean"}, + {"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 = [ diff --git a/event/algorithm/localisation/EllipseParametric.py b/event/algorithm/localisation/EllipseParametric.py index ce93ef5..f65d19d 100644 --- a/event/algorithm/localisation/EllipseParametric.py +++ b/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], diff --git a/event/algorithm/localisation/EllipsoidParametric.py b/event/algorithm/localisation/EllipsoidParametric.py index 438c5f6..741412a 100644 --- a/event/algorithm/localisation/EllipsoidParametric.py +++ b/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 diff --git a/event/algorithm/localisation/SphericalIntersection.py b/event/algorithm/localisation/SphericalIntersection.py index 6cd5589..c29c4b0 100644 --- a/event/algorithm/localisation/SphericalIntersection.py +++ b/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 \ No newline at end of file diff --git a/event/event.py b/event/event.py index 17df55d..9d85e0d 100644 --- a/event/event.py +++ b/event/event.py @@ -29,8 +29,10 @@ api = [] # init config tDelete = 60 adsbAssociator = AdsbAssociator() -ellipseParametric = EllipseParametric("mean", 200, 500) -ellipsoidParametric = EllipsoidParametric("mean", 100, 500) +ellipseParametricMean = EllipseParametric("mean", 150, 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": - localisation = ellipseParametric - elif item["localisation"] == "ellipsoid-parametric": - localisation = ellipsoidParametric + if item["localisation"] == "ellipse-parametric-mean": + localisation = ellipseParametricMean + elif item["localisation"] == "ellipse-parametric-min": + 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 \ - item["localisation"] == "ellipsoid-parametric": + if item["localisation"] == "ellipse-parametric-mean" or \ + 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() diff --git a/script/plot_accuracy.py b/script/plot_accuracy.py index 9937071..34f1d21 100644 --- a/script/plot_accuracy.py +++ b/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() diff --git a/script/plot_associate.py b/script/plot_associate.py index ecdf0a4..8a087e8 100644 --- a/script/plot_associate.py +++ b/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()