gps-denied-onboard/_docs/02_tasks/todo/AZ-386_c5_eskf_baseline.md

C5 EskfStateEstimator — mandatory simple-baseline

Task: AZ-386_c5_eskf_baseline Name: C5 EskfStateEstimator — mandatory simple-baseline (IT-12 engine rule at C5) Description: Implement EskfStateEstimator, the mandatory simple-baseline StateEstimator per AC-2.1a engine rule applied at the state-estimator level. ESKF (Error-State Kalman Filter) over a 16-state vector (position 3 + velocity 3 + orientation 3 + accel bias 3 + gyro bias 3 + IMU dt scalar). Update on add_vio (relative-pose measurement); update on add_pose_anchor (absolute-pose measurement; respects pose.covariance_mode per AZ-383 contract — JACOBIAN does NOT skip the ESKF update because ESKF doesn't have a graph; it integrates as a normal measurement). add_fc_imu propagates the prediction step using the FC IMU window. current_estimate returns the current state + 6×6 covariance from the error-state covariance matrix (project from 16×16 down to 6×6 pose subspace). smoothed_history(n) returns recent past states from a circular buffer (NOT actually smoothed since ESKF is forward-only; entries have smoothed=False per honesty — the simple-baseline doesn't pretend to smooth). health_snapshot reports a simplified IsamState derivation. Selectable via config.state.strategy = "eskf" + BUILD_STATE_ESKF flag. Complexity: 5 points Dependencies: AZ-381 (Protocol + DTOs), AZ-276 (ImuPreintegrator consumed for IMU prediction step), AZ-277 (SE3Utils), AZ-279 (WgsConverter), AZ-263, AZ-269, AZ-266, AZ-272 Component: c5_state (epic AZ-260 / E-C5) Tracker: AZ-386 Epic: AZ-260 (E-C5)

Document Dependencies

• _docs/02_document/contracts/c5_state/state_estimator_protocol.md — Protocol surface; EstimatorOutput shape.
• _docs/02_document/components/07_c5_state/description.md — § 1 (mandatory simple-baseline; AC-2.1a engine rule applied at C5).
• _docs/02_document/architecture.md — AC-2.1a engine rule semantics.

Problem

Without this task, IT-12 (engine rule comparative study at the state-estimator level) has no baseline to compare iSAM2 against. ADR-002 also requires the mandatory simple-baseline to exist as a real binary that can be selected at runtime; without it, the IT-12 verdict is unprovable.

Outcome

• src/gps_denied_onboard/components/c5_state/eskf_baseline.py defining:
  • EskfStateEstimator class implementing StateEstimator Protocol.
  • 16-state error-state Kalman filter implementation (NumPy-based; no GTSAM).
  • All 6 Protocol methods implemented per the description above.
  • Module-level create(config, imu_preintegrator, se3_utils, wgs_converter, fdr_client) -> StateEstimator.
• BUILD_STATE_ESKF build flag wiring (ON in research; OFF in airborne-default per ADR-002 build-time exclusion).
• Honest reporting: smoothed_history entries flagged smoothed=False (because ESKF doesn't smooth); health_snapshot.isam2_state mapped to a simplified ESKF state model (TRACKING when filter is healthy; DEGRADED when innovation magnitude exceeds threshold; LOST on filter divergence).
• _last_anchor_ns tracked for last_satellite_anchor_age_ms (same semantics as the iSAM2 estimator).
• Unit tests: ESKF prediction step accuracy on synthetic IMU sequence; relative-pose update; absolute-pose update; convergence on synthetic data; SPD covariance; configurable measurement noise; honest smoothed=False reporting.

Scope

Included

• EskfStateEstimator impl.
• 16-state error-state Kalman filter NumPy impl.
• All 6 Protocol methods.
• BUILD_STATE_ESKF flag wiring.
• SPD-invariant defensive check on every emitted covariance.
• Unit tests + parametrised configuration tests.

Excluded

• iSAM2 estimator — already AZ-382.
• Source-label state machine — owned by AZ-385 (this task uses the same injection point).
• Smoothed history → FDR — owned by AZ-387.
• AC-5.2 fallback — owned by AZ-388.

Acceptance Criteria

AC-1: Protocol conformance — passes isinstance against StateEstimator.

AC-2: ESKF prediction step accuracy — on synthetic IMU sequence with known ground-truth trajectory, position drift < 1 m over 5 s.

AC-3: Relative-pose update — add_vio updates the state with the VIO measurement; covariance shrinks on consistent measurements.

AC-4: Absolute-pose update — add_pose_anchor updates the state with the absolute measurement regardless of covariance_mode (no skip; ESKF doesn't have a graph).

AC-5: SPD covariance — every emitted EstimatorOutput.covariance_6x6 is SPD; non-SPD raises EstimatorFatalError.

AC-6: smoothed_history(n) honest smoothed=False — every entry has smoothed=False (ESKF doesn't smooth).

AC-7: BUILD_STATE_ESKF=OFF rejection — factory rejection via StateEstimatorConfigError per AZ-381 Protocol task contract.

AC-8: Source-label state machine integration — same injection point as iSAM2 estimator (AZ-385 wires both).

AC-9: Filter divergence handling — when innovation exceeds 10× the measurement-covariance norm, raise EstimatorFatalError; AC-5.2 fallback fires downstream.

AC-10: Composition wiring — config.state.strategy = "eskf" + BUILD_STATE_ESKF=ON → factory returns EskfStateEstimator instance.

Non-Functional Requirements

• add_vio p99 ≤ 5 ms.
• add_pose_anchor p99 ≤ 10 ms.
• current_estimate p99 ≤ 5 ms.
• Memory ≤ 5 MB resident (ESKF state vector + buffers).

Constraints

• NumPy-based; no GTSAM dependency.
• 16-state vector dimension is fixed.
• Single-writer thread.
• SPD-invariant defensive check is mandatory.
• Honest reporting: smoothed=False (no pretending to smooth).

Risks & Mitigation

• Risk: ESKF impl bugs — comprehensive unit tests with synthetic ground truth (AC-2..AC-4).
• Risk: Filter divergence under spoofed measurements — AC-9 detects via innovation magnitude.

Runtime Completeness

• Named capability: ESKF mandatory simple-baseline StateEstimator.
• Production code: real NumPy ESKF impl, real prediction + update steps, real SPD-invariant defensive check.
• Unacceptable substitutes: a wrapped GTSAM ISAM2 (defeats the simple-baseline contract); smoothed=True lies (defeats honesty).