[AZ-918] [AZ-919] [AZ-920] [AZ-921] [AZ-922] VIO/ESKF baseline fixes

Derkachi e2e Tier-2 divergence had three stacked root causes; this
commit ships fixes for all three plus the IMU prerequisite they
depend on, plus a baseline cheirality gate for cv2.recoverPose.

AZ-918  MAVLink IMU adapters now convert raw mG/mrad-s + FRD body to
        SI m/s^2 + rad/s + FLU body via helpers.imu_units. Without
        this the ESKF receives values ~1000x too small with wrong-
        sign Y/Z and cannot function at all.

AZ-919  Composition root wires EskfNominalAltitudeProvider into the
        KLT/RANSAC strategy via the AZ-331 factory introspect path;
        OKVIS2 and VINS-Mono are unaffected.

AZ-920  KLT/RANSAC recovers metric translation via Ground Sampling
        Distance when AGL is available; otherwise falls through with
        scale_quality=direction_only/unknown (no fake scale invented).

AZ-921  VioOutput.scale_quality signal; ESKF add_vio adapts R_meas
        position block based on the flag (1e6 inflation when scale is
        direction_only/unknown to keep the filter consistent).

AZ-922  KLT/RANSAC cheirality gate rejects single-frame rotations
        beyond a config threshold (default 30 deg), catching
        cv2.recoverPose twisted-pair flips that cause immediate ESKF
        divergence on low-parallax aerial scenes.

Verification:
- Tier-1 (macOS) unit suite: 2346 passed, 0 failed.
- Tier-2 (Jetson) Derkachi e2e: divergence moves from frame 5
  (mahalanobis^2 3757) to frame 233 (mahalanobis^2 212). Remaining
  drift is open-loop attitude accumulation, not cheirality.

Follow-up tickets filed:
- AZ-923 closed as misdiagnosed: EskfNominalAltitudeProvider was
  already correct (nominal_pos.z IS the AGL when takeoff origin sits
  at ground level); the early-frame AGL near zero reflects the drone
  being stationary on the ground, not a provider bug.
- AZ-942 filed: cross-check VIO rotation against IMU preintegrator
  (consistency gate) - more physically grounded than the coarse
  AZ-922 threshold and likely required to absorb the frame-233 drift.

Co-authored-by: Cursor <cursoragent@cursor.com>

src/gps_denied_onboard/helpers/altitude_provider.py

@@ -0,0 +1,93 @@
+"""Above-Ground-Level (AGL) provider abstraction for C1 VIO scale recovery.
+
+The monocular KLT/RANSAC strategy (AZ-334) recovers metric scale from the
+unit-length translation that ``cv2.recoverPose`` emits by using ground
+sample distance (GSD), which requires the current drone height above the
+ground plane. The AGL signal lives in the C5 state estimator's nominal
+position; this module wraps that read so the C1 strategy does not import
+or hold a direct reference to the C5 estimator (which is built later in
+the composition-root topological order).
+
+AZ-919 introduces only the interface + plumbing. The GSD scale-recovery
+math lands in AZ-920, and the degraded-mode signal in AZ-921.
+"""
+
+from __future__ import annotations
+
+from typing import TYPE_CHECKING, Protocol, runtime_checkable
+
+from gps_denied_onboard._types.state import IsamState
+
+if TYPE_CHECKING:
+    from collections.abc import Callable
+
+    from gps_denied_onboard.components.c5_state.eskf_baseline import (
+        EskfStateEstimator,
+    )
+
+__all__ = [
+    "AltitudeProvider",
+    "EskfNominalAltitudeProvider",
+]
+
+
+@runtime_checkable
+class AltitudeProvider(Protocol):
+    """Read the drone's current AGL height in metres, or ``None``.
+
+    Producers MUST return ``None`` whenever the local-ENU origin has not
+    yet been anchored (pre cold-start) or the underlying estimator is
+    in :class:`IsamState.LOST`. Consumers MUST treat ``None`` as "no
+    reliable AGL" and fall back to a non-scale-recovery code path
+    (AZ-921 formalises that fallback as a degraded VIO output).
+    """
+
+    def agl_m(self, now_ns: int) -> float | None:  # pragma: no cover - Protocol
+        """Return AGL in metres at ``now_ns`` (monotonic), or ``None``.
+
+        ``now_ns`` is the same monotonic timebase used by the C1 strategy
+        for ``VioOutput.emitted_at_ns``. It is currently advisory — the
+        ESKF impl does not interpolate — but the parameter is in the
+        Protocol so future implementations (e.g. an LPF-smoothed AGL or
+        a DEM-aware provider) can interpolate or extrapolate without a
+        breaking change.
+        """
+        ...
+
+
+class EskfNominalAltitudeProvider:
+    """Concrete :class:`AltitudeProvider` backed by the C5 ESKF estimator.
+
+    Reads AGL as the Z component of the ESKF nominal-position vector in
+    local-ENU. The takeoff origin is anchored at local-ENU ``(0, 0, 0)``
+    when ``set_takeoff_origin`` lands, so ``nominal_pos_z`` IS the AGL
+    once the origin has been set — no separate cold-start-altitude
+    subtraction is needed.
+
+    The estimator instance is supplied through a callable rather than
+    held directly because the composition root builds C1 (where this
+    provider is wired) before C5. The callable closes over the
+    composition root's mutable ``constructed`` dict and resolves the
+    estimator at every ``agl_m`` call, which is the same time the C1
+    strategy actually consumes the AGL signal (well after the topo
+    order has built C5).
+    """
+
+    def __init__(
+        self,
+        estimator_supplier: Callable[[], EskfStateEstimator | None],
+    ) -> None:
+        self._estimator_supplier = estimator_supplier
+
+    def agl_m(self, now_ns: int) -> float | None:
+        estimator = self._estimator_supplier()
+        if estimator is None:
+            return None
+        if getattr(estimator, "_takeoff_origin_set", None) is None:
+            return None
+        if getattr(estimator, "_isam2_state", None) == IsamState.LOST:
+            return None
+        nominal_pos = getattr(estimator, "_nominal_pos", None)
+        if nominal_pos is None:
+            return None
+        return float(nominal_pos[2])

src/gps_denied_onboard/helpers/imu_units.py

@@ -0,0 +1,76 @@
+"""MAVLink IMU unit + frame conversion to the nav-side SI/FLU contract.
+
+The MAVLink ``RAW_IMU`` and ``SCALED_IMU2`` / ``SCALED_IMU3`` messages
+ship accelerometer values as **milli-g** (mG, where ``1 mG = 9.80665e-3
+m/s²``) and gyroscope values as **milli-radians per second** (mrad/s).
+The body frame is **FRD** (X-forward, Y-right, Z-down).
+
+The downstream consumers (``c5_state.eskf_baseline``, the
+GTSAM-backed ``helpers.imu_preintegrator``) expect:
+
+* Accelerometer in **m/s²**.
+* Gyroscope in **rad/s**.
+* Body frame **FLU / Z-up** so the body Z axis is parallel to the
+  ENU world Z axis at identity rotation. The ``c5_state``
+  stationary-attitude unit test pins this with a specific-force
+  vector of ``(0, 0, +9.80665)``.
+
+This module is the single source of truth for that conversion.
+Every adapter that constructs an ``ImuSample`` or
+``ImuTelemetrySample`` from raw MAVLink fields MUST route through
+``mavlink_imu_to_si_flu`` (AZ-918).
+
+The conversion is intentionally pure: no allocations beyond the
+returned tuples, no logging, no clock access. Cheap enough to call
+once per IMU sample at 100 Hz on the Tier-2 (Jetson) hot path.
+"""
+
+from __future__ import annotations
+
+from typing import Final
+
+__all__ = [
+    "MG_TO_M_S2",
+    "MRAD_S_TO_RAD_S",
+    "mavlink_imu_to_si_flu",
+]
+
+MG_TO_M_S2: Final[float] = 9.80665e-3
+MRAD_S_TO_RAD_S: Final[float] = 1.0e-3
+
+
+def mavlink_imu_to_si_flu(
+    *,
+    xacc: float,
+    yacc: float,
+    zacc: float,
+    xgyro: float,
+    ygyro: float,
+    zgyro: float,
+) -> tuple[tuple[float, float, float], tuple[float, float, float]]:
+    """Convert MAVLink ``RAW_IMU``/``SCALED_IMU*`` fields to SI/FLU.
+
+    Args:
+        xacc, yacc, zacc: MAVLink accelerometer fields in mG (FRD body).
+        xgyro, ygyro, zgyro: MAVLink gyroscope fields in mrad/s (FRD body).
+
+    Returns:
+        Two 3-tuples ``(accel_xyz_si_flu, gyro_xyz_si_flu)``:
+
+        * Accel in m/s², body frame FLU (Z-up).
+        * Gyro in rad/s, body frame FLU (Z-up).
+
+        The FRD→FLU transform is a Y- and Z-axis negation (X is
+        unchanged because both conventions point X forward).
+    """
+    accel_xyz_si_flu = (
+        xacc * MG_TO_M_S2,
+        -yacc * MG_TO_M_S2,
+        -zacc * MG_TO_M_S2,
+    )
+    gyro_xyz_si_flu = (
+        xgyro * MRAD_S_TO_RAD_S,
+        -ygyro * MRAD_S_TO_RAD_S,
+        -zgyro * MRAD_S_TO_RAD_S,
+    )
+    return accel_xyz_si_flu, gyro_xyz_si_flu