mirror of
https://github.com/30hours/3lips.git
synced 2024-11-18 12:33:58 +00:00
Fix indentation
This commit is contained in:
parent
ad19fe9754
commit
543d5dcb60
17 changed files with 1504 additions and 1531 deletions
|
@ -89,7 +89,7 @@ def api():
|
||||||
if not all(item in valid['localisations'] for item in localisations_api):
|
if not all(item in valid['localisations'] for item in localisations_api):
|
||||||
return 'Invalid localisation'
|
return 'Invalid localisation'
|
||||||
if not all(item in valid['adsbs'] for item in adsbs_api):
|
if not all(item in valid['adsbs'] for item in adsbs_api):
|
||||||
return 'Invalid ADSB']
|
return 'Invalid ADSB'
|
||||||
# send to event handler
|
# send to event handler
|
||||||
try:
|
try:
|
||||||
reply_chunks = message_api_request.send_message(api)
|
reply_chunks = message_api_request.send_message(api)
|
||||||
|
|
|
@ -19,7 +19,6 @@ function event_adsb() {
|
||||||
// Schedule the next fetch after a delay (e.g., 5 seconds)
|
// Schedule the next fetch after a delay (e.g., 5 seconds)
|
||||||
setTimeout(event_adsb, 1000);
|
setTimeout(event_adsb, 1000);
|
||||||
});
|
});
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Function to update aircraft points
|
// Function to update aircraft points
|
||||||
|
|
|
@ -47,12 +47,14 @@ class Message:
|
||||||
thread.start()
|
thread.start()
|
||||||
|
|
||||||
def handle_client(self, conn, addr):
|
def handle_client(self, conn, addr):
|
||||||
|
|
||||||
"""
|
"""
|
||||||
Handle communication with a connected client.
|
@brief Handle communication with a connected client.
|
||||||
:param conn (socket.socket): The socket object for the connected client.
|
@param conn (socket.socket): The socket object for the connected client.
|
||||||
:param addr (tuple): The address (host, port) of the connected client.
|
@param addr (tuple): The address (host, port) of the connected client.
|
||||||
:return None.
|
@return None.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
with conn:
|
with conn:
|
||||||
while True:
|
while True:
|
||||||
data = conn.recv(8096)
|
data = conn.recv(8096)
|
||||||
|
@ -75,11 +77,13 @@ class Message:
|
||||||
conn.sendall(reply[i:i + 8096].encode())
|
conn.sendall(reply[i:i + 8096].encode())
|
||||||
|
|
||||||
def send_message(self, message):
|
def send_message(self, message):
|
||||||
|
|
||||||
"""
|
"""
|
||||||
Send a message to the specified host and port.
|
@brief Send a message to the specified host and port.
|
||||||
:param message (str): The message to be sent.
|
@param message (str): The message to be sent.
|
||||||
:return generator: A generator yielding chunks of the reply.
|
@return generator: A generator yielding chunks of the reply.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as client_socket:
|
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as client_socket:
|
||||||
client_socket.settimeout(3)
|
client_socket.settimeout(3)
|
||||||
try:
|
try:
|
||||||
|
|
|
@ -21,21 +21,29 @@ class Geometry:
|
||||||
|
|
||||||
def lla2ecef(lat, lon, alt):
|
def lla2ecef(lat, lon, alt):
|
||||||
|
|
||||||
|
"""
|
||||||
|
@brief Converts geodetic coordinates (latitude, longitude, altitude) to ECEF coordinates.
|
||||||
|
@param lat (float): Geodetic latitude in degrees.
|
||||||
|
@param lon (float): Geodetic longitude in degrees.
|
||||||
|
@param alt (float): Altitude above the ellipsoid in meters.
|
||||||
|
@return ecef_x (float): ECEF x-coordinate in meters.
|
||||||
|
@return ecef_y (float): ECEF y-coordinate in meters.
|
||||||
|
@return ecef_z (float): ECEF z-coordinate in meters.
|
||||||
|
"""
|
||||||
|
|
||||||
# WGS84 constants
|
# WGS84 constants
|
||||||
a = 6378137.0 # semi-major axis in meters
|
a = 6378137.0 # semi-major axis in meters
|
||||||
f = 1 / 298.257223563 # flattening
|
f = 1 / 298.257223563 # flattening
|
||||||
e = 0.081819190842622
|
e = 0.081819190842622
|
||||||
|
|
||||||
# Convert latitude and longitude to radians
|
|
||||||
lat_rad = math.radians(lat)
|
lat_rad = math.radians(lat)
|
||||||
lon_rad = math.radians(lon)
|
lon_rad = math.radians(lon)
|
||||||
|
|
||||||
# Calculate the auxiliary values
|
|
||||||
cos_lat = math.cos(lat_rad)
|
cos_lat = math.cos(lat_rad)
|
||||||
sin_lat = math.sin(lat_rad)
|
sin_lat = math.sin(lat_rad)
|
||||||
N = a / math.sqrt(1 - f * (2 - f) * sin_lat**2)
|
N = a / math.sqrt(1 - f * (2 - f) * sin_lat**2)
|
||||||
|
|
||||||
# Calculate ECEF coordinates
|
# calculate ECEF coordinates
|
||||||
ecef_x = (N + alt) * cos_lat * math.cos(lon_rad)
|
ecef_x = (N + alt) * cos_lat * math.cos(lon_rad)
|
||||||
ecef_y = (N + alt) * cos_lat * math.sin(lon_rad)
|
ecef_y = (N + alt) * cos_lat * math.sin(lon_rad)
|
||||||
ecef_z = ((1-(e**2)) * N + alt) * sin_lat
|
ecef_z = ((1-(e**2)) * N + alt) * sin_lat
|
||||||
|
@ -43,11 +51,21 @@ class Geometry:
|
||||||
return ecef_x, ecef_y, ecef_z
|
return ecef_x, ecef_y, ecef_z
|
||||||
|
|
||||||
def ecef2lla(x, y, z):
|
def ecef2lla(x, y, z):
|
||||||
|
|
||||||
|
"""
|
||||||
|
@brief Converts ECEF coordinates to geodetic coordinates (latitude, longitude, altitude).
|
||||||
|
@param x (float): ECEF x-coordinate in meters.
|
||||||
|
@param y (float): ECEF y-coordinate in meters.
|
||||||
|
@param z (float): ECEF z-coordinate in meters.
|
||||||
|
@return lat (float): Geodetic latitude in degrees.
|
||||||
|
@return lon (float): Geodetic longitude in degrees.
|
||||||
|
@return alt (float): Altitude above the ellipsoid in meters.
|
||||||
|
"""
|
||||||
|
|
||||||
# WGS84 ellipsoid constants:
|
# WGS84 ellipsoid constants:
|
||||||
a = 6378137
|
a = 6378137
|
||||||
e = 8.1819190842622e-2
|
e = 8.1819190842622e-2
|
||||||
|
|
||||||
# Calculations:
|
|
||||||
b = math.sqrt(a**2 * (1 - e**2))
|
b = math.sqrt(a**2 * (1 - e**2))
|
||||||
ep = math.sqrt((a**2 - b**2) / b**2)
|
ep = math.sqrt((a**2 - b**2) / b**2)
|
||||||
p = math.sqrt(x**2 + y**2)
|
p = math.sqrt(x**2 + y**2)
|
||||||
|
@ -57,12 +75,10 @@ class Geometry:
|
||||||
N = a / math.sqrt(1 - e**2 * math.sin(lat)**2)
|
N = a / math.sqrt(1 - e**2 * math.sin(lat)**2)
|
||||||
alt = p / math.cos(lat) - N
|
alt = p / math.cos(lat) - N
|
||||||
|
|
||||||
# Return lon in range [0, 2*pi)
|
# return lon in range [0, 2*pi)
|
||||||
lon = lon % (2 * math.pi)
|
lon = lon % (2 * math.pi)
|
||||||
|
|
||||||
# Correct for numerical instability in altitude near exact poles:
|
# correct for numerical instability in altitude near exact poles:
|
||||||
# (after this correction, error is about 2 millimeters, which is about
|
|
||||||
# the same as the numerical precision of the overall function)
|
|
||||||
k = abs(x) < 1e-10 and abs(y) < 1e-10
|
k = abs(x) < 1e-10 and abs(y) < 1e-10
|
||||||
alt = abs(z) - b if k else alt
|
alt = abs(z) - b if k else alt
|
||||||
|
|
||||||
|
@ -72,58 +88,37 @@ class Geometry:
|
||||||
return lat, lon, alt
|
return lat, lon, alt
|
||||||
|
|
||||||
def enu2ecef(e1, n1, u1, lat, lon, alt):
|
def enu2ecef(e1, n1, u1, lat, lon, alt):
|
||||||
|
|
||||||
"""
|
"""
|
||||||
ENU to ECEF
|
@brief Converts East-North-Up (ENU) coordinates to ECEF coordinates.
|
||||||
|
@param e1 (float): Target east ENU coordinate in meters.
|
||||||
Parameters
|
@param n1 (float): Target north ENU coordinate in meters.
|
||||||
----------
|
@param u1 (float): Target up ENU coordinate in meters.
|
||||||
|
@param lat (float): Observer geodetic latitude in degrees.
|
||||||
e1 : float
|
@param lon (float): Observer geodetic longitude in degrees.
|
||||||
target east ENU coordinate (meters)
|
@param alt (float): Observer geodetic altitude in meters.
|
||||||
n1 : float
|
@return x (float): Target x ECEF coordinate in meters.
|
||||||
target north ENU coordinate (meters)
|
@return y (float): Target y ECEF coordinate in meters.
|
||||||
u1 : float
|
@return z (float): Target z ECEF coordinate in meters.
|
||||||
target up ENU coordinate (meters)
|
|
||||||
lat0 : float
|
|
||||||
Observer geodetic latitude
|
|
||||||
lon0 : float
|
|
||||||
Observer geodetic longitude
|
|
||||||
h0 : float
|
|
||||||
observer altitude above geodetic ellipsoid (meters)
|
|
||||||
|
|
||||||
|
|
||||||
Results
|
|
||||||
-------
|
|
||||||
x : float
|
|
||||||
target x ECEF coordinate (meters)
|
|
||||||
y : float
|
|
||||||
target y ECEF coordinate (meters)
|
|
||||||
z : float
|
|
||||||
target z ECEF coordinate (meters)
|
|
||||||
"""
|
"""
|
||||||
|
|
||||||
x0, y0, z0 = Geometry.lla2ecef(lat, lon, alt)
|
x0, y0, z0 = Geometry.lla2ecef(lat, lon, alt)
|
||||||
dx, dy, dz = Geometry.enu2uvw(e1, n1, u1, lat, lon)
|
dx, dy, dz = Geometry.enu2uvw(e1, n1, u1, lat, lon)
|
||||||
|
|
||||||
return x0 + dx, y0 + dy, z0 + dz
|
return x0 + dx, y0 + dy, z0 + dz
|
||||||
|
|
||||||
def enu2uvw(east, north, up, lat, lon):
|
def enu2uvw(east, north, up, lat, lon):
|
||||||
|
|
||||||
"""
|
"""
|
||||||
Parameters
|
@brief Converts East-North-Up (ENU) coordinates to UVW coordinates.
|
||||||
----------
|
@param east (float): Target east ENU coordinate in meters.
|
||||||
|
@param north (float): Target north ENU coordinate in meters.
|
||||||
e1 : float
|
@param up (float): Target up ENU coordinate in meters.
|
||||||
target east ENU coordinate (meters)
|
@param lat (float): Observer geodetic latitude in degrees.
|
||||||
n1 : float
|
@param lon (float): Observer geodetic longitude in degrees.
|
||||||
target north ENU coordinate (meters)
|
@return u (float): Target u coordinate in meters.
|
||||||
u1 : float
|
@return v (float): Target v coordinate in meters.
|
||||||
target up ENU coordinate (meters)
|
@return w (float): Target w coordinate in meters.
|
||||||
|
|
||||||
Results
|
|
||||||
-------
|
|
||||||
|
|
||||||
u : float
|
|
||||||
v : float
|
|
||||||
w : float
|
|
||||||
"""
|
"""
|
||||||
|
|
||||||
lat = math.radians(lat)
|
lat = math.radians(lat)
|
||||||
|
@ -140,16 +135,16 @@ class Geometry:
|
||||||
def ecef2enu(x, y, z, lat, lon, alt):
|
def ecef2enu(x, y, z, lat, lon, alt):
|
||||||
|
|
||||||
"""
|
"""
|
||||||
@brief From observer to target, ECEF => ENU.
|
@brief Converts ECEF coordinates to East-North-Up (ENU) coordinates.
|
||||||
@param x (float): Target x ECEF coordinate (m).
|
@param x (float): Target x ECEF coordinate in meters.
|
||||||
@param y (float): Target y ECEF coordinate (m).
|
@param y (float): Target y ECEF coordinate in meters.
|
||||||
@param z (float): Target z ECEF coordinate (m).
|
@param z (float): Target z ECEF coordinate in meters.
|
||||||
@param lat (float): Observer geodetic latitude (deg).
|
@param lat (float): Observer geodetic latitude in degrees.
|
||||||
@param lon (float): Observer geodetic longitude (deg).
|
@param lon (float): Observer geodetic longitude in degrees.
|
||||||
@param alt (float): Observer geodetic altituder (m).
|
@param alt (float): Observer geodetic altitude in meters.
|
||||||
@return east (float): Target east ENU coordinate (m).
|
@return east (float): Target east ENU coordinate in meters.
|
||||||
@return north (float): Target north ENU coordinate (m).
|
@return north (float): Target north ENU coordinate in meters.
|
||||||
@return up (float): Target up ENU coordinate (m).
|
@return up (float): Target up ENU coordinate in meters.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
x0, y0, z0 = Geometry.lla2ecef(lat, lon, alt)
|
x0, y0, z0 = Geometry.lla2ecef(lat, lon, alt)
|
||||||
|
@ -158,15 +153,15 @@ class Geometry:
|
||||||
def uvw2enu(u, v, w, lat, lon):
|
def uvw2enu(u, v, w, lat, lon):
|
||||||
|
|
||||||
"""
|
"""
|
||||||
@brief
|
@brief Converts UVW coordinates to East-North-Up (ENU) coordinates.
|
||||||
@param u (float): Shifted ECEF coordinate (m).
|
@param u (float): Shifted ECEF coordinate in the u-direction (m).
|
||||||
@param v (float): Shifted ECEF coordinate (m).
|
@param v (float): Shifted ECEF coordinate in the v-direction (m).
|
||||||
@param w (float): Shifted ECEF coordinate (m).
|
@param w (float): Shifted ECEF coordinate in the w-direction (m).
|
||||||
@param lat (float): Observer geodetic latitude (deg).
|
@param lat (float): Observer geodetic latitude in degrees.
|
||||||
@param lon (float): Observer geodetic longitude (deg).
|
@param lon (float): Observer geodetic longitude in degrees.
|
||||||
@param e (float): Target east ENU coordinate (m).
|
@return e (float): Target east ENU coordinate in meters.
|
||||||
@param n (float): Target north ENU coordinate (m).
|
@return n (float): Target north ENU coordinate in meters.
|
||||||
@param u (float): Target up ENU coordinate (m).
|
@return u (float): Target up ENU coordinate in meters.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
lat = math.radians(lat)
|
lat = math.radians(lat)
|
||||||
|
@ -186,10 +181,24 @@ class Geometry:
|
||||||
|
|
||||||
def distance_ecef(point1, point2):
|
def distance_ecef(point1, point2):
|
||||||
|
|
||||||
|
"""
|
||||||
|
@brief Computes the Euclidean distance between two points in ECEF coordinates.
|
||||||
|
@param point1 (tuple): Coordinates of the first point (x, y, z) in meters.
|
||||||
|
@param point2 (tuple): Coordinates of the second point (x, y, z) in meters.
|
||||||
|
@return distance (float): Euclidean distance between the two points in meters.
|
||||||
|
"""
|
||||||
|
|
||||||
return math.sqrt(
|
return math.sqrt(
|
||||||
(point2[0]-point1[0])**2 +
|
(point2[0]-point1[0])**2 +
|
||||||
(point2[1]-point1[1])**2 +
|
(point2[1]-point1[1])**2 +
|
||||||
(point2[2]-point1[2])**2)
|
(point2[2]-point1[2])**2)
|
||||||
|
|
||||||
def average_points(points):
|
def average_points(points):
|
||||||
|
|
||||||
|
"""
|
||||||
|
@brief Computes the average point from a list of points.
|
||||||
|
@param points (list): List of points, where each point is a tuple of coordinates (x, y, z) in meters.
|
||||||
|
@return average_point (list): Coordinates of the average point (x_avg, y_avg, z_avg) in meters.
|
||||||
|
"""
|
||||||
|
|
||||||
return [sum(coord) / len(coord) for coord in zip(*points)]
|
return [sum(coord) / len(coord) for coord in zip(*points)]
|
|
@ -192,4 +192,3 @@ class EllipseParametric:
|
||||||
output.append([x, y, z])
|
output.append([x, y, z])
|
||||||
|
|
||||||
return output
|
return output
|
||||||
|
|
|
@ -46,15 +46,12 @@ class AdsbTruth:
|
||||||
|
|
||||||
# store relevant data
|
# store relevant data
|
||||||
if adsb:
|
if adsb:
|
||||||
|
|
||||||
# loop over aircraft
|
# loop over aircraft
|
||||||
for aircraft in adsb["aircraft"]:
|
for aircraft in adsb["aircraft"]:
|
||||||
|
|
||||||
if aircraft.get("seen_pos") and \
|
if aircraft.get("seen_pos") and \
|
||||||
aircraft.get("alt_geom") and \
|
aircraft.get("alt_geom") and \
|
||||||
aircraft.get("flight") and \
|
aircraft.get("flight") and \
|
||||||
aircraft.get("seen_pos") < self.seen_pos_limit:
|
aircraft.get("seen_pos") < self.seen_pos_limit:
|
||||||
|
|
||||||
output[aircraft["hex"]] = {}
|
output[aircraft["hex"]] = {}
|
||||||
output[aircraft["hex"]]["lat"] = aircraft["lat"]
|
output[aircraft["hex"]]["lat"] = aircraft["lat"]
|
||||||
output[aircraft["hex"]]["lon"] = aircraft["lon"]
|
output[aircraft["hex"]]["lon"] = aircraft["lon"]
|
||||||
|
@ -62,5 +59,4 @@ class AdsbTruth:
|
||||||
output[aircraft["hex"]]["flight"] = aircraft["flight"]
|
output[aircraft["hex"]]["flight"] = aircraft["flight"]
|
||||||
output[aircraft["hex"]]["timestamp"] = \
|
output[aircraft["hex"]]["timestamp"] = \
|
||||||
adsb["now"] - aircraft["seen_pos"]
|
adsb["now"] - aircraft["seen_pos"]
|
||||||
|
|
||||||
return output
|
return output
|
||||||
|
|
|
@ -4,6 +4,7 @@
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import math
|
import math
|
||||||
|
|
||||||
from algorithm.geometry.Geometry import Geometry
|
from algorithm.geometry.Geometry import Geometry
|
||||||
|
|
||||||
class Ellipsoid:
|
class Ellipsoid:
|
||||||
|
|
|
@ -2,52 +2,49 @@ import argparse
|
||||||
import json
|
import json
|
||||||
import sys
|
import sys
|
||||||
from datetime import datetime
|
from datetime import datetime
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import matplotlib.pyplot as plt
|
import matplotlib.pyplot as plt
|
||||||
|
|
||||||
from geometry.Geometry import Geometry
|
from geometry.Geometry import Geometry
|
||||||
|
|
||||||
def parse_posix_time(value):
|
def parse_posix_time(value):
|
||||||
|
|
||||||
try:
|
try:
|
||||||
return int(value)
|
return int(value)
|
||||||
except ValueError:
|
except ValueError:
|
||||||
raise argparse.ArgumentTypeError("Invalid POSIX time format")
|
raise argparse.ArgumentTypeError("Invalid POSIX time format")
|
||||||
|
|
||||||
def parse_command_line_arguments():
|
def parse_command_line_arguments():
|
||||||
parser = argparse.ArgumentParser(description="Process command line arguments.")
|
|
||||||
|
|
||||||
|
parser = argparse.ArgumentParser(description="Process command line arguments.")
|
||||||
parser.add_argument("json_file", type=str, help="Input JSON file path")
|
parser.add_argument("json_file", type=str, help="Input JSON file path")
|
||||||
parser.add_argument("target_name", type=str, help="Target name")
|
parser.add_argument("target_name", type=str, help="Target name")
|
||||||
parser.add_argument("--start_time", type=parse_posix_time, help="Optional start time in POSIX seconds")
|
parser.add_argument("--start_time", type=parse_posix_time, help="Optional start time in POSIX seconds")
|
||||||
parser.add_argument("--stop_time", type=parse_posix_time, help="Optional stop time in POSIX seconds")
|
parser.add_argument("--stop_time", type=parse_posix_time, help="Optional stop time in POSIX seconds")
|
||||||
|
|
||||||
return parser.parse_args()
|
return parser.parse_args()
|
||||||
|
|
||||||
def interpolate_positions(timestamp_vector, truth_timestamp, truth_position):
|
def interpolate_positions(timestamp_vector, truth_timestamp, truth_position):
|
||||||
# Convert lists to NumPy arrays for easier manipulation
|
|
||||||
|
# convert lists to NumPy arrays for easier manipulation
|
||||||
truth_timestamp = np.array(truth_timestamp)
|
truth_timestamp = np.array(truth_timestamp)
|
||||||
truth_position = np.array(truth_position)
|
truth_position = np.array(truth_position)
|
||||||
|
|
||||||
# Interpolate positions for the new timestamp vector
|
# interpolate positions for the new timestamp vector
|
||||||
interpolated_positions = np.zeros((len(timestamp_vector), truth_position.shape[1]))
|
interpolated_positions = np.zeros((len(timestamp_vector), truth_position.shape[1]))
|
||||||
|
|
||||||
for i in range(truth_position.shape[1]):
|
for i in range(truth_position.shape[1]):
|
||||||
interpolated_positions[:, i] = np.interp(timestamp_vector, truth_timestamp, truth_position[:, i])
|
interpolated_positions[:, i] = np.interp(timestamp_vector, truth_timestamp, truth_position[:, i])
|
||||||
|
|
||||||
return interpolated_positions
|
return interpolated_positions
|
||||||
|
|
||||||
def calculate_rmse(actual_values, predicted_values):
|
def calculate_rmse(actual_values, predicted_values):
|
||||||
# Convert lists to NumPy arrays for easy calculations
|
|
||||||
|
# convert to numpy arrays
|
||||||
actual_values = np.array(actual_values)
|
actual_values = np.array(actual_values)
|
||||||
predicted_values = np.array(predicted_values)
|
predicted_values = np.array(predicted_values)
|
||||||
|
|
||||||
# Calculate the squared differences
|
# rms error
|
||||||
squared_diff = (actual_values - predicted_values) ** 2
|
squared_diff = (actual_values - predicted_values) ** 2
|
||||||
|
|
||||||
# Calculate the mean squared error
|
|
||||||
mean_squared_error = np.mean(squared_diff)
|
mean_squared_error = np.mean(squared_diff)
|
||||||
|
|
||||||
# Calculate the root mean squared error
|
|
||||||
rmse = np.sqrt(mean_squared_error)
|
rmse = np.sqrt(mean_squared_error)
|
||||||
|
|
||||||
return rmse
|
return rmse
|
||||||
|
@ -72,8 +69,8 @@ def main():
|
||||||
print("Start Time:", start_time)
|
print("Start Time:", start_time)
|
||||||
print("Stop Time:", stop_time)
|
print("Stop Time:", stop_time)
|
||||||
|
|
||||||
# get LLA coords from first radar
|
# get LLA coords from first radar or Adelaide CBD
|
||||||
radar4_lla = [-34.91041, 138.68924, 210]
|
radar4_lla = [-34.9286, 138.5999, 50]
|
||||||
|
|
||||||
# extract data of interest
|
# extract data of interest
|
||||||
server = json_data[0][0]["server"]
|
server = json_data[0][0]["server"]
|
||||||
|
@ -83,25 +80,16 @@ def main():
|
||||||
truth_timestamp = []
|
truth_timestamp = []
|
||||||
truth_position = []
|
truth_position = []
|
||||||
for item in json_data:
|
for item in json_data:
|
||||||
|
|
||||||
for method in item:
|
for method in item:
|
||||||
|
|
||||||
if method["server"] != server:
|
if method["server"] != server:
|
||||||
continue
|
continue
|
||||||
|
|
||||||
if start_time and method["timestamp_event"]/1000 < start_time:
|
if start_time and method["timestamp_event"]/1000 < start_time:
|
||||||
continue
|
continue
|
||||||
|
|
||||||
if stop_time and method["timestamp_event"]/1000 > stop_time:
|
if stop_time and method["timestamp_event"]/1000 > stop_time:
|
||||||
continue
|
continue
|
||||||
|
|
||||||
# store target data
|
# store target data
|
||||||
method_localisation = method["localisation"]
|
method_localisation = method["localisation"]
|
||||||
|
|
||||||
# override skip a method
|
|
||||||
#if method_localisation == "spherical-intersection":
|
|
||||||
#continue
|
|
||||||
|
|
||||||
if method_localisation not in position:
|
if method_localisation not in position:
|
||||||
position[method_localisation] = {}
|
position[method_localisation] = {}
|
||||||
position[method_localisation]["timestamp"] = []
|
position[method_localisation]["timestamp"] = []
|
||||||
|
@ -133,6 +121,7 @@ def main():
|
||||||
method["truth"][args.target_name]["lon"],
|
method["truth"][args.target_name]["lon"],
|
||||||
method["truth"][args.target_name]["alt"]])
|
method["truth"][args.target_name]["alt"]])
|
||||||
|
|
||||||
|
# store event timestamp
|
||||||
timestamp.append(method["timestamp_event"])
|
timestamp.append(method["timestamp_event"])
|
||||||
|
|
||||||
# remove duplicates in truth data
|
# remove duplicates in truth data
|
||||||
|
@ -163,11 +152,6 @@ 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))
|
|
||||||
position2 = {}
|
|
||||||
position2["ellipse-parametric-mean"] = position["ellipse-parametric-mean"]
|
|
||||||
position2["ellipsoid-parametric-mean"] = position["ellipsoid-parametric-mean"]
|
|
||||||
position2["spherical-intersection"] = position["spherical-intersection"]
|
|
||||||
mark = ['x', 'o', 's']
|
mark = ['x', 'o', 's']
|
||||||
position_reord = ["ellipse-parametric-mean", "ellipsoid-parametric-mean", "spherical-intersection"]
|
position_reord = ["ellipse-parametric-mean", "ellipsoid-parametric-mean", "spherical-intersection"]
|
||||||
fig, axes = plt.subplots(3, 1, figsize=(5, 7), sharex=True)
|
fig, axes = plt.subplots(3, 1, figsize=(5, 7), sharex=True)
|
||||||
|
@ -176,14 +160,13 @@ def main():
|
||||||
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_reord:
|
for method in position_reord:
|
||||||
print(position[method])
|
|
||||||
if "detections_enu" not in position[method]:
|
if "detections_enu" not in position[method]:
|
||||||
continue
|
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, marker=mark[i], label=method)
|
plt.plot(position[method]["timestamp"],
|
||||||
|
yaxis_target, marker=mark[i], label=method)
|
||||||
plt.xlabel('Timestamp')
|
plt.xlabel('Timestamp')
|
||||||
if i == 0:
|
if i == 0:
|
||||||
plt.ylabel('ENU X (m)')
|
plt.ylabel('ENU X (m)')
|
||||||
|
@ -191,7 +174,6 @@ def main():
|
||||||
plt.ylabel('ENU Y (m)')
|
plt.ylabel('ENU Y (m)')
|
||||||
if i == 2:
|
if i == 2:
|
||||||
plt.ylabel('ENU Z (m)')
|
plt.ylabel('ENU Z (m)')
|
||||||
|
|
||||||
plt.subplot(3, 1, 1)
|
plt.subplot(3, 1, 1)
|
||||||
plt.legend(prop = {"size": 8})
|
plt.legend(prop = {"size": 8})
|
||||||
plt.tight_layout()
|
plt.tight_layout()
|
||||||
|
@ -205,17 +187,11 @@ def main():
|
||||||
continue
|
continue
|
||||||
table[method] = {}
|
table[method] = {}
|
||||||
for i in range(3):
|
for i in range(3):
|
||||||
|
|
||||||
yaxis_truth = np.array([pos[i] for pos in truth_position_resampled_enu])
|
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"]))
|
matching_indices = np.isin(np.array(timestamp), np.array(position[method]["timestamp"]))
|
||||||
yaxis_truth_target = yaxis_truth[matching_indices]
|
yaxis_truth_target = yaxis_truth[matching_indices]
|
||||||
|
|
||||||
yaxis_target = [pos[i] for pos in position[method]["detections_enu"]]
|
yaxis_target = [pos[i] for pos in position[method]["detections_enu"]]
|
||||||
table[method][str(i)] = calculate_rmse(yaxis_target, yaxis_truth_target)
|
table[method][str(i)] = calculate_rmse(yaxis_target, yaxis_truth_target)
|
||||||
#print('test')
|
|
||||||
#print(yaxis_target)
|
|
||||||
#print(yaxis_truth_target)
|
|
||||||
|
|
||||||
print(table)
|
print(table)
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
|
|
|
@ -2,25 +2,26 @@ import argparse
|
||||||
import json
|
import json
|
||||||
import sys
|
import sys
|
||||||
from datetime import datetime
|
from datetime import datetime
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import matplotlib.pyplot as plt
|
import matplotlib.pyplot as plt
|
||||||
|
|
||||||
from geometry.Geometry import Geometry
|
from geometry.Geometry import Geometry
|
||||||
|
|
||||||
def parse_posix_time(value):
|
def parse_posix_time(value):
|
||||||
|
|
||||||
try:
|
try:
|
||||||
return int(value)
|
return int(value)
|
||||||
except ValueError:
|
except ValueError:
|
||||||
raise argparse.ArgumentTypeError("Invalid POSIX time format")
|
raise argparse.ArgumentTypeError("Invalid POSIX time format")
|
||||||
|
|
||||||
def parse_command_line_arguments():
|
def parse_command_line_arguments():
|
||||||
parser = argparse.ArgumentParser(description="Process command line arguments.")
|
|
||||||
|
|
||||||
|
parser = argparse.ArgumentParser(description="Process command line arguments.")
|
||||||
parser.add_argument("json_file", type=str, help="Input JSON file path")
|
parser.add_argument("json_file", type=str, help="Input JSON file path")
|
||||||
parser.add_argument("target_name", type=str, help="Target name")
|
parser.add_argument("target_name", type=str, help="Target name")
|
||||||
parser.add_argument("--start_time", type=parse_posix_time, help="Optional start time in POSIX seconds")
|
parser.add_argument("--start_time", type=parse_posix_time, help="Optional start time in POSIX seconds")
|
||||||
parser.add_argument("--stop_time", type=parse_posix_time, help="Optional stop time in POSIX seconds")
|
parser.add_argument("--stop_time", type=parse_posix_time, help="Optional stop time in POSIX seconds")
|
||||||
|
|
||||||
return parser.parse_args()
|
return parser.parse_args()
|
||||||
|
|
||||||
def main():
|
def main():
|
||||||
|
@ -48,31 +49,22 @@ def main():
|
||||||
timestamp = []
|
timestamp = []
|
||||||
associated = {}
|
associated = {}
|
||||||
for item in json_data:
|
for item in json_data:
|
||||||
|
|
||||||
first_result = item[0]
|
first_result = item[0]
|
||||||
|
|
||||||
if first_result["server"] != server:
|
if first_result["server"] != server:
|
||||||
print('error')
|
print('error')
|
||||||
sys.exit(-1)
|
sys.exit(-1)
|
||||||
|
|
||||||
if start_time and first_result["timestamp_event"]/1000 < start_time:
|
if start_time and first_result["timestamp_event"]/1000 < start_time:
|
||||||
continue
|
continue
|
||||||
|
|
||||||
if stop_time and first_result["timestamp_event"]/1000 > stop_time:
|
if stop_time and first_result["timestamp_event"]/1000 > stop_time:
|
||||||
continue
|
continue
|
||||||
|
|
||||||
# store association data
|
# store association data
|
||||||
if "detections_associated" in first_result:
|
if "detections_associated" in first_result:
|
||||||
|
|
||||||
if args.target_name in first_result["detections_associated"]:
|
if args.target_name in first_result["detections_associated"]:
|
||||||
|
|
||||||
for radar in first_result["detections_associated"][args.target_name]:
|
for radar in first_result["detections_associated"][args.target_name]:
|
||||||
|
|
||||||
if radar['radar'] not in associated:
|
if radar['radar'] not in associated:
|
||||||
associated[radar['radar']] = []
|
associated[radar['radar']] = []
|
||||||
else:
|
else:
|
||||||
associated[radar['radar']].append(first_result["timestamp_event"])
|
associated[radar['radar']].append(first_result["timestamp_event"])
|
||||||
|
|
||||||
timestamp.append(first_result["timestamp_event"])
|
timestamp.append(first_result["timestamp_event"])
|
||||||
|
|
||||||
# data massaging
|
# data massaging
|
||||||
|
@ -89,15 +81,11 @@ def main():
|
||||||
timestamp = [value for value in timestamp if value >= start_time]
|
timestamp = [value for value in timestamp if value >= start_time]
|
||||||
timestamp = [value for value in timestamp if value <= stop_time]
|
timestamp = [value for value in timestamp if value <= stop_time]
|
||||||
|
|
||||||
print(associated)
|
|
||||||
|
|
||||||
data = []
|
data = []
|
||||||
for radar in radars:
|
for radar in radars:
|
||||||
result = [1 if value in associated[radar] else 0 for value in timestamp]
|
result = [1 if value in associated[radar] else 0 for value in timestamp]
|
||||||
data.append(result)
|
data.append(result)
|
||||||
|
|
||||||
print(data)
|
|
||||||
|
|
||||||
# plot x, y, z
|
# plot x, y, z
|
||||||
plt.figure(figsize=(8,4))
|
plt.figure(figsize=(8,4))
|
||||||
img = plt.imshow(data, aspect='auto', interpolation='none')
|
img = plt.imshow(data, aspect='auto', interpolation='none')
|
||||||
|
|
|
@ -4,28 +4,28 @@ from algorithm.geometry.Geometry import Geometry
|
||||||
class TestGeometry(unittest.TestCase):
|
class TestGeometry(unittest.TestCase):
|
||||||
|
|
||||||
def test_lla2ecef(self):
|
def test_lla2ecef(self):
|
||||||
# Test case 1
|
|
||||||
|
# test case 1
|
||||||
result = Geometry.lla2ecef(-34.9286, 138.5999, 50)
|
result = Geometry.lla2ecef(-34.9286, 138.5999, 50)
|
||||||
self.assertAlmostEqual(result[0], -3926830.77177051, places=3)
|
self.assertAlmostEqual(result[0], -3926830.77177051, places=3)
|
||||||
self.assertAlmostEqual(result[1], 3461979.19806774, places=3)
|
self.assertAlmostEqual(result[1], 3461979.19806774, places=3)
|
||||||
self.assertAlmostEqual(result[2], -3631404.11418915, places=3)
|
self.assertAlmostEqual(result[2], -3631404.11418915, places=3)
|
||||||
|
|
||||||
# Test case 2
|
# test case 2
|
||||||
result = Geometry.lla2ecef(0, 0, 0)
|
result = Geometry.lla2ecef(0, 0, 0)
|
||||||
self.assertAlmostEqual(result[0], 6378137.0, places=3)
|
self.assertAlmostEqual(result[0], 6378137.0, places=3)
|
||||||
self.assertAlmostEqual(result[1], 0, places=3)
|
self.assertAlmostEqual(result[1], 0, places=3)
|
||||||
self.assertAlmostEqual(result[2], 0, places=3)
|
self.assertAlmostEqual(result[2], 0, places=3)
|
||||||
|
|
||||||
# Add more test cases as needed
|
|
||||||
|
|
||||||
def test_ecef2lla(self):
|
def test_ecef2lla(self):
|
||||||
# Test case 1
|
|
||||||
|
# test case 1
|
||||||
result = Geometry.ecef2lla(-3926830.77177051, 3461979.19806774, -3631404.11418915)
|
result = Geometry.ecef2lla(-3926830.77177051, 3461979.19806774, -3631404.11418915)
|
||||||
self.assertAlmostEqual(result[0], -34.9286, places=4)
|
self.assertAlmostEqual(result[0], -34.9286, places=4)
|
||||||
self.assertAlmostEqual(result[1], 138.5999, places=4)
|
self.assertAlmostEqual(result[1], 138.5999, places=4)
|
||||||
self.assertAlmostEqual(result[2], 50, places=3)
|
self.assertAlmostEqual(result[2], 50, places=3)
|
||||||
|
|
||||||
# Test case 2
|
# test case 2
|
||||||
result = Geometry.ecef2lla(6378137.0, 0, 0)
|
result = Geometry.ecef2lla(6378137.0, 0, 0)
|
||||||
self.assertAlmostEqual(result[0], 0, places=4)
|
self.assertAlmostEqual(result[0], 0, places=4)
|
||||||
self.assertAlmostEqual(result[1], 0, places=4)
|
self.assertAlmostEqual(result[1], 0, places=4)
|
||||||
|
@ -33,16 +33,17 @@ class TestGeometry(unittest.TestCase):
|
||||||
|
|
||||||
def test_enu2ecef(self):
|
def test_enu2ecef(self):
|
||||||
|
|
||||||
|
# test case 1
|
||||||
result = Geometry.enu2ecef(0, 0, 0, -34.9286, 138.5999, 50)
|
result = Geometry.enu2ecef(0, 0, 0, -34.9286, 138.5999, 50)
|
||||||
self.assertAlmostEqual(result[0], -3926830.77177051, places=3)
|
self.assertAlmostEqual(result[0], -3926830.77177051, places=3)
|
||||||
self.assertAlmostEqual(result[1], 3461979.19806774, places=3)
|
self.assertAlmostEqual(result[1], 3461979.19806774, places=3)
|
||||||
self.assertAlmostEqual(result[2], -3631404.11418915, places=3)
|
self.assertAlmostEqual(result[2], -3631404.11418915, places=3)
|
||||||
|
|
||||||
|
# test case 2
|
||||||
result = Geometry.enu2ecef(-1000, 2000, 3000, -34.9286, 138.5999, 50)
|
result = Geometry.enu2ecef(-1000, 2000, 3000, -34.9286, 138.5999, 50)
|
||||||
self.assertAlmostEqual(result[0], -3928873.3865007, places=3)
|
self.assertAlmostEqual(result[0], -3928873.3865007, places=3)
|
||||||
self.assertAlmostEqual(result[1], 3465113.14948365, places=3)
|
self.assertAlmostEqual(result[1], 3465113.14948365, places=3)
|
||||||
self.assertAlmostEqual(result[2], -3631482.0474089, places=3)
|
self.assertAlmostEqual(result[2], -3631482.0474089, places=3)
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
unittest.main()
|
unittest.main()
|
||||||
|
|
Loading…
Reference in a new issue