github-mirrors/3lips
1
0
Fork 0
mirror of https://github.com/30hours/3lips.git synced 2024-11-08 12:25:42 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

Various bug fixes and adding mean/min methods

Browse source
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
Show stats Download patch file Download diff file Expand all files Collapse all files

6
api/main.py
View file

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

2
event/algorithm/localisation/EllipseParametric.py
View file

@ -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],

3
event/algorithm/localisation/EllipsoidParametric.py
View file

@ -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

24
event/algorithm/localisation/SphericalIntersection.py
View file

@ -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)):
@ -142,7 +123,4 @@ class SphericalIntersection:
else:
output[target]["points"].append(x_t_list[1])
print('SX points:')
print(x_t_list)
return output

32
event/event.py
View file

@ -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()

44
script/plot_accuracy.py
View file

@ -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()

2
script/plot_associate.py
View file

@ -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()