From 21c2d549f31f3ddf4cef7a348d47285789058335 Mon Sep 17 00:00:00 2001 From: 30hours Date: Tue, 12 Mar 2024 09:11:21 +0000 Subject: [PATCH] Lots of progress forgot --- api/map/event/radar.js | 2 +- event/algorithm/associator/AdsbAssociator.py | 2 +- .../localisation/EllipseParametric.py | 78 +++++++++++++------ .../localisation/SphericalIntersection.py | 74 +++++++++++++----- script/plot_accuracy.py | 4 + 5 files changed, 112 insertions(+), 48 deletions(-) diff --git a/api/map/event/radar.js b/api/map/event/radar.js index fde9312..2cef74e 100644 --- a/api/map/event/radar.js +++ b/api/map/event/radar.js @@ -16,7 +16,7 @@ function event_radar() { return; } - removeEntitiesOlderThanAndFade("detection", 10, 0.5); + removeEntitiesOlderThanAndFade("detection", 90, 0.5); for (const key in data["detections_localised"]) { if (data["detections_localised"].hasOwnProperty(key)) { diff --git a/event/algorithm/associator/AdsbAssociator.py b/event/algorithm/associator/AdsbAssociator.py index 39411ba..f709365 100644 --- a/event/algorithm/associator/AdsbAssociator.py +++ b/event/algorithm/associator/AdsbAssociator.py @@ -101,7 +101,7 @@ class AdsbAssociator: 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['doppler'][i]*(299792458/fc))*delta_t)/1000 radar_detections['delay'][i] = delay # distance from aircraft to all detections diff --git a/event/algorithm/localisation/EllipseParametric.py b/event/algorithm/localisation/EllipseParametric.py index 1da29fb..b912897 100644 --- a/event/algorithm/localisation/EllipseParametric.py +++ b/event/algorithm/localisation/EllipseParametric.py @@ -27,7 +27,7 @@ class EllipseParametric: """ self.ellipsoids = [] - self.nSamples = 80 + self.nSamples = 150 self.threshold = 800 def process(self, assoc_detections, radar_data): @@ -100,12 +100,40 @@ class EllipseParametric: # if valid_point: # samples_intersect.append(point1) + # average_point = self.average_points(samples_intersect) + # samples_intersect = [average_point] + + min_distance = self.threshold + min_point1 = None + for point1 in target_samples[target][radar_keys[0]]: + valid_point = True + distance_from_point1 = [self.threshold]*(len(radar_keys)-1) + # loop over each other list + for i in range(1, len(radar_keys)): + if i > 1 and distance_from_point1[i-1] > self.threshold: + valid_point = False + break + # loop points in other list + for point2 in target_samples[target][radar_keys[i]]: + distance = Geometry.distance_ecef(point1, point2) + if distance < distance_from_point1[i-1]: + distance_from_point1[i-1] = distance + norm = math.sqrt(sum(x ** 2 for x in distance_from_point1)) + if valid_point and norm < min_distance: + min_distance = norm + min_point1 = point1 + + if min_point1 is not None: + samples_intersect.append(min_point1) + else: + return output + # 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) + # 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] = {} @@ -166,33 +194,33 @@ class EllipseParametric: 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): + def closest_points_bruteforce(self, 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)) + 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 - with ThreadPoolExecutor() as executor: - executor.map(calculate_distance, itertools.product(*point_sets)) - 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)] \ No newline at end of file diff --git a/event/algorithm/localisation/SphericalIntersection.py b/event/algorithm/localisation/SphericalIntersection.py index e3fe70d..6b8fae7 100644 --- a/event/algorithm/localisation/SphericalIntersection.py +++ b/event/algorithm/localisation/SphericalIntersection.py @@ -46,9 +46,11 @@ class SphericalIntersection: print(radar) print(radar_data[radar]["config"]) reference_lla = [ - radar_data[radar]["config"]["location"][self.type]["latitude"], - radar_data[radar]["config"]["location"][self.type]["longitude"], - radar_data[radar]["config"]["location"][self.type]["altitude"]] + radar_data[radar]["config"]["location"][self.not_type]["latitude"], + radar_data[radar]["config"]["location"][self.not_type]["longitude"], + radar_data[radar]["config"]["location"][self.not_type]["altitude"]] + reference_ecef = Geometry.lla2ecef(reference_lla[0], + reference_lla[1], reference_lla[2]) for target in assoc_detections: @@ -58,7 +60,7 @@ class SphericalIntersection: S = np.zeros((nDetections, 3)) # additional vector - z = np.zeros((nDetections, 1)) + z_vec = np.zeros((nDetections, 1)) # bistatic range vector r r = np.zeros((nDetections, 1)) @@ -78,38 +80,68 @@ class SphericalIntersection: S[index, :] = [x_enu, y_enu, z_enu] # add to z - x2, y2, z2 = Geometry.lla2ecef( - config['location'][self.not_type]['latitude'], - config['location'][self.not_type]['longitude'], - config['location'][self.not_type]['altitude']) - distance = Geometry.distance_ecef([x, y, z], [x2, y2, z2]) - z[index, :] = (x**2 + y**2 + z**2 - distance**2)/2 + distance = Geometry.distance_ecef( + [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, :] = radar["delay"] + distance + 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 + a = S_star @ z_vec b = S_star @ r R_t = [0, 0] - R_t[0] = (-2*(a.T @ b) - np.sqrt(4*(a.T @ b)**2 - \ - 4*((b.T @ b)-1)*(a.T @ a)))/2*((b.T @ b)-1) - R_t[1] = (-2*(a.T @ b) + np.sqrt(4*(a.T @ b)**2 - \ - 4*((b.T @ b)-1)*(a.T @ a)))/2*((b.T @ b)-1) + discrimninant = 4*((a.T @ b)**2) - 4*((b.T @ b) - 1)*(a.T @ a) + if discriminant >= 0: + 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: + 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] - x_t[0] = S_star @ (z + r*R_t[0]) - x_t[1] = S_star @ (z + r*R_t[1]) + x_t[0] = S_star @ (z_vec + r*R_t[0]) + x_t[1] = S_star @ (z_vec + r*R_t[1]) # use solution with highest altitude 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)): + x, y, z = Geometry.enu2ecef(x_t_list[index][0], + x_t_list[index][1], + x_t_list[index][2], + reference_lla[0], + reference_lla[1], + reference_lla[2]) + lat, lon, alt = Geometry.ecef2lla(x, y, z) + x_t_list[index] = [lat, lon, alt] + if x_t[0][2] > x_t[1][2]: - output[target]["points"].append(x_t[0]) + output[target]["points"].append(x_t_list[0]) else: - output[target]["points"].append(x_t[1]) + output[target]["points"].append(x_t_list[1]) print('SX points:') - print(x_t) + print(x_t_list) return output \ No newline at end of file diff --git a/script/plot_accuracy.py b/script/plot_accuracy.py index 974b8ad..9937071 100644 --- a/script/plot_accuracy.py +++ b/script/plot_accuracy.py @@ -81,6 +81,10 @@ def main(): # store target data method_localisation = method["localisation"] + + if method_localisation == "spherical-intersection": + continue + if method_localisation not in position: position[method_localisation] = {} position[method_localisation]["timestamp"] = []