def ecef2lla(self, ecef, tolerance=1e-9):
"""
Convert Earth-centered, Earth-fixed coordinates to lat, lon, alt.
Input: ecef - (x, y, z) in (m, m, m)
Output: lla - (lat, lon, alt) in (decimal degrees, decimal degrees, m)
"""
# Decompose the input
x = ecef[0]
y = ecef[1]
z = ecef[2]
# Calculate lon
lon = atan2(y, x)
# Initialize the variables to calculate lat and alt
alt = 0
N = self.a
p = sqrt(x**2 + y**2)
lat = 0
previousLat = 90
# Iterate until tolerance is reached
while abs(lat - previousLat) >= tolerance:
previousLat = lat
sinLat = z / (N * (1 - self.e**2) + alt)
lat = atan((z + self.e**2 * N * sinLat) / p)
N = self.a / sqrt(1 - (self.e * sinLat)**2)
alt = p / cos(lat) - N
# Return the lla coordinates
return (geo.rad2deg(lat), geo.rad2deg(lon), alt)
def ecef2lla(self, ecef, tolerance=1e-9):
"""
Convert Earth-centered, Earth-fixed coordinates to lat, lon, alt.
Input: ecef - (x, y, z) in (m, m, m)
Output: lla - (lat, lon, alt) in (decimal degrees, decimal degrees, m)
"""
# Decompose the input
x = ecef[0]
y = ecef[1]
z = ecef[2]
# Calculate lon
lon = atan2(y, x)
# Initialize the variables to calculate lat and alt
alt = 0
N = self.a
p = sqrt(x**2 + y**2)
lat = 0
previousLat = 90
# Iterate until tolerance is reached
while abs(lat - previousLat) >= tolerance:
previousLat = lat
sinLat = z / (N * (1 - self.e**2) + alt)
lat = atan((z + self.e**2 * N * sinLat) / p)
N = self.a / sqrt(1 - (self.e * sinLat)**2)
alt = p / cos(lat) - N
# Return the lla coordinates
return (geo.rad2deg(lat), geo.rad2deg(lon), alt)
def ned2pae(self, ned):
"""
Converts the local north, east, down coordinates into range, azimuth,
and elevation angles
Input: ned - (north, east, down) in (m, m, m)
Output: pae - (p, alpha, epsilon) in (m, degrees, degrees)
"""
p = geo.euclideanDistance(ned)
alpha = atan2(ned[1], ned[0])
epsilon = atan2(-ned[2], sqrt(ned[0]**2 + ned[1]**2))
return [p, geo.rad2deg(alpha), geo.rad2deg(epsilon)]
def ned2pae(self, ned):
"""
Converts the local north, east, down coordinates into range, azimuth,
and elevation angles
Input: ned - (north, east, down) in (m, m, m)
Output: pae - (p, alpha, epsilon) in (m, degrees, degrees)
"""
p = geo.euclideanDistance(ned)
alpha = atan2(ned[1], ned[0])
epsilon = atan2(-ned[2], sqrt(ned[0]**2 + ned[1]**2))
return [p, geo.rad2deg(alpha), geo.rad2deg(epsilon)]
