mirror of
https://github.com/30hours/3lips.git
synced 2024-11-08 12:25:42 +00:00
I think I finally got enu2ecef working
This commit is contained in:
parent
6d9027f1be
commit
f3a621608c
7 changed files with 144 additions and 26 deletions
|
@ -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"]
|
||||
|
|
|
@ -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();
|
|
@ -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()
|
|
@ -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
|
|
@ -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])
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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()
|
||||
|
|
Loading…
Reference in a new issue