134 lines
4.0 KiB
Python
134 lines
4.0 KiB
Python
#!/usr/bin/env python3
|
|
"""
|
|
Test ECEF to LLA conversion with your reported values.
|
|
"""
|
|
import math
|
|
|
|
|
|
def ecef_to_lla(x: float, y: float, z: float) -> tuple[float, float, float]:
|
|
"""Convert ECEF to latitude, longitude, altitude (WGS84)."""
|
|
# WGS84 constants
|
|
a = 6378137.0 # Semi-major axis (meters)
|
|
e2 = 6.69437999014e-3 # First eccentricity squared
|
|
|
|
# Calculate longitude
|
|
lon_rad = math.atan2(y, x)
|
|
lon = math.degrees(lon_rad)
|
|
|
|
# Calculate latitude (iterative)
|
|
p = math.sqrt(x * x + y * y)
|
|
lat_rad = math.atan2(z, p * (1 - e2))
|
|
|
|
# Iterate to refine latitude and altitude
|
|
for _ in range(10):
|
|
N = a / math.sqrt(1 - e2 * math.sin(lat_rad) ** 2)
|
|
alt = p / math.cos(lat_rad) - N if abs(math.cos(lat_rad)) > 1e-10 else 0
|
|
lat_new = math.atan2(z, p * (1 - e2 * N / (N + alt)))
|
|
if abs(lat_new - lat_rad) < 1e-12:
|
|
break
|
|
lat_rad = lat_new
|
|
|
|
# Final altitude calculation
|
|
N = a / math.sqrt(1 - e2 * math.sin(lat_rad) ** 2)
|
|
if abs(math.cos(lat_rad)) > 1e-10:
|
|
alt = p / math.cos(lat_rad) - N
|
|
else:
|
|
alt = abs(z) / math.sin(lat_rad) - N * (1 - e2)
|
|
|
|
lat = math.degrees(lat_rad)
|
|
|
|
return lat, lon, alt
|
|
|
|
|
|
def haversine_distance(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
|
|
"""Calculate distance between two points using Haversine formula."""
|
|
EARTH_RADIUS_M = 6371008.8
|
|
lat1_rad = math.radians(lat1)
|
|
lat2_rad = math.radians(lat2)
|
|
delta_lat = math.radians(lat2 - lat1)
|
|
delta_lon = math.radians(lon2 - lon1)
|
|
a = math.sin(delta_lat / 2) ** 2 + math.cos(lat1_rad) * math.cos(lat2_rad) * math.sin(delta_lon / 2) ** 2
|
|
return 2 * EARTH_RADIUS_M * math.asin(min(1.0, math.sqrt(a)))
|
|
|
|
|
|
# Your reported ECEF values
|
|
ecef_x = -1755798.4445
|
|
ecef_y = 11515742.5663
|
|
ecef_z = -3206834.7481
|
|
|
|
print("Testing ECEF to LLA Conversion")
|
|
print("=" * 80)
|
|
print(f"\nInput ECEF Coordinates:")
|
|
print(f" X: {ecef_x:14.4f} m")
|
|
print(f" Y: {ecef_y:14.4f} m")
|
|
print(f" Z: {ecef_z:14.4f} m")
|
|
|
|
# Check distance from Earth's center
|
|
distance_from_center = math.sqrt(ecef_x**2 + ecef_y**2 + ecef_z**2)
|
|
print(f"\nDistance from Earth center: {distance_from_center:,.2f} m ({distance_from_center/1000:.2f} km)")
|
|
print(f"Expected: ~6,371 km (Earth's radius)")
|
|
|
|
if 6300000 < distance_from_center < 6500000:
|
|
print("✓ ECEF magnitude looks correct!")
|
|
else:
|
|
print("✗ WARNING: ECEF magnitude seems wrong!")
|
|
|
|
# Convert to LLA
|
|
lat, lon, alt = ecef_to_lla(ecef_x, ecef_y, ecef_z)
|
|
|
|
print(f"\nConverted Geodetic Coordinates (WGS84):")
|
|
print(f" Latitude: {lat:12.7f}°")
|
|
print(f" Longitude: {lon:12.7f}°")
|
|
print(f" Altitude: {alt:12.2f} m")
|
|
|
|
# Check if coordinates make sense
|
|
if -90 <= lat <= 90 and -180 <= lon <= 180:
|
|
print("\n✓ Lat/Lon within valid range")
|
|
else:
|
|
print("\n✗ Lat/Lon OUT OF RANGE!")
|
|
|
|
if -500 < alt < 10000:
|
|
print(f"✓ Altitude seems reasonable ({alt:.2f} m)")
|
|
else:
|
|
print(f"✗ WARNING: Altitude seems unusual ({alt:.2f} m)")
|
|
|
|
# Your rover position
|
|
rover_lat = 36.1140884
|
|
rover_lon = -97.0880663
|
|
|
|
print(f"\nYour Rover Position:")
|
|
print(f" Latitude: {rover_lat:12.7f}°")
|
|
print(f" Longitude: {rover_lon:12.7f}°")
|
|
|
|
# Calculate baseline
|
|
baseline = haversine_distance(rover_lat, rover_lon, lat, lon)
|
|
|
|
print(f"\nBaseline Distance:")
|
|
print(f" {baseline:,.2f} m")
|
|
print(f" {baseline/1000:,.3f} km")
|
|
|
|
if baseline < 200000: # Less than 200 km
|
|
print(f"✓ Baseline seems reasonable for RTK service")
|
|
else:
|
|
print(f"✗ WARNING: Baseline very large ({baseline/1000:.1f} km)")
|
|
|
|
# Show location description
|
|
print(f"\nBase Station Location:")
|
|
if lat > 0:
|
|
print(f" {abs(lat):.4f}° North", end="")
|
|
else:
|
|
print(f" {abs(lat):.4f}° South", end="")
|
|
|
|
if lon > 0:
|
|
print(f", {abs(lon):.4f}° East")
|
|
else:
|
|
print(f", {abs(lon):.4f}° West")
|
|
|
|
# Rough location check
|
|
if 24 < lat < 49 and -125 < lon < -66:
|
|
print(" 📍 Location: Continental United States")
|
|
elif 0 < lat < 90 and 60 < lon < 180:
|
|
print(" 📍 Location: Asia (possibly India/Southeast Asia)")
|
|
else:
|
|
print(" 📍 Location: Other")
|