[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/components/c1_vio/_gsd_scale.py

@@ -0,0 +1,175 @@
+"""Monocular VIO metric scale recovery via Ground Sample Distance (GSD).
+
+`cv2.recoverPose` returns a unit-length translation direction; the
+absolute metric scale is unrecoverable from a single camera with no
+known-size reference. AZ-920 closes that gap for the Derkachi flight
+geometry — nadir-pointing camera, fixed-wing aircraft in cruise,
+piecewise-planar agricultural terrain — by using the textbook
+ground-plane / camera-height scale-recovery method.
+
+The key observation: for a nadir-pointing pinhole camera at AGL ``h``
+metres with focal length ``f_x`` in pixels, the GSD (ground sample
+distance, m/px) is ``h / f_x``. The mean inlier optical-flow magnitude
+between consecutive frames, in pixels, therefore corresponds to a
+metric ground-plane displacement of ``mean_flow_px * h / f_x``. That
+displacement is the magnitude of the camera-XY-plane component of the
+translation; dividing by ``||t_unit_xy||`` yields the scale factor to
+apply to the unit translation vector.
+
+References (web-search-verified 2026-05-27):
+
+- Tian et al. 2021, "A light-weight scale recovery framework based on
+  plane geometry", `arXiv:2101.05995 <https://arxiv.org/pdf/2101.05995v1>`_.
+- Song & Chandraker, "Robust Scale Estimation in Real-Time Monocular
+  SFM for Autonomous Driving", CVPR 2014.
+- Wikipedia, `Ground sample distance
+  <https://en.wikipedia.org/wiki/Ground_sample_distance>`_.
+
+This module is intentionally a pure-Python helper: no side effects, no
+logging, no clock access. The caller (``KltRansacStrategy.process_frame``)
+guards the integration boundary and decides what to do when the
+returned uncertainty is ``inf`` — that decision is the AZ-921 surface.
+"""
+
+from __future__ import annotations
+
+import math
+from typing import Final
+
+import numpy as np
+import numpy.typing as npt
+
+from gps_denied_onboard._types.nav import ScaleQuality
+
+__all__ = [
+    "INFINITE_SCALE_UNCERTAINTY_M",
+    "RELATIVE_DISPLACEMENT_NOISE",
+    "ScaleRecoveryResult",
+    "recover_scale",
+]
+
+
+# 10% relative-displacement noise budget covering GSD focal-length
+# error (~3% per camera_info.md), AGL bias from the ESKF integrator,
+# and inlier-flow-mean residual scatter. Pin until AZ-920 lands a real
+# Jetson run and we can measure the residual.
+RELATIVE_DISPLACEMENT_NOISE: Final[float] = 0.10
+
+# Sentinel returned when scale is not recoverable in the current frame
+# (AGL unknown, near-pure-vertical motion, zero inlier flow). The
+# caller treats this as "do not trust translation magnitude".
+INFINITE_SCALE_UNCERTAINTY_M: Final[float] = math.inf
+
+# AZ-921 — below this horizontal-component magnitude the camera is
+# moving essentially along its optical axis (climb/descent for a nadir
+# cam), which is degenerate for the GSD MAGNITUDE recovery — but the
+# DIRECTION of t_unit is still meaningful, so this case routes to
+# scale_quality="direction_only" rather than "unknown".
+_MIN_T_HORIZONTAL_MAGNITUDE: Final[float] = 0.05
+
+# AZ-921 — below this mean inlier flow magnitude the strategy cannot
+# distinguish "stationary aircraft" from "scene-locked features that
+# happen not to move" (hover, stale features). Routes to
+# scale_quality="unknown" because the geometry is too degenerate to
+# trust either the magnitude or the direction of t_unit.
+_MIN_MEAN_FLOW_PX: Final[float] = 0.5
+
+
+class ScaleRecoveryResult:
+    """Lightweight return tuple — keeps three named values together.
+
+    Not a frozen dataclass to avoid the import-cost overhead on the
+    KLT/RANSAC hot path (called every frame).
+    """
+
+    __slots__ = ("quality", "scale_factor", "scale_uncertainty_m")
+
+    def __init__(
+        self,
+        scale_factor: float,
+        scale_uncertainty_m: float,
+        quality: ScaleQuality,
+    ) -> None:
+        self.scale_factor = scale_factor
+        self.scale_uncertainty_m = scale_uncertainty_m
+        self.quality = quality
+
+    def is_recoverable(self) -> bool:
+        return math.isfinite(self.scale_uncertainty_m)
+
+    def __repr__(self) -> str:
+        return (
+            f"ScaleRecoveryResult(scale_factor={self.scale_factor!r}, "
+            f"scale_uncertainty_m={self.scale_uncertainty_m!r}, "
+            f"quality={self.quality!r})"
+        )
+
+
+def recover_scale(
+    *,
+    t_unit: npt.NDArray[np.float64],
+    pts_prev: npt.NDArray[np.float64],
+    pts_curr: npt.NDArray[np.float64],
+    intrinsics_3x3: npt.NDArray[np.float64],
+    agl_m: float | None,
+) -> ScaleRecoveryResult:
+    """Compute the metric scale factor for a monocular unit translation.
+
+    Args:
+        t_unit: Unit-length 3-vector from ``cv2.recoverPose`` (camera
+            coords; OpenCV convention is X-right, Y-down, Z-forward).
+        pts_prev: Nx2 inlier feature pixel coordinates in the previous
+            frame.
+        pts_curr: Nx2 inlier feature pixel coordinates in the current
+            frame.
+        intrinsics_3x3: Camera intrinsic matrix; the function uses
+            ``f_x = K[0, 0]`` as the nadir-pinhole focal length in
+            pixels (``f_y`` is assumed equal — the Derkachi factory
+            calibration matches this).
+        agl_m: Above-ground-level height in metres, or ``None`` if the
+            AltitudeProvider could not resolve it.
+
+    Returns:
+        :class:`ScaleRecoveryResult`. ``scale_uncertainty_m`` is
+        :data:`INFINITE_SCALE_UNCERTAINTY_M` when scale cannot be
+        recovered (AGL unknown, zero focal length, degenerate flow,
+        near-pure-vertical motion). In every degenerate case the
+        ``scale_factor`` is ``0.0`` so multiplying a unit translation
+        yields a zero translation — safe default that lets the ESKF's
+        own bias prior absorb the missing constraint.
+
+    Notes:
+        Geometry assumption: nadir-pointing pinhole camera. The
+        derivation is in the module docstring. When the assumption
+        breaks (banked turn, gimbal not nadir, mountainous terrain),
+        AZ-921 will gate this output via a ``scale_quality`` signal.
+    """
+    if agl_m is None or agl_m <= 0.0:
+        return ScaleRecoveryResult(0.0, INFINITE_SCALE_UNCERTAINTY_M, "unknown")
+
+    fx = float(intrinsics_3x3[0, 0])
+    if fx <= 0.0 or not math.isfinite(fx):
+        return ScaleRecoveryResult(0.0, INFINITE_SCALE_UNCERTAINTY_M, "unknown")
+
+    if pts_prev.shape[0] == 0 or pts_curr.shape[0] == 0:
+        return ScaleRecoveryResult(0.0, INFINITE_SCALE_UNCERTAINTY_M, "unknown")
+    if pts_prev.shape != pts_curr.shape:
+        return ScaleRecoveryResult(0.0, INFINITE_SCALE_UNCERTAINTY_M, "unknown")
+
+    flow = pts_curr - pts_prev
+    flow_magnitudes_px = np.linalg.norm(flow, axis=1)
+    mean_flow_px = float(np.mean(flow_magnitudes_px))
+    if not math.isfinite(mean_flow_px) or mean_flow_px < _MIN_MEAN_FLOW_PX:
+        return ScaleRecoveryResult(0.0, INFINITE_SCALE_UNCERTAINTY_M, "unknown")
+
+    t_horizontal = float(np.hypot(t_unit[0], t_unit[1]))
+    if not math.isfinite(t_horizontal) or t_horizontal < _MIN_T_HORIZONTAL_MAGNITUDE:
+        return ScaleRecoveryResult(0.0, INFINITE_SCALE_UNCERTAINTY_M, "direction_only")
+
+    # GSD m/px = AGL / f_x for nadir pinhole.
+    gsd_m_per_px = agl_m / fx
+    displacement_m = mean_flow_px * gsd_m_per_px
+    scale_factor = displacement_m / t_horizontal
+    scale_uncertainty_m = displacement_m * RELATIVE_DISPLACEMENT_NOISE
+
+    return ScaleRecoveryResult(scale_factor, scale_uncertainty_m, "metric")

src/gps_denied_onboard/components/c1_vio/config.py

@@ -28,6 +28,7 @@ min features for pose, per-frame debug log). Only consulted when
 
 from __future__ import annotations
 
+import math
 from dataclasses import dataclass, field
 from typing import Final
 
@@ -214,6 +215,15 @@ class KltRansacConfig:
     internal RANSAC; default 1.0 px in normalised image coordinates
     (OpenCV's documented default for forward-looking cameras).
 
+    ``max_frame_rotation_rad`` is the AZ-922 cheirality gate
+    threshold — pose-recovery results whose rotation angle exceeds
+    this value are rejected as the essential-matrix twisted-pair
+    ambiguity (``cv2.recoverPose`` can return a 180° rotation when
+    parallax is low). Default 0.524 rad (30°) — a generous upper bound
+    for any physically plausible single-frame body rotation in
+    RESTRICT-UAV-3 within the 33 ms / 30 fps frame budget. Must be
+    in (0, π].
+
     ``per_frame_debug_log`` enables a DEBUG log line per
     ``process_frame`` — OFF by default (would flood at 3 Hz steady-state).
     """
@@ -224,6 +234,7 @@ class KltRansacConfig:
     min_features_for_pose: int = 30
     ransac_inlier_ratio: float = 0.5
     essential_matrix_ransac_threshold_px: float = 1.0
+    max_frame_rotation_rad: float = math.radians(30.0)
     per_frame_debug_log: bool = False
 
     def __post_init__(self) -> None:
@@ -257,6 +268,11 @@ class KltRansacConfig:
                 "KltRansacConfig.essential_matrix_ransac_threshold_px must be "
                 f"> 0; got {self.essential_matrix_ransac_threshold_px}"
             )
+        if not (0.0 < self.max_frame_rotation_rad <= math.pi):
+            raise ConfigError(
+                "KltRansacConfig.max_frame_rotation_rad must be in (0, pi]; "
+                f"got {self.max_frame_rotation_rad}"
+            )
 
 
 @dataclass(frozen=True)

src/gps_denied_onboard/components/c1_vio/klt_ransac.py

@@ -74,6 +74,7 @@ Risk mitigations (see task spec for full text):
 
 from __future__ import annotations
 
+import math
 from typing import TYPE_CHECKING, Any, Final, Literal
 
 import cv2
@@ -82,6 +83,7 @@ import numpy as np
 from gps_denied_onboard._types.nav import (
     FeatureQuality,
     ImuBias,
+    ScaleQuality,
     VioHealth,
     VioOutput,
     VioState,
@@ -92,6 +94,10 @@ from gps_denied_onboard.components.c1_vio._facade_spine import (
     bias_norm,
     se3_from_4x4,
 )
+from gps_denied_onboard.components.c1_vio._gsd_scale import (
+    ScaleRecoveryResult,
+    recover_scale,
+)
 from gps_denied_onboard.components.c1_vio.errors import (
     VioFatalError,
     VioInitializingError,
@@ -117,6 +123,7 @@ if TYPE_CHECKING:
     from gps_denied_onboard.components.c1_vio.config import KltRansacConfig
     from gps_denied_onboard.config import Config
     from gps_denied_onboard.fdr_client.client import FdrClient
+    from gps_denied_onboard.helpers.altitude_provider import AltitudeProvider
 
 
 __all__ = ["KltRansacStrategy"]
@@ -134,6 +141,13 @@ _ESSENTIAL_MATRIX_DOF: Final[int] = 5
 # (relative pose is identity anyway). Documented limit, not derived.
 _INIT_STATE_COVARIANCE_SCALAR: Final[float] = 10.0
 
+# AZ-920: floor on the per-axis position variance for scale-recovered
+# translations. Even with perfect inlier flow the GSD has a
+# ~3 % focal-length error (see flight_derkachi/camera_info.md) and
+# the AGL has ESKF integrator bias, so a sub-millimetre cov is
+# implausible. 1 cm sigma per axis is the smallest defensible value.
+_MIN_POSITION_VARIANCE_M2: Final[float] = 1.0e-4
+
 
 def _grayscale(image: npt.NDArray[Any]) -> npt.NDArray[Any]:
     """Coerce a NavCameraFrame image to OpenCV's expected 2D ``uint8``.
@@ -168,6 +182,21 @@ def _grayscale(image: npt.NDArray[Any]) -> npt.NDArray[Any]:
     )
 
 
+def _rotation_angle_rad(R: np.ndarray) -> float:
+    """Axis-angle magnitude of a 3x3 rotation matrix.
+
+    AZ-922 cheirality gate primitive. Uses ``θ = arccos((trace(R) - 1) / 2)``;
+    the ``arccos`` argument is clipped to ``[-1, +1]`` to defend against
+    floating-point drift on near-identity rotations (where ``trace`` can
+    exceed 3.0 by ~1e-15) and on the 180° antipode (where it can dip
+    below -1.0 by the same amount). The returned angle is always in
+    ``[0, π]``.
+    """
+    trace = float(np.trace(R[:3, :3]))
+    cos_theta = max(-1.0, min(1.0, (trace - 1.0) / 2.0))
+    return float(math.acos(cos_theta))
+
+
 def _intrinsics_3x3(calibration: CameraCalibration) -> np.ndarray:
     """Pull the 3x3 intrinsics matrix from a CameraCalibration DTO.
 
@@ -208,6 +237,7 @@ class KltRansacStrategy(FacadeSpine):
         *,
         fdr_client: FdrClient,
         clock: Clock | None = None,
+        altitude_provider: AltitudeProvider | None = None,
     ) -> None:
         c1_block = config.components["c1_vio"]
         if c1_block.strategy != _STRATEGY_LABEL:
@@ -227,6 +257,12 @@ class KltRansacStrategy(FacadeSpine):
         self._feature_threshold: int = self._cfg.min_features_for_pose
         self._producer_id: str = _PRODUCER_ID
         self._strategy_label: str = _STRATEGY_LABEL
+        # AZ-919: plumbing only — stored on the instance but not yet consumed.
+        # The scale-recovery math that consumes this lands in AZ-920;
+        # the degraded-mode signal when the provider yields ``None``
+        # lands in AZ-921. Default ``None`` preserves AC-3 bug-for-bug
+        # compatibility with strategies that have not been re-wired.
+        self._altitude_provider: AltitudeProvider | None = altitude_provider
 
         # Per-frame state.
         self._prev_gray: np.ndarray | None = None
@@ -383,20 +419,70 @@ class KltRansacStrategy(FacadeSpine):
                 reason="recover_pose_no_solution",
             )
 
-        # 7. Build SE(3) from (R, t).
+        # 6.5 AZ-922 — cheirality gate. ``cv2.recoverPose`` can return
+        # a 180°-flipped rotation (essential-matrix twisted-pair
+        # ambiguity) under low-parallax conditions. Reject any
+        # single-frame rotation that exceeds the RESTRICT-UAV-3
+        # angular-velocity envelope. Routes through the same
+        # _pose_recovery_failed path as any other failed-pose case so
+        # AC-7 (LOST after N consecutive failures) is honoured.
+        R_3x3 = np.asarray(R, dtype=np.float64)
+        rotation_angle_rad = _rotation_angle_rad(R_3x3)
+        if rotation_angle_rad > self._cfg.max_frame_rotation_rad:
+            return self._pose_recovery_failed(
+                frame_id_str,
+                emitted_at_ns,
+                prior_feature_count=prior_feature_count,
+                reason=(
+                    f"implausible_rotation_angle: "
+                    f"{math.degrees(rotation_angle_rad):.1f}deg > "
+                    f"{math.degrees(self._cfg.max_frame_rotation_rad):.1f}deg threshold"
+                ),
+            )
+
+        # 7. AZ-920 — recover metric scale for the otherwise-unit
+        # translation. Operates on the final inlier set (AZ-282 +
+        # findEssentialMat's internal RANSAC). Only fires when the
+        # AltitudeProvider is wired by the AZ-919 composition root;
+        # when it is None (unit-test construction, legacy callers) the
+        # strategy preserves pre-AZ-920 behaviour bug-for-bug
+        # (AZ-919 AC-3 compatibility).
+        t_unit = np.asarray(t, dtype=np.float64).flatten()
+        t_metric = t_unit
+        scale_result: ScaleRecoveryResult | None = None
+        if self._altitude_provider is not None:
+            if em_mask is not None:
+                em_mask_flat = np.asarray(em_mask).flatten().astype(bool)
+                final_pts_prev = pts_prev[em_mask_flat]
+                final_pts_curr = pts_curr[em_mask_flat]
+            else:
+                final_pts_prev = pts_prev
+                final_pts_curr = pts_curr
+            agl_m = self._altitude_provider.agl_m(emitted_at_ns)
+            scale_result = recover_scale(
+                t_unit=t_unit,
+                pts_prev=final_pts_prev,
+                pts_curr=final_pts_curr,
+                intrinsics_3x3=K,
+                agl_m=agl_m,
+            )
+            if scale_result.is_recoverable():
+                t_metric = scale_result.scale_factor * t_unit
+
+        # 8. Build SE(3) from (R, t_metric).
         pose_4x4 = np.eye(4, dtype=np.float64)
         pose_4x4[:3, :3] = np.asarray(R, dtype=np.float64)
-        pose_4x4[:3, 3] = np.asarray(t, dtype=np.float64).flatten()
+        pose_4x4[:3, 3] = t_metric
         pose = se3_from_4x4(pose_4x4)
 
-        # 8. Final inlier count for state classification + covariance.
+        # 9. Final inlier count for state classification + covariance.
         final_inlier_count = (
             int(np.count_nonzero(em_mask)) if em_mask is not None else ransac_result.inlier_count
         )
         if final_inlier_count < _ESSENTIAL_MATRIX_DOF:
             final_inlier_count = ransac_result.inlier_count
 
-        # 9. Estimate covariance from the AZ-282 median residual +
+        # 10. Estimate covariance from the AZ-282 median residual +
         # inlier-count penalty. Honest-covariance invariant (AC-9): no
         # client-side floor; the formula is residual_var / DOF + small
         # inlier-count term so the cov grows monotonically as inliers
@@ -406,6 +492,24 @@ class KltRansacStrategy(FacadeSpine):
             inlier_count=final_inlier_count,
         )
 
+        # 11. AZ-920/AZ-921 — when scale_quality == "metric", replace the
+        # position block of cov with the metric variance from GSD scale
+        # recovery so consumers reading pose_covariance_6x6 directly get
+        # honest m² units. When scale_quality is "direction_only" or
+        # "unknown" the cov stays in the geometry-natural units; the
+        # ESKF (AZ-921 add_vio) inflates its own R_meas based on the
+        # scale_quality flag, so it does not need the cov to encode the
+        # "do not trust me" message.
+        scale_quality: ScaleQuality = (
+            scale_result.quality if scale_result is not None else "unknown"
+        )
+        if scale_result is not None and scale_quality == "metric":
+            pos_var = max(
+                float(scale_result.scale_uncertainty_m) ** 2,
+                _MIN_POSITION_VARIANCE_M2,
+            )
+            cov[0:3, 0:3] = np.eye(3, dtype=np.float64) * pos_var
+
         # 10. Build VioOutput.
         fq = FeatureQuality(
             tracked=int(final_inlier_count),
@@ -423,6 +527,7 @@ class KltRansacStrategy(FacadeSpine):
             imu_bias=self._latest_bias,
             feature_quality=fq,
             emitted_at_ns=emitted_at_ns,
+            scale_quality=scale_quality,
         )
 
         # 11. Success path — reset lost counter, classify state.

src/gps_denied_onboard/components/c5_state/eskf_baseline.py

@@ -56,7 +56,6 @@ import numpy as np
 from numpy.linalg import LinAlgError
 
 from gps_denied_onboard._types.geo import LatLonAlt
-from gps_denied_onboard.clock.wall_clock import WallClock
 from gps_denied_onboard._types.state import (
     EstimatorHealth,
     EstimatorOutput,
@@ -64,22 +63,23 @@ from gps_denied_onboard._types.state import (
     PoseSourceLabel,
     Quat,
 )
+from gps_denied_onboard.clock.wall_clock import WallClock
 from gps_denied_onboard.components.c5_state._fallback_watcher import (
     FallbackEngagementCallback,
     FallbackRecoveryCallback,
     FallbackSubscription,
     FallbackWatcher,
 )
-from gps_denied_onboard.components.c5_state._source_label_sm import (
-    RejectionCallback,
-    RejectionSubscription,
-    SourceLabelStateMachine,
-)
 from gps_denied_onboard.components.c5_state._orthorectifier import (
     MidFlightTileWriter,
     Orthorectifier,
     OrthorectifierThresholds,
 )
+from gps_denied_onboard.components.c5_state._source_label_sm import (
+    RejectionCallback,
+    RejectionSubscription,
+    SourceLabelStateMachine,
+)
 from gps_denied_onboard.components.c5_state.config import C5StateConfig
 from gps_denied_onboard.components.c5_state.errors import (
     EstimatorAlreadyStartedError,
@@ -139,6 +139,17 @@ _PROC_SIGMA_BG: Final[float] = 0.0001
 # Fall-back measurement noise when the input DTO has no covariance.
 _DEFAULT_MEAS_SIGMA: Final[float] = 0.1
 
+# AZ-921 — translation-block R_meas adaptation when the VIO scale signal
+# is degraded. The unit-length translation from cv2.recoverPose carries
+# direction information but no magnitude; we trust the direction at the
+# DIRECTION-ONLY level so the rotation update can still constrain the
+# state, then a follow-up scale-recovery event will reduce the position
+# uncertainty. UNKNOWN is the "effectively skip the translation update"
+# variance — large enough that the Kalman gain block is essentially
+# zero, while still finite so the Mahalanobis innovation gate computes.
+_DIRECTION_ONLY_TRANSLATION_SIGMA_M: Final[float] = 8.0
+_UNKNOWN_TRANSLATION_SIGMA_M: Final[float] = 1000.0
+
 # Filter divergence threshold (AC-9): Mahalanobis ``r^T S^{-1} r`` >
 # (10 sigma)^2 = 100 triggers EstimatorFatalError + AC-5.2 fallback.
 # The spec wording is "innovation exceeds 10x the measurement-covariance
@@ -243,8 +254,8 @@ class EskfStateEstimator(StateEstimator):
         # historically did NOT cache the full DTO so we keep a
         # dedicated field rather than abuse ``_prev_vio_*``.
         self._orthorectifier: Orthorectifier | None = None
-        self._latest_nav_frame: "NavCameraFrame | None" = None
-        self._latest_vio: "VioOutput | None" = None
+        self._latest_nav_frame: NavCameraFrame | None = None
+        self._latest_vio: VioOutput | None = None
 
         self._log.debug(
             "c5.state.eskf_initialised",
@@ -296,7 +307,7 @@ class EskfStateEstimator(StateEstimator):
         """
         self._orthorectifier = orthorectifier
 
-    def set_latest_nav_frame(self, frame: "NavCameraFrame") -> None:
+    def set_latest_nav_frame(self, frame: NavCameraFrame) -> None:
         """AZ-389 — buffer the most recent nav-camera frame.
 
         Quiet no-op when no orthorectifier is attached. See
@@ -541,6 +552,12 @@ class EskfStateEstimator(StateEstimator):
         H[0:3, _IDX_POS] = np.eye(3)
         H[3:6, _IDX_ROT] = prev_R  # rotate body-frame perturbation back to world
         R_meas = _measurement_noise(vio.pose_covariance_6x6)
+        # AZ-921 — VIO scale-quality gating on the translation block of
+        # R_meas. The rotation block is left as the VIO emitted it
+        # regardless of scale_quality; only the magnitude of t is
+        # affected by monocular scale ambiguity, not its direction in
+        # body frame or the rotation matrix.
+        R_meas = _adapt_vio_r_meas(R_meas, vio.scale_quality)
 
         try:
             self._kalman_update(H, residual, R_meas, source="vio")
@@ -991,7 +1008,7 @@ def create(
     wgs_converter: Any,
     fdr_client: Any,
     mid_flight_tile_writer: MidFlightTileWriter | None = None,
-    camera_calibration: "CameraCalibration | None" = None,
+    camera_calibration: CameraCalibration | None = None,
     flight_id: str | None = None,
     companion_id: str | None = None,
 ) -> tuple[StateEstimator, Any]:
@@ -1086,6 +1103,30 @@ def _measurement_noise(covariance: Any | None) -> np.ndarray:
     return cov
 
 
+def _adapt_vio_r_meas(r_meas: np.ndarray, scale_quality: str) -> np.ndarray:
+    """AZ-921 — translation-block adaptation per VIO scale-quality.
+
+    When the VIO strategy reports it could not recover metric scale,
+    the translation block of R_meas is overwritten so the Kalman gain
+    in that block goes to (near) zero, leaving the rotation block
+    free to constrain the state. The Mahalanobis innovation gate
+    stays at 100 (10 sigma); large translation R_meas keeps the test
+    finite and well below the threshold even on large residuals.
+
+    The function does not mutate the input array.
+    """
+    if scale_quality == "metric":
+        return r_meas
+    if scale_quality == "direction_only":
+        translation_variance = _DIRECTION_ONLY_TRANSLATION_SIGMA_M**2
+    else:
+        # "unknown" and any defensive default
+        translation_variance = _UNKNOWN_TRANSLATION_SIGMA_M**2
+    adapted = r_meas.copy()
+    adapted[0:3, 0:3] = np.eye(3, dtype=np.float64) * translation_variance
+    return adapted
+
+
 def _skew(v: np.ndarray) -> np.ndarray:
     return np.array(
         [

src/gps_denied_onboard/components/c8_fc_adapter/_inbound_mavlink.py

@@ -36,6 +36,7 @@ from gps_denied_onboard._types.geo import LatLonAlt
 from gps_denied_onboard.clock.wall_clock import WallClock
 from gps_denied_onboard.components.c8_fc_adapter._subscription import SubscriptionBus
 from gps_denied_onboard.components.c8_fc_adapter._telemetry_rings import TelemetryRing
+from gps_denied_onboard.helpers.imu_units import mavlink_imu_to_si_flu
 from gps_denied_onboard.logging import get_logger
 
 if TYPE_CHECKING:
@@ -194,17 +195,26 @@ class PymavlinkInboundDecoder:
     # Per-type handlers
 
     def _handle_imu(self, msg: Any) -> bool:
-        # pymavlink RAW_IMU exposes raw signed-16 ticks; we forward as-is.
-        accel = (float(msg.xacc), float(msg.yacc), float(msg.zacc))
-        gyro = (float(msg.xgyro), float(msg.ygyro), float(msg.zgyro))
+        # RAW_IMU / SCALED_IMU2 wire format is mG accel + mrad/s gyro in
+        # FRD body; convert to SI/FLU at the boundary (AZ-918) so
+        # downstream consumers see the contract ImuTelemetrySample
+        # advertises.
+        accel_si_flu, gyro_si_flu = mavlink_imu_to_si_flu(
+            xacc=float(msg.xacc),
+            yacc=float(msg.yacc),
+            zacc=float(msg.zacc),
+            xgyro=float(msg.xgyro),
+            ygyro=float(msg.ygyro),
+            zgyro=float(msg.zgyro),
+        )
         # `time_usec` is FC-side; we override with monotonic_ns at decode boundary
         # for the FdrTelemetryFrame.received_at; the payload's ts_ns is the
         # FC sensor stamp (epoch us -> ns).
         sensor_ts_ns = int(msg.time_usec) * 1000
         payload = ImuTelemetrySample(
             ts_ns=sensor_ts_ns,
-            accel_xyz=accel,
-            gyro_xyz=gyro,
+            accel_xyz=accel_si_flu,
+            gyro_xyz=gyro_si_flu,
         )
         return self._dispatch(TelemetryKind.IMU_SAMPLE, payload, ring=self.imu_ring)
 

src/gps_denied_onboard/components/c8_fc_adapter/tlog_replay_adapter.py

@@ -81,6 +81,7 @@ from gps_denied_onboard.components.c8_fc_adapter.errors import (
 )
 from gps_denied_onboard.components.c8_fc_adapter.interface import MavlinkTransport
 from gps_denied_onboard.fdr_client.records import FdrRecord
+from gps_denied_onboard.helpers.imu_units import mavlink_imu_to_si_flu
 from gps_denied_onboard.helpers.iso_timestamps import iso_ts_now
 from gps_denied_onboard.logging import get_logger
 
@@ -716,20 +717,18 @@ class TlogReplayFcAdapter:
 
     def _handle_imu(self, msg: Any) -> bool:
         sensor_ts_ns = int(getattr(msg, "time_usec", 0)) * 1000
-        accel = (
-            float(msg.xacc),
-            float(msg.yacc),
-            float(msg.zacc),
-        )
-        gyro = (
-            float(msg.xgyro),
-            float(msg.ygyro),
-            float(msg.zgyro),
+        accel_si_flu, gyro_si_flu = mavlink_imu_to_si_flu(
+            xacc=float(msg.xacc),
+            yacc=float(msg.yacc),
+            zacc=float(msg.zacc),
+            xgyro=float(msg.xgyro),
+            ygyro=float(msg.ygyro),
+            zgyro=float(msg.zgyro),
         )
         payload = ImuTelemetrySample(
             ts_ns=sensor_ts_ns,
-            accel_xyz=accel,
-            gyro_xyz=gyro,
+            accel_xyz=accel_si_flu,
+            gyro_xyz=gyro_si_flu,
         )
         return self._dispatch(TelemetryKind.IMU_SAMPLE, payload, msg=msg)