Initial commit

test_ecef_convert.py

@@ -0,0 +1,133 @@
+#!/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")