mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-23 06:46:38 +00:00
Yuzviak
8a577d4295
Previous commit 56d2e98 asserted lat=48.1351/lon=11.5820/alt=520 for
ECEF (4177789.3, 855098.1, 4727807.9) — those numbers were a
copy-paste guess from an external converter, not consistent with the
stated ECEF input. Both Heikkinen closed-form and Bowring iterative
independently give lat≈48.1414°, lon≈11.5674°, alt≈570.75 m from that
input. Implementation was correct; test data was wrong.
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
81 lines
3.1 KiB
Python
81 lines
3.1 KiB
Python
"""Coordinate conversion helpers — ECEF↔WGS84 + Euler→quaternion."""
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from gps_denied.testing.coord import ecef_to_wgs84, euler_to_quaternion
|
|
|
|
|
|
# --- ECEF → WGS84 ---
|
|
|
|
def test_ecef_origin_is_on_equator_prime_meridian():
|
|
# Point on equator at lon=0, alt=0 is at x=6378137, y=0, z=0 (WGS84 semi-major)
|
|
lat, lon, alt = ecef_to_wgs84(6378137.0, 0.0, 0.0)
|
|
assert lat == pytest.approx(0.0, abs=1e-6)
|
|
assert lon == pytest.approx(0.0, abs=1e-6)
|
|
assert alt == pytest.approx(0.0, abs=1e-3)
|
|
|
|
|
|
def test_ecef_north_pole():
|
|
# Semi-minor axis b ≈ 6356752.314 — north pole, lat=90, lon undefined but typically 0
|
|
lat, lon, alt = ecef_to_wgs84(0.0, 0.0, 6356752.314245)
|
|
assert lat == pytest.approx(90.0, abs=1e-4)
|
|
assert alt == pytest.approx(0.0, abs=1e-2)
|
|
|
|
|
|
def test_ecef_known_point_near_munich():
|
|
# Input ECEF (4177789.3, 855098.1, 4727807.9) m — point near Munich, Germany.
|
|
# Reference geodetic (Heikkinen closed-form, cross-checked vs Bowring iterative):
|
|
# lat≈48.14141°, lon≈11.56740°, alt≈570.75 m.
|
|
lat, lon, alt = ecef_to_wgs84(4177789.3, 855098.1, 4727807.9)
|
|
assert lat == pytest.approx(48.14141, abs=1e-4)
|
|
assert lon == pytest.approx(11.56740, abs=1e-4)
|
|
assert alt == pytest.approx(570.75, abs=1.0)
|
|
|
|
|
|
def test_ecef_vpair_sample_point():
|
|
# From VPAIR sample poses_query.txt first row:
|
|
# 4023518.0, 510303.75, 4906569.65 — should be in Bonn/Eifel region, Germany
|
|
# (VPAIR was recorded near Bonn). Expected lat ~50.7°, lon ~7.2°, alt ~(200-400) m.
|
|
lat, lon, alt = ecef_to_wgs84(4023518.0, 510303.75, 4906569.65)
|
|
assert 50.0 < lat < 51.5, f"lat={lat}"
|
|
assert 6.5 < lon < 8.0, f"lon={lon}"
|
|
# Bonn-Eifel area ground elevation 100-500 m
|
|
assert 100.0 < alt < 700.0, f"alt={alt}"
|
|
|
|
|
|
# --- Euler → quaternion ---
|
|
|
|
def test_euler_zero_is_identity_quaternion():
|
|
qx, qy, qz, qw = euler_to_quaternion(0.0, 0.0, 0.0)
|
|
assert qx == pytest.approx(0.0, abs=1e-12)
|
|
assert qy == pytest.approx(0.0, abs=1e-12)
|
|
assert qz == pytest.approx(0.0, abs=1e-12)
|
|
assert qw == pytest.approx(1.0, abs=1e-12)
|
|
|
|
|
|
def test_euler_yaw_90_deg_about_z():
|
|
# Yaw = π/2 around world z, roll=pitch=0
|
|
# Expected quaternion: (0, 0, sin(π/4), cos(π/4)) ≈ (0, 0, 0.7071, 0.7071)
|
|
qx, qy, qz, qw = euler_to_quaternion(0.0, 0.0, np.pi / 2)
|
|
assert qx == pytest.approx(0.0, abs=1e-10)
|
|
assert qy == pytest.approx(0.0, abs=1e-10)
|
|
assert qz == pytest.approx(np.sin(np.pi / 4), abs=1e-10)
|
|
assert qw == pytest.approx(np.cos(np.pi / 4), abs=1e-10)
|
|
|
|
|
|
def test_euler_roll_90_deg_about_x():
|
|
# Roll = π/2 around body x, pitch=yaw=0
|
|
qx, qy, qz, qw = euler_to_quaternion(np.pi / 2, 0.0, 0.0)
|
|
assert qx == pytest.approx(np.sin(np.pi / 4), abs=1e-10)
|
|
assert qy == pytest.approx(0.0, abs=1e-10)
|
|
assert qz == pytest.approx(0.0, abs=1e-10)
|
|
assert qw == pytest.approx(np.cos(np.pi / 4), abs=1e-10)
|
|
|
|
|
|
def test_euler_unit_norm():
|
|
# Arbitrary angles — the returned quaternion must be unit norm
|
|
qx, qy, qz, qw = euler_to_quaternion(0.3, -0.7, 1.2)
|
|
norm = (qx * qx + qy * qy + qz * qz + qw * qw) ** 0.5
|
|
assert norm == pytest.approx(1.0, abs=1e-12)
|