mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-23 10:01:12 +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:
@@ -14,6 +14,7 @@ verifiable via ``sys.modules``).
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import inspect
|
||||
import os
|
||||
from typing import TYPE_CHECKING
|
||||
|
||||
@@ -26,6 +27,7 @@ if TYPE_CHECKING:
|
||||
)
|
||||
from gps_denied_onboard.components.c13_fdr import FdrClient
|
||||
from gps_denied_onboard.config.schema import Config
|
||||
from gps_denied_onboard.helpers.altitude_provider import AltitudeProvider
|
||||
|
||||
__all__ = ["build_vio_strategy"]
|
||||
|
||||
@@ -74,6 +76,7 @@ def build_vio_strategy(
|
||||
config: "Config",
|
||||
*,
|
||||
fdr_client: "FdrClient",
|
||||
altitude_provider: "AltitudeProvider | None" = None,
|
||||
) -> "VioStrategy":
|
||||
"""Construct the :class:`VioStrategy` impl selected by config.
|
||||
|
||||
@@ -82,11 +85,18 @@ def build_vio_strategy(
|
||||
:class:`StrategyNotAvailableError` BEFORE any import.
|
||||
3. Lazily imports the concrete strategy module.
|
||||
4. Constructs and returns the strategy instance, passing
|
||||
``config`` and ``fdr_client``.
|
||||
``config`` and ``fdr_client`` and (if the strategy's
|
||||
``__init__`` accepts it) ``altitude_provider`` — AZ-919.
|
||||
|
||||
Raises :class:`StrategyNotAvailableError` when the compile-time
|
||||
flag is OFF (canonical Tier-0 path) or when the concrete strategy
|
||||
module has not been built yet (AZ-332 / AZ-333 / AZ-334 pending).
|
||||
|
||||
The ``altitude_provider`` kwarg is introspected against the
|
||||
selected strategy's ``__init__`` signature so OKVIS2 / VINS-Mono
|
||||
do not need to grow a parameter they do not yet consume; the
|
||||
KLT/RANSAC strategy accepts it (per AZ-919 AC-3) and will use it
|
||||
for GSD-based scale recovery in AZ-920.
|
||||
"""
|
||||
block = _c1_config(config)
|
||||
strategy = block.strategy
|
||||
@@ -111,4 +121,7 @@ def build_vio_strategy(
|
||||
"yet (AZ-332 / AZ-333 / AZ-334 pending)."
|
||||
) from exc
|
||||
strategy_cls = getattr(module, class_name)
|
||||
return strategy_cls(config, fdr_client=fdr_client)
|
||||
kwargs: dict[str, object] = {"fdr_client": fdr_client}
|
||||
if "altitude_provider" in inspect.signature(strategy_cls).parameters:
|
||||
kwargs["altitude_provider"] = altitude_provider
|
||||
return strategy_cls(config, **kwargs)
|
||||
|
||||
Reference in New Issue
Block a user