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

I think I finally got enu2ecef working

Browse source
This commit is contained in:
30hours 2024-02-29 15:28:03 +00:00
parent 6d9027f1be
commit f3a621608c
7 changed files with 144 additions and 26 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
api/Dockerfile
View file

@ -13,4 +13,4 @@ EXPOSE 5000
ENV FLASK_APP=api.py
# run Gunicorn instead of the default Flask development server
CMD ["gunicorn", "--bind", "0.0.0.0:5000", "main:app"]
CMD ["gunicorn", "--bind", "0.0.0.0:5000", "--timeout", "60", "main:app"]

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

@ -16,8 +16,19 @@ function event_radar() {
const target = data["detections_localised"][key];
const points = target["points"];
removeEntitiesByType("test");
for (const point in points) {
console.log(points[point]);
addPoint(
points[point][0],
points[point][1],
points[point][2],
"test",
style_point.color,
style_point.pointSize,
style_point.type,
Date.now()
);
}
}
@ -32,4 +43,10 @@ function event_radar() {
setTimeout(event_radar, 1000);
});
}
}
var style_point = {};
style_point.color = 'rgba(128, 0, 0, 1.0)';
style_point.pointSize = 10;
style_point.type = "test";
style_point.timestamp = Date.now();

50
event/algorithm/coordreg/EllipsoidParametric.py
View file

@ -94,38 +94,50 @@ class EllipsoidParametric:
"""
# rotation matrix
phi = math.pi/2 - ellipsoid.pitch
phi = ellipsoid.pitch
theta = ellipsoid.yaw
phi = 0
theta = 0
R = np.array([
[np.cos(phi)*np.cos(theta), -np.sin(phi)*np.cos(theta), np.sin(theta)],
[np.sin(phi), np.cos(phi), 0],
[-np.cos(phi)*np.sin(theta), np.sin(phi)*np.sin(theta), np.cos(theta)]
])
# rotation matrix normal
# theta = ellipsoid.pitch_plane
# phi = ellipsoid.yaw_plane
# R2 = np.array([
# [np.cos(phi)*np.cos(theta), -np.sin(phi)*np.cos(theta), np.sin(theta)],
# [np.sin(phi), np.cos(phi), 0],
# [-np.cos(phi)*np.sin(theta), np.sin(phi)*np.sin(theta), np.cos(theta)]
# ])
# compute samples vectorised
a = (bistatic_range-ellipsoid.distance)/2
b = np.sqrt(a**2 - (ellipsoid.distance/2))
a = (bistatic_range+ellipsoid.distance)/2
b = np.sqrt(a**2 - (ellipsoid.distance/2)**2)
u_values = np.linspace(0, 2 * np.pi, n)
v_values = np.linspace(-np.pi/2, np.pi/2, n)
v_values = np.linspace(-np.pi, np.pi, n)
u, v = np.meshgrid(u_values, v_values, indexing='ij')
x = a * np.cos(u)
y = b * np.sin(u) * np.cos(v)
z = b * np.sin(u) * np.sin(v)
# x = a * np.cos(u)
# y = b * np.sin(u) * np.cos(v)
# z = b * np.sin(u) * np.sin(v)
x = a * np.cos(u) * np.cos(v)
y = b * np.sin(u)
z = a * np.cos(u) * np.sin(v)
r = np.stack([x, y, z], axis=-1).reshape(-1, 3)
r_1 = np.dot(r, R) + ellipsoid.midpoint
#r_1 = np.dot(r, np.dot(R, R2)) + ellipsoid.midpoint
#r_1 = np.dot(r, R) + ellipsoid.midpoint
#r_1 = np.dot(r, R)
r_1 = r
lat, lon, alt = Geometry.ecef2lla(ellipsoid.midpoint[0],
ellipsoid.midpoint[1], ellipsoid.midpoint[2])
print(lat, flush=True)
print(lon, flush=True)
print(alt, flush=True)
a, b, c = Geometry.ecef2lla(
ellipsoid.midpoint[0], ellipsoid.midpoint[1], ellipsoid.midpoint[2])
lat, lon, alt = Geometry.ecef2lla(ellipsoid.f1[0],
ellipsoid.f1[1], ellipsoid.f1[2])
print(ellipsoid.f1, flush=True)
print(lat, flush=True)
print(lon, flush=True)
print(alt, flush=True)
for i in range(len(r_1)):
x, y, z = Geometry.enu2ecef(r_1[i][0], r_1[i][1], r_1[i][2],
a, b, c)
r_1[i] = [x, y, z]
return r_1.tolist()

70
event/algorithm/geometry/Geometry.py
View file

@ -15,7 +15,7 @@ class Geometry:
def __init__(self, f1, f2, name):
"""
@brief Constructor for the Ellipsoid class.
@brief Constructor for the Geometry class.
"""
def lla2ecef(latitude, longitude, altitude):
@ -68,4 +68,70 @@ class Geometry:
lat = math.degrees(lat)
lon = math.degrees(lon)
return lat, lon, alt
return lat, lon, alt
def enu2ecef(e1, n1, u1, lat, lon, alt):
"""
ENU to ECEF
Parameters
----------
e1 : float
target east ENU coordinate (meters)
n1 : float
target north ENU coordinate (meters)
u1 : float
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)
dx, dy, dz = Geometry.enu2uvw(e1, n1, u1, lat, lon)
return x0 + dx, y0 + dy, z0 + dz
def enu2uvw(east, north, up, lat, lon):
"""
Parameters
----------
e1 : float
target east ENU coordinate (meters)
n1 : float
target north ENU coordinate (meters)
u1 : float
target up ENU coordinate (meters)
Results
-------
u : float
v : float
w : float
"""
lat = math.radians(lat)
lon = math.radians(lon)
t = math.cos(lat) * up - math.sin(lat) * north
w = math.sin(lat) * up + math.cos(lat) * north
u = math.cos(lon) * t - math.sin(lon) * east
v = math.sin(lon) * t + math.cos(lon) * east
return u, v, w

10
event/data/Ellipsoid.py
View file

@ -37,3 +37,13 @@ class Ellipsoid:
(f2[0] - f1[0])**2 +
(f2[1] - f1[1])**2 +
(f2[2] - f1[2])**2)
# rotate to normal plane on WGS-84
length = math.sqrt(
self.midpoint[0]**2 +
self.midpoint[1]**2 +
self.midpoint[2]**2
)
vector = [x / length for x in self.midpoint]
self.pitch_plane = math.asin(-vector[1])
self.yaw_plane = math.atan2(-vector[0], -vector[2])

4
event/event.py
View file

@ -123,11 +123,11 @@ async def event():
[x_rx, y_rx, z_rx],
'radar4.30hours.dev'
)
pointsEcef = ellipsoidParametric.sample(ellipsoid, 10000, 5)
pointsEcef = ellipsoidParametric.sample(ellipsoid, 25000, 15)
pointsLla = []
for point in pointsEcef:
lat, lon, alt = Geometry.ecef2lla(point[0], point[1], point[2])
pointsLla.append([lat, lon, alt])
pointsLla.append([round(lat, 4), round(lon, 4), round(alt)])
localised_dets["test"]["points"] = pointsLla
# output data to API

13
test/event/TestGeometry.py
View file

@ -31,5 +31,18 @@ class TestGeometry(unittest.TestCase):
self.assertAlmostEqual(result[1], 0, places=4)
self.assertAlmostEqual(result[2], 0, places=3)
def test_enu2ecef(self):
result = Geometry.enu2ecef(0, 0, 0, -34.9286, 138.5999, 50)
self.assertAlmostEqual(result[0], -3926830.77177051, places=3)
self.assertAlmostEqual(result[1], 3461979.19806774, places=3)
self.assertAlmostEqual(result[2], -3631404.11418915, places=3)
result = Geometry.enu2ecef(-1000, 2000, 3000, -34.9286, 138.5999, 50)
self.assertAlmostEqual(result[0], -3928873.3865007, places=3)
self.assertAlmostEqual(result[1], 3465113.14948365, places=3)
self.assertAlmostEqual(result[2], -3631482.0474089, places=3)
if __name__ == '__main__':
unittest.main()