mirror of
https://github.com/30hours/3lips.git
synced 2024-11-08 12:25:42 +00:00
Ellipsoid is now aligned properly
This commit is contained in:
parent
f3a621608c
commit
2c7c4358b7
4 changed files with 80 additions and 30 deletions
|
@ -94,14 +94,22 @@ class EllipsoidParametric:
|
|||
"""
|
||||
|
||||
# rotation matrix
|
||||
phi = np.pi/2 - ellipsoid.pitch
|
||||
theta = ellipsoid.yaw + np.pi/2
|
||||
phi = np.deg2rad(3.834)
|
||||
theta = -np.deg2rad(-77+90)
|
||||
|
||||
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)]
|
||||
# ])
|
||||
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)]
|
||||
[np.cos(theta), -np.sin(theta)*np.cos(phi), np.sin(theta)*np.sin(phi)],
|
||||
[np.sin(theta), np.cos(theta)*np.cos(phi), -np.cos(theta)*np.sin(phi)],
|
||||
[0, np.sin(phi), np.cos(phi)]
|
||||
])
|
||||
|
||||
# rotation matrix normal
|
||||
|
@ -129,8 +137,8 @@ class EllipsoidParametric:
|
|||
|
||||
#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
|
||||
r_1 = np.dot(r, R)
|
||||
#r_1 = r
|
||||
|
||||
a, b, c = Geometry.ecef2lla(
|
||||
ellipsoid.midpoint[0], ellipsoid.midpoint[1], ellipsoid.midpoint[2])
|
||||
|
|
|
@ -10,15 +10,16 @@ class Geometry:
|
|||
"""
|
||||
@class Geometry
|
||||
@brief A class to store geometric functions.
|
||||
@details Assumes WGS-84 ellipsoid for all functions.
|
||||
"""
|
||||
|
||||
def __init__(self, f1, f2, name):
|
||||
def __init__(self):
|
||||
|
||||
"""
|
||||
@brief Constructor for the Geometry class.
|
||||
"""
|
||||
|
||||
def lla2ecef(latitude, longitude, altitude):
|
||||
def lla2ecef(lat, lon, alt):
|
||||
|
||||
# WGS84 constants
|
||||
a = 6378137.0 # semi-major axis in meters
|
||||
|
@ -26,8 +27,8 @@ class Geometry:
|
|||
e = 0.081819190842622
|
||||
|
||||
# Convert latitude and longitude to radians
|
||||
lat_rad = math.radians(latitude)
|
||||
lon_rad = math.radians(longitude)
|
||||
lat_rad = math.radians(lat)
|
||||
lon_rad = math.radians(lon)
|
||||
|
||||
# Calculate the auxiliary values
|
||||
cos_lat = math.cos(lat_rad)
|
||||
|
@ -35,9 +36,9 @@ class Geometry:
|
|||
N = a / math.sqrt(1 - f * (2 - f) * sin_lat**2)
|
||||
|
||||
# Calculate ECEF coordinates
|
||||
ecef_x = (N + altitude) * cos_lat * math.cos(lon_rad)
|
||||
ecef_y = (N + altitude) * cos_lat * math.sin(lon_rad)
|
||||
ecef_z = ((1-(e**2)) * N + altitude) * sin_lat
|
||||
ecef_x = (N + alt) * cos_lat * math.cos(lon_rad)
|
||||
ecef_y = (N + alt) * cos_lat * math.sin(lon_rad)
|
||||
ecef_z = ((1-(e**2)) * N + alt) * sin_lat
|
||||
|
||||
return ecef_x, ecef_y, ecef_z
|
||||
|
||||
|
@ -135,3 +136,50 @@ class Geometry:
|
|||
v = math.sin(lon) * t + math.cos(lon) * east
|
||||
|
||||
return u, v, w
|
||||
|
||||
def ecef2enu(x, y, z, lat, lon, alt):
|
||||
|
||||
"""
|
||||
@brief From observer to target, ECEF => ENU.
|
||||
@param x (float): Target x ECEF coordinate (m).
|
||||
@param y (float): Target y ECEF coordinate (m).
|
||||
@param z (float): Target z ECEF coordinate (m).
|
||||
@param lat (float): Observer geodetic latitude (deg).
|
||||
@param lon (float): Observer geodetic longitude (deg).
|
||||
@param alt (float): Observer geodetic altituder (m).
|
||||
@return east (float): Target east ENU coordinate (m).
|
||||
@return north (float): Target north ENU coordinate (m).
|
||||
@return up (float): Target up ENU coordinate (m).
|
||||
"""
|
||||
|
||||
x0, y0, z0 = Geometry.lla2ecef(lat, lon, alt)
|
||||
return Geometry.uvw2enu(x - x0, y - y0, z - z0, lat, lon)
|
||||
|
||||
def uvw2enu(u, v, w, lat, lon):
|
||||
|
||||
"""
|
||||
@brief
|
||||
@param u (float): Shifted ECEF coordinate (m).
|
||||
@param v (float): Shifted ECEF coordinate (m).
|
||||
@param w (float): Shifted ECEF coordinate (m).
|
||||
@param lat (float): Observer geodetic latitude (deg).
|
||||
@param lon (float): Observer geodetic longitude (deg).
|
||||
@param e (float): Target east ENU coordinate (m).
|
||||
@param n (float): Target north ENU coordinate (m).
|
||||
@param u (float): Target up ENU coordinate (m).
|
||||
"""
|
||||
|
||||
lat = math.radians(lat)
|
||||
lon = math.radians(lon)
|
||||
|
||||
cos_lat = math.cos(lat)
|
||||
sin_lat = math.sin(lat)
|
||||
cos_lon = math.cos(lon)
|
||||
sin_lon = math.sin(lon)
|
||||
|
||||
t = cos_lon * u + sin_lon * v
|
||||
e = -sin_lon * u + cos_lon * v
|
||||
u = cos_lat * t + sin_lat * w
|
||||
n = -sin_lat * t + cos_lat * w
|
||||
|
||||
return e, n, u
|
|
@ -4,6 +4,7 @@
|
|||
"""
|
||||
|
||||
import math
|
||||
from algorithm.geometry.Geometry import Geometry
|
||||
|
||||
class Ellipsoid:
|
||||
|
||||
|
@ -29,21 +30,14 @@ class Ellipsoid:
|
|||
# dependent members
|
||||
self.midpoint = [(f1[0]+f2[0])/2,
|
||||
(f1[1]+f2[1])/2, (f1[2]+f2[2])/2]
|
||||
vector = (f2[0]-f1[0], f2[1]-f1[1], f2[2]-f1[2])
|
||||
self.yaw = math.atan2(vector[1], vector[0])
|
||||
self.pitch = math.atan2(vector[2],
|
||||
math.sqrt(vector[0]**2 + vector[1]**2))
|
||||
midpoint_lla = Geometry.ecef2lla(
|
||||
self.midpoint[0], self.midpoint[1], self.midpoint[2])
|
||||
vector_enu = Geometry.ecef2enu(f1[0], f1[1], f1[2],
|
||||
midpoint_lla[0], midpoint_lla[1], midpoint_lla[2])
|
||||
self.yaw = -math.atan2(vector_enu[1], vector_enu[0])-math.pi/2
|
||||
self.pitch = math.atan2(vector_enu[2],
|
||||
math.sqrt(vector_enu[0]**2 + vector_enu[1]**2))
|
||||
self.distance = math.sqrt(
|
||||
(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,7 +123,7 @@ async def event():
|
|||
[x_rx, y_rx, z_rx],
|
||||
'radar4.30hours.dev'
|
||||
)
|
||||
pointsEcef = ellipsoidParametric.sample(ellipsoid, 25000, 15)
|
||||
pointsEcef = ellipsoidParametric.sample(ellipsoid, 6000, 15)
|
||||
pointsLla = []
|
||||
for point in pointsEcef:
|
||||
lat, lon, alt = Geometry.ecef2lla(point[0], point[1], point[2])
|
||||
|
|
Loading…
Reference in a new issue