[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>

tests/unit/c5_state/test_az921_eskf_add_vio_scale_quality.py

@@ -0,0 +1,187 @@
+"""AZ-921 AC-5 — `EskfStateEstimator.add_vio` translation-R_meas
+adaptation per `VioOutput.scale_quality`.
+
+Two layers of coverage:
+
+1. **Unit** — `_adapt_vio_r_meas` is the pure helper that owns the
+   block-wise adaptation. Three tests pin its three branches.
+2. **Integration** — `EskfStateEstimator.add_vio` is run end-to-end
+   with each `scale_quality` branch and the resulting nominal position
+   delta is compared. ``"metric"`` produces a large position update
+   (the Kalman gain trusts the measurement); ``"unknown"`` produces an
+   essentially-zero position update (the gain block is ~0).
+"""
+
+from __future__ import annotations
+
+import dataclasses
+from typing import Any
+from unittest import mock
+
+import numpy as np
+import pytest
+
+from gps_denied_onboard._types.nav import (
+    FeatureQuality,
+    ImuBias,
+    ScaleQuality,
+    VioOutput,
+)
+from gps_denied_onboard.components.c5_state import C5StateConfig
+from gps_denied_onboard.components.c5_state.eskf_baseline import (
+    _DIRECTION_ONLY_TRANSLATION_SIGMA_M,
+    _UNKNOWN_TRANSLATION_SIGMA_M,
+    EskfStateEstimator,
+    _adapt_vio_r_meas,
+)
+from gps_denied_onboard.config import load_config
+from gps_denied_onboard.config.schema import Config
+
+# ----------------------------------------------------------------------
+# _adapt_vio_r_meas helper — unit coverage.
+
+
+def test_adapt_vio_r_meas_metric_returns_input_unchanged() -> None:
+    # Arrange
+    r_meas = np.diag([0.04, 0.04, 0.04, 0.0001, 0.0001, 0.0001])
+
+    # Act
+    adapted = _adapt_vio_r_meas(r_meas, "metric")
+
+    # Assert
+    assert np.array_equal(adapted, r_meas)
+    # Identity is acceptable too — the caller MUST NOT mutate either way.
+    assert np.array_equal(r_meas, np.diag([0.04, 0.04, 0.04, 0.0001, 0.0001, 0.0001]))
+
+
+def test_adapt_vio_r_meas_direction_only_overrides_translation_block() -> None:
+    # Arrange
+    r_meas = np.diag([0.04, 0.04, 0.04, 0.0001, 0.0001, 0.0001])
+    expected_translation_var = _DIRECTION_ONLY_TRANSLATION_SIGMA_M**2
+
+    # Act
+    adapted = _adapt_vio_r_meas(r_meas, "direction_only")
+
+    # Assert — translation block becomes diag(sigma^2 * I), rotation
+    # block left as-is.
+    assert np.allclose(adapted[0:3, 0:3], np.eye(3) * expected_translation_var)
+    assert np.allclose(adapted[3:6, 3:6], r_meas[3:6, 3:6])
+    # And the input was NOT mutated.
+    assert r_meas[0, 0] == pytest.approx(0.04)
+
+
+def test_adapt_vio_r_meas_unknown_inflates_translation_far_beyond_direction_only() -> None:
+    # Arrange
+    r_meas = np.diag([0.04, 0.04, 0.04, 0.0001, 0.0001, 0.0001])
+    expected_translation_var = _UNKNOWN_TRANSLATION_SIGMA_M**2
+
+    # Act
+    adapted = _adapt_vio_r_meas(r_meas, "unknown")
+
+    # Assert
+    assert np.allclose(adapted[0:3, 0:3], np.eye(3) * expected_translation_var)
+    assert np.allclose(adapted[3:6, 3:6], r_meas[3:6, 3:6])
+    # And the input was NOT mutated.
+    assert r_meas[0, 0] == pytest.approx(0.04)
+
+
+def test_adapt_vio_r_meas_unknown_translation_var_is_strictly_larger() -> None:
+    # Sanity: an "unknown" measurement must be treated as strictly less
+    # informative than a "direction_only" one (otherwise the three-tier
+    # split is meaningless).
+    assert _UNKNOWN_TRANSLATION_SIGMA_M > _DIRECTION_ONLY_TRANSLATION_SIGMA_M
+
+
+# ----------------------------------------------------------------------
+# add_vio integration — end-to-end position-update sensitivity per
+# scale_quality.
+
+
+def _build_config(**state_overrides: Any) -> Config:
+    cfg = load_config(env={}, paths=(), require_env=False)
+    new_state = dataclasses.replace(C5StateConfig(strategy="eskf"), **state_overrides)
+    components = dict(cfg.components or {})
+    components["c5_state"] = new_state
+    return dataclasses.replace(cfg, components=components)
+
+
+def _make_estimator() -> EskfStateEstimator:
+    return EskfStateEstimator(
+        _build_config(),
+        imu_preintegrator=mock.MagicMock(),
+        se3_utils=mock.MagicMock(),
+        wgs_converter=mock.MagicMock(),
+        fdr_client=mock.MagicMock(),
+    )
+
+
+def _identity_pose() -> np.ndarray:
+    return np.eye(4, dtype=np.float64)
+
+
+def _pose_translated(x: float) -> np.ndarray:
+    p = np.eye(4, dtype=np.float64)
+    p[0, 3] = x
+    return p
+
+
+_ZERO_BIAS = ImuBias(accel_bias=(0.0, 0.0, 0.0), gyro_bias=(0.0, 0.0, 0.0))
+_NEUTRAL_FQ = FeatureQuality(tracked=20, new=2, lost=1, mean_parallax=5.0, mre_px=1.0)
+
+
+def _vio(
+    frame_id: int,
+    ts_ns: int,
+    pose: np.ndarray,
+    scale_quality: ScaleQuality,
+) -> VioOutput:
+    import gtsam
+    return VioOutput(
+        frame_id=str(frame_id),
+        relative_pose_T=gtsam.Pose3(pose),
+        pose_covariance_6x6=np.eye(6) * 0.01,
+        imu_bias=_ZERO_BIAS,
+        feature_quality=_NEUTRAL_FQ,
+        emitted_at_ns=int(ts_ns),
+        scale_quality=scale_quality,
+    )
+
+
+def _position_delta_after_vio_pair(scale_quality: ScaleQuality) -> float:
+    # Arrange — two VIO frames; first seeds, second reports a +1 m
+    # forward translation. Returns the magnitude of the nominal-pos
+    # update the ESKF applied. With scale_quality="metric" the Kalman
+    # gain in the position block is ~1 and the update is ~1 m.
+    estimator = _make_estimator()
+    estimator.add_vio(_vio(1, 1_000_000_000, _identity_pose(), scale_quality))
+    pos_before = estimator._nominal_pos.copy()
+    estimator.add_vio(_vio(2, 1_100_000_000, _pose_translated(1.0), scale_quality))
+
+    # Act / Assert wrapper — returns the metric (m) the caller compares.
+    return float(np.linalg.norm(estimator._nominal_pos - pos_before))
+
+
+def test_metric_scale_quality_applies_full_position_update() -> None:
+    # Assert — with metric scale, the ESKF trusts the +1 m measurement
+    # almost completely (R_meas[0:3, 0:3] = 0.01 m^2 = 10 cm sigma).
+    metric_delta = _position_delta_after_vio_pair("metric")
+    assert metric_delta > 0.5
+
+
+def test_unknown_scale_quality_yields_near_zero_position_update() -> None:
+    # Assert — with unknown scale, the ESKF inflates R_meas to (1000 m)^2
+    # so the Kalman gain block is ~0 and the +1 m measurement does
+    # essentially nothing to the position state.
+    unknown_delta = _position_delta_after_vio_pair("unknown")
+    assert unknown_delta < 1.0e-3
+
+
+def test_direction_only_scale_quality_yields_partial_update_between_metric_and_unknown() -> None:
+    # Assert — direction_only sits between the two extremes. The
+    # specific magnitude depends on the prior covariance + chosen
+    # sigma, but the ordering is what matters.
+    metric_delta = _position_delta_after_vio_pair("metric")
+    direction_only_delta = _position_delta_after_vio_pair("direction_only")
+    unknown_delta = _position_delta_after_vio_pair("unknown")
+
+    assert unknown_delta < direction_only_delta < metric_delta