[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
@@ -86,8 +86,19 @@ def test_ac1_ap_raw_imu_decode() -> None:
frame = received[0]
assert frame.kind is TelemetryKind.IMU_SAMPLE
assert isinstance(frame.payload, ImuTelemetrySample)
assert frame.payload.accel_xyz == (10.0, 20.0, -981.0)
assert frame.payload.gyro_xyz == (1.0, 2.0, 3.0)
# AZ-918: raw MAVLink (xacc=10, yacc=20, zacc=-981) mG FRD →
# SI/FLU m/s² via mavlink_imu_to_si_flu (negate Y, negate Z, *9.80665e-3).
# zacc raw is -981 mG; FRD→FLU negates Z, giving +981 mG = +9.62 m/s².
assert frame.payload.accel_xyz == pytest.approx((
10 * 9.80665e-3,
-20 * 9.80665e-3,
981 * 9.80665e-3,
))
assert frame.payload.gyro_xyz == pytest.approx((
1 * 1.0e-3,
-2 * 1.0e-3,
-3 * 1.0e-3,
))
# received_at is monotonic_ns at decode boundary (positive, non-zero)
assert frame.received_at > 0
@@ -621,7 +632,12 @@ def test_ac10_corrupt_frame_does_not_kill_decoder(
assert len(err_logs) == 1
imu_frames = [f for f in received if f.kind is TelemetryKind.IMU_SAMPLE]
assert len(imu_frames) == 1
assert imu_frames[0].payload.accel_xyz == (1.0, 2.0, 3.0)
# AZ-918: raw MAVLink (xacc=1, yacc=2, zacc=3) mG FRD → SI/FLU.
assert imu_frames[0].payload.accel_xyz == pytest.approx((
1 * 9.80665e-3,
-2 * 9.80665e-3,
-3 * 9.80665e-3,
))
# ----------------------------------------------------------------------
@@ -12,7 +12,6 @@ faked via :class:`_FakeTlog` so tests run without the real C extension.
from __future__ import annotations
import os
import time
from pathlib import Path
from types import SimpleNamespace
@@ -50,7 +49,6 @@ from gps_denied_onboard.components.c8_fc_adapter.tlog_replay_adapter import (
TlogReplayFcAdapter,
)
# ----------------------------------------------------------------------
# Fixtures + helpers
@@ -379,7 +377,19 @@ def test_ac2_ap_dialect_frame_mapping_in_tlog_order(
TelemetryKind.MAV_STATE,
]
assert isinstance(received[0].payload, ImuTelemetrySample)
assert received[0].payload.accel_xyz == (10.0, 20.0, -981.0)
# AZ-918: raw MAVLink (xacc=10, yacc=20, zacc=-981) mG FRD →
# SI/FLU m/s² via mavlink_imu_to_si_flu (negate Y, negate Z, *9.80665e-3).
# zacc raw is -981 mG; FRD→FLU negates Z, giving +981 mG = +9.62 m/s².
assert received[0].payload.accel_xyz == pytest.approx((
10 * 9.80665e-3,
-20 * 9.80665e-3,
981 * 9.80665e-3,
))
assert received[0].payload.gyro_xyz == pytest.approx((
1 * 1.0e-3,
-2 * 1.0e-3,
-3 * 1.0e-3,
))
assert isinstance(received[1].payload, AttitudeSample)
assert received[1].payload.yaw_rad == pytest.approx(1.5)
assert isinstance(received[2].payload, GpsHealth)