mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 08:41:12 +00:00
Oleksandr Bezdieniezhnykh
b3ad94c155
Replaces the last three NotImplementedError placeholders on GtsamIsam2StateEstimator with real Marginals + output methods: - current_estimate(): recovers the 6x6 Marginals covariance for the most-recently committed pose key, enforces the SPD invariant via np.linalg.cholesky (Invariant 10), converts the local-ENU pose translation to WGS84 via the shared WgsConverter, derives a body->world quaternion, and emits a fresh EstimatorOutput (smoothed=False, Invariant 4). On SPD failure transitions isam2_state -> LOST and raises EstimatorFatalError (AC-5.2 path). - smoothed_history(n): iterates the smoother's active POSE keys via _smoother.calculateEstimate().keys() (filtered by GTSAM symbol char) and the smoother timestamps via ts_map.at(key) - workaround for the pinned gtsam_unstable build's non-iterable FixedLagSmootherKeyTimestampMap. Bounded by K (Invariant 6); every entry has smoothed=True (Invariant 7). - health_snapshot(): cheap O(1) accumulator read; reports IsamState lifecycle, pose-key count, AC-NEW-8 cov_norm_growing_for_s rolling 60s deque-backed counter, and spoof_promotion_blocked via the AZ-385 state machine injection point. Adds two public injection points for AZ-385/composition root: set_enu_origin(LatLonAlt) and attach_source_label_state_machine(machine). Defaults: (0, 0, 0) ENU origin, VISUAL_PROPAGATED source label, spoof_promotion_blocked=False. Wires _record_committed_pose_key into the three add_* success paths so current_estimate only reads keys that have real values in iSAM2. The JACOBIAN path in add_pose_anchor deliberately skips this call - Invariant 3 keeps the JACOBIAN pose out of the iSAM2 graph. Tests: +27 in tests/unit/c5_state/test_az384_marginals_outputs.py covering all 10 ACs. Three obsolete AZ-382 tests (test_ac10_*_raises_named_az384) removed. Full suite: 589 passed, 2 skipped. Co-authored-by: Cursor <cursoragent@cursor.com>
gps-denied-onboard
Companion onboard system for GPS-denied UAV navigation. Detailed design and architecture documentation lives under _docs/.
Quick links
- Problem statement:
_docs/00_problem/problem.md - Architecture:
_docs/02_document/architecture.md - Module layout (file ownership):
_docs/02_document/module-layout.md - Component docs:
_docs/02_document/components/ - Test specs:
_docs/02_document/tests/ - Deployment:
_docs/02_document/deployment/
Local development
python -m venv .venv && source .venv/bin/activate
pip install -e ".[dev]"
pytest -q tests/unit/
For full Tier-1 integration via Docker, see _docs/02_document/deployment/containerization.md.
Build matrix
Four binaries built from this codebase: airborne, research, operator-tooling, replay-cli. CMake BUILD_* flags gate component inclusion per binary — see cmake/build_options.cmake and _docs/02_document/module-layout.md § Build-Time Exclusion Map.