mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 21:21:13 +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>