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

Ellipsoid is now aligned properly

Browse source
This commit is contained in:
30hours 2024-03-01 03:49:37 +00:00
parent f3a621608c
commit 2c7c4358b7
4 changed files with 80 additions and 30 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

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

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

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

22
event/data/Ellipsoid.py
View file

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

2
event/event.py
View file

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