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

Lots of progress forgot

Browse source
This commit is contained in:
30hours 2024-03-12 09:11:21 +00:00
parent 33cc3574ee
commit 21c2d549f3
5 changed files with 112 additions and 48 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
api/map/event/radar.js
View file

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

2
event/algorithm/associator/AdsbAssociator.py
View file

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

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

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

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

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

4
script/plot_accuracy.py
View file

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