mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 21:41:13 +00:00
[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:
@@ -53,7 +53,6 @@ from gps_denied_onboard.config.schema import Config, RuntimeConfig
|
||||
from gps_denied_onboard.fdr_client.client import FdrClient
|
||||
from gps_denied_onboard.fdr_client.records import FdrRecord
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# Helpers — keep boilerplate out of the AC test bodies.
|
||||
|
||||
@@ -528,6 +527,19 @@ def test_ac6_low_inlier_count_emits_degraded_with_monotonic_covariance(
|
||||
calibration,
|
||||
)
|
||||
|
||||
# AZ-922 — pin cv2.recoverPose to identity rotation for frame 2 too.
|
||||
# Real cv2 on this synthetic input returns a near-180° rotation that
|
||||
# the cheirality gate would reject; this test asserts FACADE state
|
||||
# classification on inlier loss, not cv2's geometry on synthetic data.
|
||||
def _identity_recover_pose(
|
||||
*_a: Any, **_k: Any
|
||||
) -> tuple[int, np.ndarray, np.ndarray, np.ndarray]:
|
||||
R = np.eye(3, dtype=np.float64)
|
||||
t = np.array([[0.01], [0.0], [0.0]], dtype=np.float64)
|
||||
return 1, R, t, np.ones((1, 1), dtype=np.uint8)
|
||||
|
||||
monkeypatch.setattr(klt_ransac_module.cv2, "recoverPose", _identity_recover_pose)
|
||||
|
||||
# Frame 2 — first successful pose recovery (TRACKING).
|
||||
out_tracking = strategy.process_frame(
|
||||
_frame(idx=2, image=_synthetic_frame_image(seed=31, shift_x=3)),
|
||||
|
||||
Reference in New Issue
Block a user