test(e2e): add ECEF→WGS84 and Euler→quaternion helpers

Closed-form Heikkinen method for ECEF conversion (centimetre accuracy,
no iteration). ZYX aerospace-convention Euler → quaternion. Both needed
by upcoming VPAIRAdapter rewrite; reusable for other datasets shipping
ECEF or Euler poses (e.g. some MARS-LVIG releases).

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>

src/gps_denied/testing/coord.py

@@ -0,0 +1,76 @@
+"""Coordinate conversion helpers used by dataset adapters.
+
+- `ecef_to_wgs84`: closed-form Zhu/Heikkinen conversion (no iteration), accurate
+  to centimetres for any point above the Earth's surface.
+- `euler_to_quaternion`: aerospace ZYX intrinsic convention (yaw → pitch → roll
+  applied to body frame in that order). Inputs in radians.
+"""
+
+from __future__ import annotations
+
+import math
+
+
+# WGS84 constants
+_A = 6_378_137.0                 # semi-major axis, metres
+_F = 1.0 / 298.257223563         # flattening
+_B = _A * (1.0 - _F)             # semi-minor axis
+_E2 = 1.0 - (_B * _B) / (_A * _A)   # first eccentricity squared
+_EP2 = (_A * _A) / (_B * _B) - 1.0  # second eccentricity squared
+
+
+def ecef_to_wgs84(x: float, y: float, z: float) -> tuple[float, float, float]:
+    """ECEF (metres) → WGS84 (lat° N, lon° E, altitude metres above ellipsoid).
+
+    Uses Heikkinen's closed-form method — no iteration, one pass, centimetre
+    accuracy for any realistic aerial vehicle altitude.
+    """
+    r = math.sqrt(x * x + y * y)
+    if r == 0.0:
+        # Pole: lon is conventionally 0, lat is ±90 depending on sign of z
+        lat = 90.0 if z >= 0 else -90.0
+        alt = abs(z) - _B
+        return lat, 0.0, alt
+
+    ee = _A * _A - _B * _B
+    f = 54.0 * _B * _B * z * z
+    g = r * r + (1.0 - _E2) * z * z - _E2 * ee
+    c = (_E2 * _E2 * f * r * r) / (g * g * g)
+    s = (1.0 + c + math.sqrt(c * c + 2.0 * c)) ** (1.0 / 3.0)
+    p = f / (3.0 * (s + 1.0 / s + 1.0) ** 2 * g * g)
+    q = math.sqrt(1.0 + 2.0 * _E2 * _E2 * p)
+    r0 = -(p * _E2 * r) / (1.0 + q) + math.sqrt(
+        0.5 * _A * _A * (1.0 + 1.0 / q)
+        - (p * (1.0 - _E2) * z * z) / (q * (1.0 + q))
+        - 0.5 * p * r * r
+    )
+    u = math.sqrt((r - _E2 * r0) ** 2 + z * z)
+    v = math.sqrt((r - _E2 * r0) ** 2 + (1.0 - _E2) * z * z)
+    z0 = (_B * _B * z) / (_A * v)
+
+    lat = math.degrees(math.atan((z + _EP2 * z0) / r))
+    lon = math.degrees(math.atan2(y, x))
+    alt = u * (1.0 - (_B * _B) / (_A * v))
+    return lat, lon, alt
+
+
+def euler_to_quaternion(
+    roll: float, pitch: float, yaw: float
+) -> tuple[float, float, float, float]:
+    """Aerospace ZYX intrinsic Euler → (qx, qy, qz, qw).
+
+    Rotation order: yaw (about world z) → pitch (about intermediate y) → roll
+    (about body x). All inputs in radians. Output quaternion has unit norm.
+    """
+    cr = math.cos(roll * 0.5)
+    sr = math.sin(roll * 0.5)
+    cp = math.cos(pitch * 0.5)
+    sp = math.sin(pitch * 0.5)
+    cy = math.cos(yaw * 0.5)
+    sy = math.sin(yaw * 0.5)
+
+    qw = cr * cp * cy + sr * sp * sy
+    qx = sr * cp * cy - cr * sp * sy
+    qy = cr * sp * cy + sr * cp * sy
+    qz = cr * cp * sy - sr * sp * cy
+    return qx, qy, qz, qw