Various bug fixes and adding mean/min methods

This commit is contained in:
30hours 2024-03-14 01:05:15 +00:00
parent 9f31e3190c
commit 129bf2fcc8
7 changed files with 77 additions and 36 deletions

View file

@ -25,8 +25,10 @@ associators = [
] ]
# todo: ellipse conic int (analytic), SX, arc length # todo: ellipse conic int (analytic), SX, arc length
localisations = [ 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"} {"name": "Spherical Intersection", "id": "spherical-intersection"}
] ]
adsbs = [ adsbs = [

View file

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

View file

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

View file

@ -13,7 +13,7 @@ class SphericalIntersection:
@class SphericalIntersection @class SphericalIntersection
@brief A class for intersecting ellipsoids using SX method. @brief A class for intersecting ellipsoids using SX method.
@details Uses associated detections from multiple radars. @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): def __init__(self):
@ -42,9 +42,6 @@ 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"]["location"][self.not_type]["latitude"], 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]["longitude"],
@ -84,24 +81,11 @@ class SphericalIntersection:
[x, y, z], [reference_ecef[0], [x, y, z], [reference_ecef[0],
reference_ecef[1], reference_ecef[2]]) reference_ecef[1], reference_ecef[2]])
R_i = (radar["delay"]*1000) + distance 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 z_vec[index, :] = (x_enu**2 + y_enu**2 + z_enu**2 - R_i**2)/2
# add to r # add to r
r[index, :] = R_i 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 # now compute matrix math
S_star = np.linalg.inv(S.T @ S) @ S.T S_star = np.linalg.inv(S.T @ S) @ S.T
a = S_star @ z_vec 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[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)) R_t[1] = (-2*(a.T @ b) + np.sqrt(discriminant))/(2*((b.T @ b)-1))
else: 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[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))) 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, 0]
@ -123,8 +106,6 @@ class SphericalIntersection:
output[target] = {} output[target] = {}
output[target]["points"] = [] output[target]["points"] = []
x_t_list = [np.squeeze(arr).tolist() for arr in x_t] 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 # convert points back to LLA
for index in range(len(x_t_list)): for index in range(len(x_t_list)):
@ -142,7 +123,4 @@ class SphericalIntersection:
else: else:
output[target]["points"].append(x_t_list[1]) output[target]["points"].append(x_t_list[1])
print('SX points:')
print(x_t_list)
return output return output

View file

@ -29,8 +29,10 @@ api = []
# init config # init config
tDelete = 60 tDelete = 60
adsbAssociator = AdsbAssociator() 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() sphericalIntersection = SphericalIntersection()
adsbTruth = AdsbTruth(5) adsbTruth = AdsbTruth(5)
save = True save = True
@ -114,10 +116,14 @@ async def event():
return return
# localisation selection # 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": elif item["localisation"] == "spherical-intersection":
localisation = sphericalIntersection localisation = sphericalIntersection
else: else:
@ -143,8 +149,10 @@ async def event():
# show ellipsoids of associated detections for 1 target # show ellipsoids of associated detections for 1 target
ellipsoids = {} 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: if associated_dets_2_radars:
# get first target key # get first target key
key = next(iter(associated_dets_2_radars)) key = next(iter(associated_dets_2_radars))
@ -169,7 +177,13 @@ async def event():
points = localisation.sample(ellipsoid, radar["delay"]*1000, 50) points = localisation.sample(ellipsoid, radar["delay"]*1000, 50)
for i in range(len(points)): for i in range(len(points)):
lat, lon, alt = Geometry.ecef2lla(points[i][0], points[i][1], points[i][2]) 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 ellipsoids[radar["radar"]] = points
stop_time = time.time() stop_time = time.time()

View file

@ -36,6 +36,22 @@ def interpolate_positions(timestamp_vector, truth_timestamp, truth_position):
return interpolated_positions 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(): def main():
# input handling # input handling
@ -82,6 +98,7 @@ def main():
# store target data # store target data
method_localisation = method["localisation"] method_localisation = method["localisation"]
# override skip a method
if method_localisation == "spherical-intersection": if method_localisation == "spherical-intersection":
continue continue
@ -146,13 +163,18 @@ def main():
radar4_lla[0], radar4_lla[1], radar4_lla[2])) radar4_lla[0], radar4_lla[1], radar4_lla[2]))
# plot x, y, z # 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): for i in range(3):
yaxis_truth = [pos[i] for pos in truth_position_resampled_enu] yaxis_truth = [pos[i] for pos in truth_position_resampled_enu]
plt.subplot(3, 1, i+1) plt.subplot(3, 1, i+1)
plt.plot(timestamp, yaxis_truth, label="ADS-B Truth") plt.plot(timestamp, yaxis_truth, label="ADS-B Truth")
for method in position: for method in position:
print(position[method])
if "detections_enu" not in position[method]:
continue
for i in range(3): for i in range(3):
print(position)
yaxis_target = [pos[i] for pos in position[method]["detections_enu"]] yaxis_target = [pos[i] for pos in position[method]["detections_enu"]]
plt.subplot(3, 1, i+1) plt.subplot(3, 1, i+1)
plt.plot(position[method]["timestamp"], yaxis_target, 'x', label=method) plt.plot(position[method]["timestamp"], yaxis_target, 'x', label=method)
@ -170,5 +192,25 @@ def main():
filename = 'plot_accuracy_' + args.target_name + '.png' filename = 'plot_accuracy_' + args.target_name + '.png'
plt.savefig('save/' + filename, bbox_inches='tight', pad_inches=0.01) 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__": if __name__ == "__main__":
main() main()

View file

@ -103,7 +103,7 @@ def main():
img = plt.imshow(data, aspect='auto', interpolation='none') img = plt.imshow(data, aspect='auto', interpolation='none')
y_extent = plt.gca().get_ylim() y_extent = plt.gca().get_ylim()
img.set_extent([start_time/1000, stop_time/1000, y_extent[1], y_extent[0]]) 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.xlabel('Timestamp')
plt.ylabel('Radar') plt.ylabel('Radar')
plt.tight_layout() plt.tight_layout()