mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 23:21:12 +00:00
Oleksandr Bezdieniezhnykh
94d2358c8b
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>
104 lines
3.2 KiB
Python
104 lines
3.2 KiB
Python
"""AZ-918 — `mavlink_imu_to_si_flu` unit + frame conversion.
|
|
|
|
Pins the conversion contract so any future change to either the
|
|
constant or the body-frame transform is a deliberate, reviewed edit.
|
|
|
|
The helper sits between every MAVLink-IMU adapter and the
|
|
``nav.ImuSample`` / ``fc.ImuTelemetrySample`` boundary, so a silent
|
|
regression here would break C5 ESKF and FDR consumers simultaneously.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
|
|
import pytest
|
|
|
|
from gps_denied_onboard.helpers.imu_units import (
|
|
MG_TO_M_S2,
|
|
MRAD_S_TO_RAD_S,
|
|
mavlink_imu_to_si_flu,
|
|
)
|
|
|
|
|
|
def test_constants_match_si_definition() -> None:
|
|
# Assert
|
|
assert MG_TO_M_S2 == pytest.approx(9.80665e-3)
|
|
assert MRAD_S_TO_RAD_S == pytest.approx(1.0e-3)
|
|
|
|
|
|
def test_stationary_frd_body_z_down_becomes_flu_body_z_up_one_g() -> None:
|
|
# Arrange — Stationary, level UAV: gravity vector in MAVLink FRD
|
|
# body frame points in +Z (down), so the measured specific force
|
|
# is -gravity = -Z. Magnitude is exactly one standard gravity.
|
|
raw_z_down_mg = -1000.0
|
|
|
|
# Act
|
|
accel_si_flu, gyro_si_flu = mavlink_imu_to_si_flu(
|
|
xacc=0.0,
|
|
yacc=0.0,
|
|
zacc=raw_z_down_mg,
|
|
xgyro=0.0,
|
|
ygyro=0.0,
|
|
zgyro=0.0,
|
|
)
|
|
|
|
# Assert — In ESKF/preintegrator FLU body the same specific force
|
|
# should appear as +Z = +9.80665 m/s² (matching the C5 ESKF unit
|
|
# test's stationary vector).
|
|
assert accel_si_flu == pytest.approx((0.0, 0.0, 9.80665))
|
|
assert gyro_si_flu == pytest.approx((0.0, 0.0, 0.0))
|
|
|
|
|
|
def test_frd_to_flu_negates_y_and_z_keeps_x() -> None:
|
|
# Arrange — distinguishable components per axis.
|
|
# Act
|
|
accel_si_flu, gyro_si_flu = mavlink_imu_to_si_flu(
|
|
xacc=100.0, yacc=200.0, zacc=-300.0,
|
|
xgyro=10.0, ygyro=20.0, zgyro=-30.0,
|
|
)
|
|
|
|
# Assert — X stays positive; Y and Z flip sign per the FRD→FLU
|
|
# body-frame transform.
|
|
assert accel_si_flu == pytest.approx((
|
|
+100.0 * MG_TO_M_S2,
|
|
-200.0 * MG_TO_M_S2,
|
|
+300.0 * MG_TO_M_S2,
|
|
))
|
|
assert gyro_si_flu == pytest.approx((
|
|
+10.0 * MRAD_S_TO_RAD_S,
|
|
-20.0 * MRAD_S_TO_RAD_S,
|
|
+30.0 * MRAD_S_TO_RAD_S,
|
|
))
|
|
|
|
|
|
def test_unit_magnitudes_match_first_csv_row_of_derkachi_fixture() -> None:
|
|
# Arrange — CSV row 0 of `data_imu.csv` (Derkachi fixture). UAV is
|
|
# in slow descent at ~0.88 m/s, near level, so |accel| should be
|
|
# close to 1 g.
|
|
raw_xacc, raw_yacc, raw_zacc = 21.0, -3.0, -984.0
|
|
|
|
# Act
|
|
accel_si_flu, _ = mavlink_imu_to_si_flu(
|
|
xacc=raw_xacc, yacc=raw_yacc, zacc=raw_zacc,
|
|
xgyro=0.0, ygyro=0.0, zgyro=0.0,
|
|
)
|
|
|
|
# Assert — magnitude is within 5% of one standard gravity (the
|
|
# body is descending, not perfectly stationary, so a tight bound
|
|
# is wrong; this test pins the order-of-magnitude only).
|
|
mag = math.sqrt(sum(c * c for c in accel_si_flu))
|
|
assert 9.30 < mag < 10.30, f"|a|={mag:.3f} m/s² is not near 1 g"
|
|
|
|
|
|
def test_zero_input_returns_zero_output() -> None:
|
|
# Act
|
|
accel, gyro = mavlink_imu_to_si_flu(
|
|
xacc=0.0, yacc=0.0, zacc=0.0,
|
|
xgyro=0.0, ygyro=0.0, zgyro=0.0,
|
|
)
|
|
|
|
# Assert
|
|
assert accel == (0.0, 0.0, 0.0)
|
|
assert gyro == (0.0, 0.0, 0.0)
|