Ellipsoid is now aligned properly

event/algorithm/coordreg/EllipsoidParametric.py

@@ -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
+        # R = np.array([
-        theta = 0
+        #   [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.cos(theta), -np.sin(theta)*np.cos(phi), np.sin(theta)*np.sin(phi)],
-          [np.sin(phi), np.cos(phi), 0],
+          [np.sin(theta), np.cos(theta)*np.cos(phi), -np.cos(theta)*np.sin(phi)],
-          [-np.cos(phi)*np.sin(theta), np.sin(phi)*np.sin(theta), np.cos(theta)]
+          [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 = np.dot(r, R)
-        r_1 = r
+        #r_1 = r
 
         a, b, c = Geometry.ecef2lla(
           ellipsoid.midpoint[0], ellipsoid.midpoint[1], ellipsoid.midpoint[2])

event/algorithm/geometry/Geometry.py

@@ -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)
+        lat_rad = math.radians(lat)
-        lon_rad = math.radians(longitude)
+        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_x = (N + alt) * cos_lat * math.cos(lon_rad)
-        ecef_y = (N + altitude) * cos_lat * math.sin(lon_rad)
+        ecef_y = (N + alt) * cos_lat * math.sin(lon_rad)
-        ecef_z = ((1-(e**2)) * N + altitude) * sin_lat
+        ecef_z = ((1-(e**2)) * N + alt) * sin_lat
 
         return ecef_x, ecef_y, ecef_z
 
@@ -134,4 +135,51 @@ class Geometry:
         u = math.cos(lon) * t - math.sin(lon) * east
         v = math.sin(lon) * t + math.cos(lon) * east
 
-        return u, v, w
+        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

event/data/Ellipsoid.py

@@ -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])
+        midpoint_lla = Geometry.ecef2lla(
-        self.yaw = math.atan2(vector[1], vector[0])
+          self.midpoint[0], self.midpoint[1], self.midpoint[2])
-        self.pitch = math.atan2(vector[2], 
+        vector_enu = Geometry.ecef2enu(f1[0], f1[1], f1[2], 
-          math.sqrt(vector[0]**2 + vector[1]**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])

event/event.py

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