mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 10:11: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,143 @@
|
||||
# Contract: `StateEstimator` Protocol
|
||||
|
||||
**Owner**: c5_state (epic AZ-260 / E-C5)
|
||||
**Producer task**: AZ-381 (Protocol + DTOs + factory + composition + concrete `ISam2GraphHandle`)
|
||||
**Consumer tasks**: AZ-382 (iSAM2 + IncrementalFixedLagSmoother wiring), AZ-383 (Factor adds), AZ-384 (Marginals + outputs), AZ-385 (Source-label + spoof gate), AZ-386 (ESKF baseline), AZ-387 (Smoothed history → FDR), AZ-388 (AC-5.2 fallback), AZ-389 (Orthorectifier → C6).
|
||||
**Version**: 1.0.0
|
||||
**Status**: draft
|
||||
**Last Updated**: 2026-05-10
|
||||
**Module-layout home**: `src/gps_denied_onboard/components/c5_state/interface.py`, `src/gps_denied_onboard/components/c5_state/__init__.py`, `src/gps_denied_onboard/runtime_root/state_factory.py`
|
||||
|
||||
## Purpose
|
||||
|
||||
Defines the public interface for the C5 state estimator: fuses `VioOutput` (C1), `PoseEstimate` (C4), and FC `ImuWindow` (C8 inbound) into the posterior pose with native 6×6 covariance. Two concrete strategies linked at build time per ADR-002: `GtsamIsam2StateEstimator` (production-default; iSAM2 + IncrementalFixedLagSmoother K=10–20 per D-C5-3) and `EskfStateEstimator` (mandatory simple-baseline per IT-12 engine rule). Selected at startup via `config.state.strategy` with `BUILD_STATE_<variant>` flag gating per ADR-002.
|
||||
|
||||
C5 owns the GTSAM iSAM2 graph (ADR-003 shared substrate); C4's `OpenCVGtsamPoseEstimator` adds factors to this graph via the `ISam2GraphHandle` Protocol (defined by AZ-355 stub; concrete impl owned by AZ-381 — first child of E-C5). Single-writer thread invariant: composition root binds C5 to the same ingest thread as C4.
|
||||
|
||||
The shared `ImuPreintegrator` (AZ-276), `SE3Utils` (AZ-277), and `WgsConverter` (AZ-279) helpers are constructor-injected.
|
||||
|
||||
## Public API
|
||||
|
||||
### Protocol: `StateEstimator`
|
||||
|
||||
```python
|
||||
@runtime_checkable
|
||||
class StateEstimator(Protocol):
|
||||
def add_vio(self, vio: VioOutput) -> None: ...
|
||||
def add_pose_anchor(self, pose: PoseEstimate) -> None: ...
|
||||
def add_fc_imu(self, imu_window: ImuWindow) -> None: ...
|
||||
def current_estimate(self) -> EstimatorOutput: ...
|
||||
def smoothed_history(self, n_keyframes: int) -> list[EstimatorOutput]: ...
|
||||
def health_snapshot(self) -> EstimatorHealth: ...
|
||||
```
|
||||
|
||||
**Invariants**:
|
||||
|
||||
1. **Single-writer thread** — every `add_*` and `current_estimate`/`smoothed_history` runs on the same ingest thread; ADR-003 GTSAM substrate is non-thread-safe.
|
||||
2. **`add_*` calls are timestamp-ordered** — composition root provides a merge queue; out-of-order arrivals are rejected with `EstimatorDegradedError`.
|
||||
3. **`add_pose_anchor(pose)` MUST inspect `pose.covariance_mode`** — `JACOBIAN` mode adds the pose to the running estimate but DOES NOT add an iSAM2 factor (per AZ-361 cross-task interaction); `MARGINALS` mode triggers the full factor add + iSAM2 update.
|
||||
4. **`current_estimate()` ALWAYS returns a fresh `EstimatorOutput`** — never None on the steady-state path; `EstimatorFatalError` propagates if iSAM2 is unrecoverable.
|
||||
5. **`source_label` reflects gate state** — `SATELLITE_ANCHORED` only when the spoof-promotion gate confirms (≥10 s `STABLE_NON_SPOOFED` AND visual-consistent next anchor); else `VISUAL_PROPAGATED` or `DEAD_RECKONED`.
|
||||
6. **`smoothed_history(n)` returns up to K keyframes** — K bounded by `IncrementalFixedLagSmoother` window (D-C5-3 K=10–20); out-of-window keyframes are NOT recoverable.
|
||||
7. **`smoothed_history(n)` entries have `smoothed=True`** — distinguishes from `current_estimate()` which has `smoothed=False`.
|
||||
8. **Spoof-rejection events ALWAYS land in FDR + GCS STATUSTEXT** — never silent (R07; C5-ST-01).
|
||||
9. **AC-5.2 fallback on 3 s no-estimate** — if `current_estimate()` would raise OR the keyframe window is empty for ≥3 s, downstream C8 emits FC IMU-only.
|
||||
10. **`covariance_6x6` is always SPD** — both strategies enforce; on numerical failure raise `EstimatorFatalError`.
|
||||
|
||||
### DTOs (in `_types/state.py`)
|
||||
|
||||
```python
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class EstimatorOutput:
|
||||
frame_id: UUID
|
||||
position_wgs84: LatLonAlt
|
||||
orientation_world_T_body: Quat
|
||||
velocity_world_mps: tuple[float, float, float]
|
||||
covariance_6x6: np.ndarray
|
||||
source_label: PoseSourceLabel
|
||||
last_satellite_anchor_age_ms: int
|
||||
smoothed: bool
|
||||
emitted_at: int
|
||||
|
||||
|
||||
class IsamState(Enum):
|
||||
INIT = "init"
|
||||
TRACKING = "tracking"
|
||||
DEGRADED = "degraded"
|
||||
LOST = "lost"
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class EstimatorHealth:
|
||||
isam2_state: IsamState
|
||||
keyframe_count: int
|
||||
cov_norm_growing_for_s: float
|
||||
spoof_promotion_blocked: bool
|
||||
```
|
||||
|
||||
### Error hierarchy (in `c5_state/errors.py`)
|
||||
|
||||
```python
|
||||
class StateEstimatorError(Exception): pass
|
||||
class EstimatorDegradedError(StateEstimatorError): pass # poor convergence; emit degraded estimate
|
||||
class EstimatorFatalError(StateEstimatorError): pass # numerical failure; AC-5.2 path
|
||||
class StateEstimatorConfigError(StateEstimatorError): pass # composition-time
|
||||
```
|
||||
|
||||
### Composition-root factory
|
||||
|
||||
```python
|
||||
def build_state_estimator(
|
||||
config: AppConfig,
|
||||
imu_preintegrator: ImuPreintegrator,
|
||||
se3_utils: SE3Utils,
|
||||
wgs_converter: WgsConverter,
|
||||
fdr_client: FdrClient,
|
||||
) -> tuple[StateEstimator, ISam2GraphHandle]:
|
||||
"""Construct the configured state estimator + return the iSAM2 graph handle for C4 to inject. Selects between gtsam_isam2 / eskf via config; ADR-002 BUILD_STATE_<variant> gating."""
|
||||
...
|
||||
```
|
||||
|
||||
Strategy resolution table:
|
||||
|
||||
| `config.state.strategy` | Module path | Class |
|
||||
|---|---|---|
|
||||
| `"gtsam_isam2"` | `gps_denied_onboard.components.c5_state.gtsam_isam2_estimator` | `GtsamIsam2StateEstimator` |
|
||||
| `"eskf"` | `gps_denied_onboard.components.c5_state.eskf_baseline` | `EskfStateEstimator` |
|
||||
|
||||
Config schema additions:
|
||||
|
||||
- `config.state.strategy` (enum; required)
|
||||
- `config.state.keyframe_window_size` (int, default 15) — D-C5-3 K=10–20
|
||||
- `config.state.spoof_promotion_min_stable_s` (float, default 10.0) — AC-NEW-2
|
||||
- `config.state.spoof_promotion_visual_consistency_tol_m` (float, default 30.0) — AC-NEW-8
|
||||
- `config.state.no_estimate_fallback_s` (float, default 3.0) — AC-5.2
|
||||
|
||||
## Test expectations summarised by Invariant
|
||||
|
||||
| Invariant | Test | Assertion |
|
||||
|---|---|---|
|
||||
| 1 | Thread-binding | second binding from a different thread → `RuntimeError` |
|
||||
| 2 | Timestamp ordering | out-of-order `add_*` → `EstimatorDegradedError` |
|
||||
| 3 | `add_pose_anchor` mode dispatch | JACOBIAN: no iSAM2 factor add; MARGINALS: factor + update |
|
||||
| 4 | `current_estimate` shape | always returns fresh `EstimatorOutput` on steady state |
|
||||
| 5 | Spoof gate | label reflects gate state |
|
||||
| 6 | Smoothed history bounded | `len(smoothed_history(100))` ≤ K |
|
||||
| 7 | Smoothed flag | every `smoothed_history` entry has `smoothed=True`; `current_estimate` has `smoothed=False` |
|
||||
| 8 | Spoof-rejection logging | FDR + GCS STATUSTEXT both fire on every gate decision |
|
||||
| 9 | AC-5.2 timeout | 3 s no estimate → fallback signal emitted |
|
||||
| 10 | SPD covariance | every emitted `covariance_6x6` is SPD |
|
||||
|
||||
## Producer-task / consumer-task split
|
||||
|
||||
1. **Protocol + composition + ISam2GraphHandle concrete** (Producer; 3 pts): Protocol, DTOs, error hierarchy, factory, config schema, Concrete `ISam2GraphHandle` impl extending the AZ-355 stub.
|
||||
2. **iSAM2 + IncrementalFixedLagSmoother K wiring** (5 pts): GTSAM graph construction, K=10–20 window, key management.
|
||||
3. **Factor adds (VIO + Pose + IMU)** (5 pts): `BetweenFactorPose3`, `GenericProjectionFactorCal3DS2`, `CombinedImuFactor` per the input DTO.
|
||||
4. **Marginals + outputs** (3 pts): `current_estimate` / `smoothed_history` / `health_snapshot` body using `Marginals`.
|
||||
5. **Source-label + spoof-promotion gate** (5 pts): `SourceLabelStateMachine` + AC-NEW-2 / AC-NEW-8 logic.
|
||||
6. **ESKF baseline** (5 pts): `EskfStateEstimator` mandatory simple-baseline (IT-12 engine rule).
|
||||
7. **Smoothed history → FDR** (3 pts): writer path + AC-4.5 invariant (NOT to FC).
|
||||
8. **AC-5.2 fallback** (3 pts): 3 s no-estimate detector + signal emission.
|
||||
9. **Orthorectifier → C6 mid-flight tile** (3 pts): orthorectifier sub-path.
|
||||
|
||||
Total: 35 pts (within the XL 34–55 band).
|
||||
Reference in New Issue
Block a user