[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>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-27 22:28:40 +03:00
parent 38170b3499
commit 94d2358c8b
32 changed files with 2320 additions and 82 deletions
@@ -26,7 +26,6 @@ from gps_denied_onboard.replay_input.csv_ground_truth import (
)
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
_DERKACHI_CSV: Path = (
Path(__file__).resolve().parents[3]
/ "_docs"
@@ -163,11 +162,13 @@ def test_paired_imu_and_gps_share_clock(tmp_path: Path) -> None:
def test_gps_unit_conversion(tmp_path: Path) -> None:
# Arrange — values exercise the deg/mm/cm-s/cdeg conversions.
# Arrange — values exercise the deg/mm/cm-s/cdeg conversions on
# the GPS columns and the mG/mrad-s + FRD→FLU conversion on the
# IMU columns (AZ-918).
header = _full_header()
row = ",".join([
"0.0", "0.0",
"10", "-3", "-980", "50", "30", "-5", # IMU stays raw
"10", "-3", "-980", "50", "30", "-5", # IMU raw (mG/mrad·s⁻¹/FRD)
"50.0809634", # lat already in degrees
"36.1115442", # lon already in degrees
"141290", # alt in mm → 141.290 m
@@ -181,7 +182,7 @@ def test_gps_unit_conversion(tmp_path: Path) -> None:
# Act
gt = load_csv_ground_truth(csv)
# Assert
# Assert — GPS in SI / decimal-degrees.
fix = gt.records[0]
assert fix.lat_deg == pytest.approx(50.0809634)
assert fix.lon_deg == pytest.approx(36.1115442)
@@ -190,6 +191,21 @@ def test_gps_unit_conversion(tmp_path: Path) -> None:
assert fix.vy_m_s == pytest.approx(6.0)
assert fix.vz_m_s == pytest.approx(-0.88)
assert fix.hdg_deg == pytest.approx(350.41)
# Assert — IMU converted to m/s² + rad/s, body frame FLU.
imu = gt.imu_samples[0]
# AZ-918: CSV ships MAVLink wire format (mG/mrad/s, FRD body); the
# parser routes through mavlink_imu_to_si_flu so consumers see SI/FLU.
# FRD→FLU negates Y and Z, so a raw -3 (yacc) / -980 (zacc) become +3 / +980.
assert imu.accel_xyz == pytest.approx((
10 * 9.80665e-3,
3 * 9.80665e-3,
980 * 9.80665e-3,
))
assert imu.gyro_xyz == pytest.approx((
50 * 1.0e-3,
-30 * 1.0e-3,
5 * 1.0e-3,
))
# ---------------------------------------------------------------------