mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 22:51:13 +00:00
Decompose Step 6 snapshot: 140 task specs + contract docs
Closes out greenfield Step 6 (Decompose) for all 14 components (C1-C13 + cross-cutting helpers/replay). Covers tasks AZ-266..AZ-446 plus the _dependencies_table.md and component contract documents. State file updated to greenfield Step 7 (Implement), not_started. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,96 @@
|
||||
# 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).
|
||||
Reference in New Issue
Block a user