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

Batch 13 — Cycle 1 Implementation Report

Batch: 13 of N Tasks landed: AZ-382 (GtsamIsam2StateEstimator skeleton — iSAM2 + IncrementalFixedLagSmoother wiring + ISam2GraphHandleImpl real bodies) Cycle: 1 Date: 2026-05-11

Scope

Task	Component	Purpose
AZ-382	C5 state estimator	Replaces the AZ-381 NotImplementedError skeleton for the iSAM2 graph handle with real GTSAM calls, and adds the production-default GtsamIsam2StateEstimator class that owns the gtsam.ISAM2 + gtsam_unstable.IncrementalFixedLagSmoother(K * frame_period_s) substrate. The estimator's StateEstimator Protocol methods (add_vio, add_pose_anchor, add_fc_imu, current_estimate, smoothed_history, health_snapshot) intentionally still raise NotImplementedError — their bodies are owned by AZ-383 (factor adds) and AZ-384 (marginals + outputs). The _isam2_handle.py four-method surface (add_factor, update, compute_marginals, last_anchor_age_ms), however, is fully implemented here, including defensive success/failure logging on every mutation (R05 mitigation).

Files added / modified

Added (prod)

• src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py — GtsamIsam2StateEstimator (StateEstimator) class + module-level create(...) factory + register() convenience. Constructor builds _isam2 = gtsam.ISAM2(gtsam.ISAM2Params()), _smoother = gtsam_unstable.IncrementalFixedLagSmoother(K * _FRAME_PERIOD_S), _graph = gtsam.NonlinearFactorGraph(), _values = gtsam.Values(), _key_for_frame: dict[UUID, int] = {}, _next_key_counter = 0, _last_anchor_ns = 0. Emits one DEBUG log kind="c5.state.isam2_initialised". Exposes key_for_frame(frame_id) -> int for AZ-383 key-management. The 6 Protocol methods raise NotImplementedError naming AZ-383 / AZ-384.

Modified (prod)

• src/gps_denied_onboard/components/c5_state/_isam2_handle.py — replaces all four NotImplementedError bodies with real GTSAM calls:
  • add_factor(factor) → self._estimator._graph.add(factor); success → DEBUG log c5.state.add_factor_ok with {factor_type, graph_size}; failure → ERROR log c5.state.add_factor_failed + raise EstimatorDegradedError.
  • update(graph, values, timestamps=None) → self._estimator._isam2.update(graph, values) then self._estimator._smoother.update(graph, values, timestamps_map); timestamps=None substitutes an empty gtsam_unstable.FixedLagSmootherKeyTimestampMap(). Either failure → ERROR log + raise EstimatorFatalError.
  • compute_marginals() → gtsam.Marginals(self._estimator._isam2.getFactorsUnsafe(), self._estimator._isam2.calculateEstimate()).
  • last_anchor_age_ms() → (time.monotonic_ns() - self._estimator._last_anchor_ns) // 1_000_000.

Added (tests)

• tests/unit/c5_state/test_az382_isam2_smoother_wiring.py — 27 tests covering all 10 ACs: substrate construction + DEBUG log; unique key assignment + idempotent re-lookup + 'x' namespace; smoother window equals K * frame_period_s; window-bounds validation [10, 20] at config-load (5 and 21 both rejected); add_factor success path + failure path with EstimatorDegradedError + structured failure log; update success path with iSAM2 estimate growing + smoother accepting empty timestamps map; update iSAM2 failure → EstimatorFatalError; update smoother failure → EstimatorFatalError; compute_marginals returns a gtsam.Marginals; last_anchor_age_ms returns very large initially and ~0 after anchor set; defensive logging on every mutation (success + failure paths); all six Protocol methods on the estimator raising NotImplementedError with AZ-383 / AZ-384 in the message; register() adds strategy to the factory registry; create() returns a handle bound to the returned estimator.

Modified (tests)

• tests/unit/c5_state/test_az381_state_protocol.py — removed the now-obsolete test_ac8_handle_methods_raise_named_task (the bodies no longer raise NotImplementedError; they're real). Replaced with a comment pointing to the new AZ-382 AC test file. The Protocol-conformance assertion test_ac8_handle_is_isam2_graph_handle was retained.

Architectural notes

• _FRAME_PERIOD_S = 1.0 / 3.0 — D-C5-3 fixes the keyframe processing rate at 3 Hz (so a K=15 window equals 5 s of wall time). This is a module-level constant, not a config knob — only K (the keyframe count) is operator-tunable per the contract.
• Empty FixedLagSmootherKeyTimestampMap shim — GTSAM's IncrementalFixedLagSmoother.update(...) does not have a no-timestamps overload (unlike ISAM2.update). The handle's update(...) substitutes an empty map when the caller passes timestamps=None so the C4 → C5 seam can stay timestamps: Any | None = None per the Protocol; AZ-383 will populate the map with per-key arrival timestamps.
• Defensive logging (R05) — every mutation logs structured success or failure with kind keys: c5.state.add_factor_ok, c5.state.add_factor_failed, c5.state.update_ok, c5.state.isam2_update_failed, c5.state.smoother_update_failed. The C5 contract makes this mandatory because silent factor-add failures bit the prototype.
• AC-4 enforcement — keyframe_window_size ∈ [10, 20] is enforced at C5StateConfig.__post_init__ (AZ-381 ground), so AZ-382 inherits the gate "for free"; the AZ-382 AC tests assert via C5StateConfig(keyframe_window_size=5) raising ConfigError.
• register() is opt-in, not auto-import — matches the C8 fc_factory pattern. Per-binary bootstrap modules and test fixtures call it explicitly; ADR-002 keeps the build-flag gate the single source of strategy availability.

Test counts

Suite	Before (B12)	After (B13)	Delta
Total passing	521	547	+26
Skipped	2	2	0
AZ-382 (new)	0	27	+27
AZ-381 (preserved)	20	19	−1 (obsolete handle-NIE test removed; behaviour now lives in AZ-382 tests)

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

Lint / type

• ruff check src/ tests/ — clean (1 fixable issue auto-corrected during the cycle).
• ruff format src/ tests/ — clean (1 file reformatted).
• mypy on the new modules — 0 errors. Pre-existing errors in logging/structured.py + c13_fdr/writer.py left untouched per scope discipline.

Acceptance evidence

AC	Test(s)	Status
AC-1 Construction	test_ac1_construction_initialises_substrate, test_ac1_construction_emits_debug_log	PASS
AC-2 Key-management	test_ac2_key_for_frame_assigns_unique_keys, test_ac2_key_for_frame_is_idempotent, test_ac2_keys_use_x_namespace	PASS
AC-3 Window size	test_ac3_smoother_window_is_k_times_frame_period	PASS
AC-4 Window validation	test_ac4_window_below_min_rejected_by_config, test_ac4_window_above_max_rejected_by_config	PASS
AC-5 add_factor real body	test_ac5_add_factor_appends_to_graph, test_ac5_add_factor_failure_raises_degraded_and_logs	PASS
AC-6 update real body	test_ac6_update_advances_isam2_and_smoother, test_ac6_update_with_none_timestamps_substitutes_empty_map, test_ac6_isam2_failure_raises_fatal, test_ac6_smoother_failure_raises_fatal	PASS
AC-7 compute_marginals real body	test_ac7_compute_marginals_returns_real_instance	PASS
AC-8 last_anchor_age_ms	test_ac8_last_anchor_age_ms_huge_when_no_anchor, test_ac8_last_anchor_age_ms_small_after_anchor_set	PASS
AC-9 Defensive logging	test_ac9_add_factor_emits_success_log, test_ac9_update_emits_success_log, plus failure-path log assertions in AC-5/AC-6 tests	PASS
AC-10 NotImplementedError on estimator methods	test_ac10_add_vio_raises_named_az383, test_ac10_add_pose_anchor_raises_named_az383, test_ac10_add_fc_imu_raises_named_az383, test_ac10_current_estimate_raises_named_az384, test_ac10_smoothed_history_raises_named_az384, test_ac10_health_snapshot_raises_named_az384	PASS

Caplog gotcha discovered

get_logger(name) unconditionally sets logger.setLevel(logging.NOTSET), which silently overrides any caplog.at_level(logging.DEBUG, logger=<name>) set by a test BEFORE the logger is fetched. Symptom: DEBUG records vanish from caplog.records. Workaround in this batch: tests that need to capture construction-time DEBUG logs use caplog.at_level(logging.DEBUG) at the root level. Worth a project-wide rule once we hit the same in another component — but not in this batch's scope.

Known forward actions (not in scope this batch)

• AZ-383 will populate add_vio / add_pose_anchor / add_fc_imu and start filling the FixedLagSmootherKeyTimestampMap with real per-key timestamps + recording _last_anchor_ns on every satellite-anchored pose.
• AZ-384 will populate current_estimate / smoothed_history / health_snapshot against the marginals computed by the handle.
• A per-binary bootstrap module (one per BUILD_STATE_* flag combination) will call gtsam_isam2_estimator.register() instead of relying on test fixtures.