gps-denied-onboard/_docs/03_implementation/batch_15_cycle1_report.md

Batch 15 — Cycle 1 Implementation Report

Batch: 15 of N Tasks landed: AZ-384 (GtsamIsam2StateEstimator — Marginals + output methods) Cycle: 1 Date: 2026-05-11

Scope

Task	Component	Purpose
AZ-384	C5 state estimator	Replaces the three remaining NotImplementedError placeholders on GtsamIsam2StateEstimator (current_estimate, smoothed_history, health_snapshot) with real implementations. current_estimate recovers the 6x6 Marginals covariance for the most-recently committed pose key, enforces the SPD invariant via Cholesky decomposition (Invariant 10), converts the local-ENU pose translation to WGS84 via the shared WgsConverter, derives a body→world quaternion from the iSAM2 pose, and emits a fresh EstimatorOutput(smoothed=False) (Invariant 4). smoothed_history(n) iterates the smoother's active POSE keys (filtered by GTSAM symbol char), sorts by per-key timestamp from IncrementalFixedLagSmoother.timestamps(), takes the most recent min(n, K) entries, and emits EstimatorOutput(smoothed=True) for each (Invariant 6 + 7). health_snapshot returns an O(1) accumulator read — IsamState lifecycle, pose-key count from the smoother estimate, AC-NEW-8 cov_norm_growing_for_s rolling-window counter, and the spoof-promotion gate query through the (still-pending AZ-385) state machine injection point.

Files added / modified

Modified (prod)

• src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py — replaced the three Protocol NotImplementedError bodies with real Marginals + output method bodies; introduced six new state fields on GtsamIsam2StateEstimator (_last_committed_pose_key, _enu_origin, _source_label_machine, _cov_norm_window, _isam2_state); added two public injection methods (set_enu_origin, attach_source_label_state_machine); added seven internal helpers (_record_committed_pose_key, _pose_at_key, _enu_pose_to_wgs84, _latest_velocity_or_zero, _derive_source_label, _spoof_promotion_blocked, _smoother_keyframe_count, _record_cov_norm_sample, _prune_cov_norm_window, _cov_norm_growing_for_s); wired _record_committed_pose_key into the three add_* success paths (sets the key the next current_estimate will read); added two module-level pure helpers (_quat_from_pose3, _enforce_spd); added module-level constants _COV_NORM_WINDOW_NS = 60 * 1e9 ns and _DEFAULT_ENU_ORIGIN = LatLonAlt(0, 0, 0).

Added (tests)

• tests/unit/c5_state/test_az384_marginals_outputs.py — 27 tests covering all 10 ACs. The shared _seed_prior helper plants a real prior factor + initial value + timestamp map onto the iSAM2 + smoother — the minimal scaffolding current_estimate needs (AZ-388 will own this seeding at startup once the AC-5.2 fallback lands).

Modified (tests)

• tests/unit/c5_state/test_az382_isam2_smoother_wiring.py — removed the three now-obsolete test_ac10_*_raises_named_az384 tests (current_estimate / smoothed_history / health_snapshot no longer raise NotImplementedError; they have real bodies). Replaced with a comment block pointing readers to both test_az383_factor_adds.py and test_az384_marginals_outputs.py.

Architectural notes

• _last_committed_pose_key tracking — set ONLY after a successful handle.update that committed a value for the key. The JACOBIAN path in add_pose_anchor deliberately does NOT call _record_committed_pose_key because Invariant 3 forbids adding the JACOBIAN pose to the iSAM2 graph (the running estimate consumes it downstream, but the graph stops growing under throttle). This means current_estimate reads only keys that have ACTUAL values in iSAM2 — no missing-key surprises in the steady state.
• ENU origin as injection point — set_enu_origin(LatLonAlt) is the wiring seam for AZ-385 (which will derive the origin from the first satellite-anchored pose via the spoof-promotion gate). Before the wire-up, the estimator falls back to _DEFAULT_ENU_ORIGIN = (0, 0, 0) — correct for tests, obviously not for flight; the composition root SHOULD call set_enu_origin before steady-state operation.
• GTSAM iteration via calculateEstimate().keys() + timestamps().at(key) — FixedLagSmootherKeyTimestampMap is not iterable in the pinned gtsam_unstable build (no items(), no keys(), no __iter__). The workaround: iterate _smoother.calculateEstimate().keys() (which returns the smoother's active values, bounded by the K-keyframe window), filter to 'x'-namespace pose keys via gtsam.symbolChr(key) == ord("x"), then probe per-key timestamps with ts_map.at(int(key)). Documented inline as a forward-compat note since a future GTSAM version may expose items() directly.
• Smoother estimate vs. iSAM2 estimate — current_estimate reads from _isam2.calculateEstimate() (the filter posterior — the most up-to-date best estimate). smoothed_history reads from _smoother.calculateEstimate() (the smoother posterior — past keyframes re-optimised given all later evidence). This split matches the C5 contract semantics: smoothed=True entries are genuinely smoothed; smoothed=False entries are filter-only.
• SPD invariant enforcement (Invariant 10) — _enforce_spd uses np.linalg.cholesky (same primitive iSAM2 uses internally). On LinAlgError we transition _isam2_state = LOST, log a structured c5.state.current_estimate_spd_failed record with the covariance Frobenius norm for forensics, and raise EstimatorFatalError — which triggers the AC-5.2 IMU-only fallback path in C8 once AZ-388 lands.
• AC-NEW-8 cov_norm_growing_for_s rolling window — implemented as a deque[(monotonic_ns, fro_norm)] lazy-pruned to a 60 s window on every _record_cov_norm_sample call. The "growing for" length is computed by walking the deque newest → oldest until the chain breaks (a sample whose norm is ≤ its successor). Cheap O(n) per snapshot, n bounded by the keyframe rate × 60 s.
• Source-label state machine — injection point only — attach_source_label_state_machine accepts any object exposing current_label() -> PoseSourceLabel and is_spoof_promotion_blocked() -> bool. AZ-385 owns the actual state-machine impl + transition logic. Defaults: VISUAL_PROPAGATED label and spoof_promotion_blocked = False. Exceptions raised by the injected machine are logged but downgraded to the defaults so a flaky state machine does NOT take down the estimator.
• No marginals call in health_snapshot — the contract NFR is health_snapshot p99 ≤ 5 µs. Strictly accumulator reads + a single calculateEstimate().keys() walk for the pose-key count. Verified by an explicit "no compute_marginals call" test.

Test counts

Suite	Before (B14)	After (B15)	Delta
Total passing	565	589	+24
Skipped	2	2	0
AZ-384 (new)	0	27	+27
AZ-382 (preserved)	24	21	−3 (obsolete test_ac10_*_raises_named_az384 tests removed; behaviour now lives in AZ-384 tests)

Run command: PYTHONPATH=src pytest tests/ -q → 589 passed, 2 skipped in ~19s.

Lint / type

• ruff check src/gps_denied_onboard/components/c5_state/ tests/unit/c5_state/ — clean.
• ruff format — one file reformatted, 12 already formatted.
• ReadLints on touched files — 0 errors.

Acceptance evidence

AC	Test(s)	Status
AC-1 Fresh EstimatorOutput	test_ac1_current_estimate_returns_fresh_estimator_output, test_ac1_no_committed_pose_key_raises_fatal	PASS
AC-2 SPD covariance	test_ac2_spd_invariant_holds_for_real_marginals, test_ac2_non_spd_marginals_raises_fatal	PASS
AC-3 WGS84 conversion	test_ac3_default_origin_is_equator, test_ac3_explicit_origin_round_trips, test_ac3_translated_pose_offsets_from_origin	PASS
AC-4 smoothed_history bounded by K	test_ac4_smoothed_history_bounded_by_k, test_ac4_smoothed_history_entries_have_smoothed_true, test_ac4_smoothed_history_empty_when_n_zero, test_ac4_smoothed_history_empty_before_seed	PASS
AC-5 current_estimate has smoothed=False	test_ac5_current_estimate_smoothed_false	PASS
AC-6 isam2_state lifecycle	test_ac6_isam2_state_init_before_first_estimate, test_ac6_isam2_state_tracking_after_estimate, test_ac6_isam2_state_lost_after_fatal	PASS
AC-7 keyframe_count accuracy	test_ac7_keyframe_count_initially_zero, test_ac7_keyframe_count_grows_with_seeded_keys	PASS
AC-8 cov_norm_growing_for_s	test_ac8_cov_norm_growing_zero_with_constant_norm, test_ac8_cov_norm_growing_increments_under_rising_norm, test_ac8_cov_norm_growing_resets_on_drop	PASS
AC-9 spoof_promotion_blocked via state machine	test_ac9_default_spoof_promotion_blocked_false, test_ac9_spoof_promotion_blocked_from_state_machine, test_ac9_state_machine_drives_source_label, test_ac9_default_source_label_is_visual_propagated	PASS
AC-10 last_satellite_anchor_age_ms pass-through	test_ac10_last_satellite_anchor_age_ms_passthrough, test_ac10_emitted_at_is_monotonic_ns	PASS
Defensive — cheap health_snapshot	test_health_snapshot_does_not_call_marginals	PASS

Known forward actions (not in scope this batch)

• AZ-385 (source-label + spoof-promotion gate) — owns the SourceLabelStateMachine implementation that AZ-384 only holds a reference to. Will also wire set_enu_origin to the first satellite-anchored pose.
• AZ-386 (ESKF baseline) — the mandatory simple-baseline EskfStateEstimator (IT-12 engine rule). Independent of AZ-384; shares the same EstimatorOutput / EstimatorHealth DTOs.
• AZ-387 (smoothed history → FDR) — the C13 FDR writer path. AZ-384's smoothed_history(n) is the input.
• AZ-388 (AC-5.2 fallback) — owns the startup add_vio-first-frame seeding so the full real-iSAM2 ↔ AZ-383 ↔ AZ-384 chain runs end-to-end. The AZ-384 test fixture's _seed_prior helper mirrors what AZ-388 will do at composition-root startup.
• _latest_velocity_or_zero — currently returns zeros until an IMU keyframe has committed a velocity value via add_fc_imu. The actual nonzero-velocity case will exercise once AZ-388 lands first-frame seeding for the IMU chain.