mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-23 08:06:37 +00:00
docs(01-eskf-core): create phase plan — 3 plans in 2 waves
This commit is contained in:
Yuzviak
@@ -0,0 +1,355 @@
|
||||
---
|
||||
phase: 01-eskf-core
|
||||
plan: 01
|
||||
type: execute
|
||||
wave: 1
|
||||
depends_on: []
|
||||
files_modified:
|
||||
- src/gps_denied/core/eskf.py
|
||||
- src/gps_denied/schemas/eskf.py
|
||||
autonomous: true
|
||||
requirements:
|
||||
- ESKF-01
|
||||
- ESKF-02
|
||||
- ESKF-03
|
||||
- ESKF-04
|
||||
- ESKF-05
|
||||
|
||||
must_haves:
|
||||
truths:
|
||||
- "ESKF propagates nominal state (position, velocity, quaternion, biases) from IMU inputs and covariance grows between measurement updates"
|
||||
- "VO measurement update reduces position uncertainty and innovation is within expected bounds"
|
||||
- "Satellite measurement update corrects absolute position and covariance tightens to satellite noise level"
|
||||
- "Confidence tier outputs HIGH when satellite correction is recent and covariance small, MEDIUM on VO-only, LOW on IMU-only"
|
||||
- "ESKF initializes from a GPS position with high-uncertainty covariance"
|
||||
artifacts:
|
||||
- path: "src/gps_denied/core/eskf.py"
|
||||
provides: "15-state ESKF implementation"
|
||||
min_lines: 250
|
||||
exports: ["ESKF"]
|
||||
- path: "src/gps_denied/schemas/eskf.py"
|
||||
provides: "ESKF data contracts"
|
||||
exports: ["ESKFState", "ESKFConfig", "ConfidenceTier", "IMUMeasurement"]
|
||||
key_links:
|
||||
- from: "src/gps_denied/core/eskf.py"
|
||||
to: "src/gps_denied/schemas/eskf.py"
|
||||
via: "import ESKFState, ESKFConfig, ConfidenceTier, IMUMeasurement"
|
||||
pattern: "from gps_denied\\.schemas\\.eskf import"
|
||||
- from: "src/gps_denied/core/eskf.py"
|
||||
to: "numpy"
|
||||
via: "matrix math for F, Q, P, K, H"
|
||||
pattern: "import numpy as np"
|
||||
---
|
||||
|
||||
<objective>
|
||||
Implement the 15-state Error-State Kalman Filter (ESKF) that fuses IMU, VO, and satellite measurements and outputs confidence-tiered position estimates.
|
||||
|
||||
Purpose: The ESKF is the central position estimator — without it, no GPS_INPUT messages can be generated, no IMU prediction can fill inter-frame gaps, and no confidence tier can drive fix_type.
|
||||
|
||||
Output: `src/gps_denied/core/eskf.py` (ESKF class) and `src/gps_denied/schemas/eskf.py` (data contracts).
|
||||
</objective>
|
||||
|
||||
<execution_context>
|
||||
@$HOME/.claude/get-shit-done/workflows/execute-plan.md
|
||||
@$HOME/.claude/get-shit-done/templates/summary.md
|
||||
</execution_context>
|
||||
|
||||
<context>
|
||||
@.planning/PROJECT.md
|
||||
@.planning/ROADMAP.md
|
||||
@.planning/STATE.md
|
||||
@.planning/codebase/CONVENTIONS.md
|
||||
@_docs/01_solution/solution.md (lines 127-253 — ESKF spec, confidence tiers, GPS_INPUT fields)
|
||||
@src/gps_denied/schemas/__init__.py
|
||||
@src/gps_denied/schemas/vo.py
|
||||
@src/gps_denied/core/coordinates.py
|
||||
</context>
|
||||
|
||||
<tasks>
|
||||
|
||||
<task type="auto">
|
||||
<name>Task 1: Create ESKF schema contracts</name>
|
||||
<files>src/gps_denied/schemas/eskf.py</files>
|
||||
<read_first>
|
||||
- src/gps_denied/schemas/__init__.py (GPSPoint, CameraParameters — the pattern for Pydantic schemas in this project)
|
||||
- src/gps_denied/schemas/vo.py (RelativePose — the VO output that ESKF consumes; note model_config for numpy)
|
||||
- _docs/01_solution/solution.md (lines 127-253 for state vector, confidence tiers, GPS_INPUT mapping)
|
||||
</read_first>
|
||||
<action>
|
||||
Create `src/gps_denied/schemas/eskf.py` with these exact types:
|
||||
|
||||
```python
|
||||
"""Error-State Kalman Filter schemas."""
|
||||
|
||||
from enum import Enum
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
from pydantic import BaseModel, Field
|
||||
|
||||
|
||||
class ConfidenceTier(str, Enum):
|
||||
"""ESKF confidence tier for GPS_INPUT fix_type mapping."""
|
||||
HIGH = "HIGH" # Satellite match <30s ago, covariance < 400 m^2
|
||||
MEDIUM = "MEDIUM" # VO tracking OK, no recent satellite match
|
||||
LOW = "LOW" # IMU-only, cuVSLAM lost
|
||||
FAILED = "FAILED" # 3+ consecutive total failures
|
||||
|
||||
|
||||
class IMUMeasurement(BaseModel):
|
||||
"""Single IMU reading from flight controller."""
|
||||
model_config = {"arbitrary_types_allowed": True}
|
||||
|
||||
accel: np.ndarray # (3,) m/s^2 in body frame
|
||||
gyro: np.ndarray # (3,) rad/s in body frame
|
||||
timestamp: float # seconds since epoch
|
||||
|
||||
|
||||
class ESKFConfig(BaseModel):
|
||||
"""ESKF tuning parameters."""
|
||||
|
||||
# Process noise (from IMU datasheet — ICM-42688-P)
|
||||
accel_noise_density: float = 6.86e-4 # 70 ug/sqrt(Hz) -> m/s^2/sqrt(Hz)
|
||||
gyro_noise_density: float = 5.24e-5 # 3.0e-3 deg/s/sqrt(Hz) -> rad/s/sqrt(Hz)
|
||||
accel_random_walk: float = 2.0e-3 # m/s^3/sqrt(Hz)
|
||||
gyro_random_walk: float = 8.73e-7 # 5.0e-5 deg/s^2/sqrt(Hz) -> rad/s^2/sqrt(Hz)
|
||||
|
||||
# VO measurement noise
|
||||
vo_position_noise: float = 0.3 # meters (cuVSLAM at 600m altitude)
|
||||
|
||||
# Satellite measurement noise range
|
||||
sat_noise_min: float = 5.0 # meters (high-confidence RANSAC)
|
||||
sat_noise_max: float = 20.0 # meters (low-confidence RANSAC)
|
||||
|
||||
# Confidence tier thresholds
|
||||
satellite_max_age: float = 30.0 # seconds
|
||||
covariance_high_threshold: float = 400.0 # m^2 (trace of position covariance)
|
||||
|
||||
# Initial covariance diagonals
|
||||
init_pos_var: float = 100.0 # m^2
|
||||
init_vel_var: float = 1.0 # (m/s)^2
|
||||
init_att_var: float = 0.01 # rad^2
|
||||
init_accel_bias_var: float = 0.01 # (m/s^2)^2
|
||||
init_gyro_bias_var: float = 1e-6 # (rad/s)^2
|
||||
|
||||
|
||||
class ESKFState(BaseModel):
|
||||
"""Full ESKF nominal state snapshot."""
|
||||
model_config = {"arbitrary_types_allowed": True}
|
||||
|
||||
position: np.ndarray # (3,) NED meters from origin
|
||||
velocity: np.ndarray # (3,) NED m/s
|
||||
quaternion: np.ndarray # (4,) [w, x, y, z] body-to-NED
|
||||
accel_bias: np.ndarray # (3,) m/s^2
|
||||
gyro_bias: np.ndarray # (3,) rad/s
|
||||
covariance: np.ndarray # (15, 15)
|
||||
timestamp: float # seconds since epoch
|
||||
confidence: ConfidenceTier
|
||||
last_satellite_time: Optional[float] = None
|
||||
last_vo_time: Optional[float] = None
|
||||
```
|
||||
|
||||
Also update `src/gps_denied/schemas/__init__.py` to add re-exports:
|
||||
|
||||
Add these imports at the end of the file:
|
||||
```python
|
||||
from gps_denied.schemas.eskf import ConfidenceTier, ESKFConfig, ESKFState, IMUMeasurement
|
||||
```
|
||||
|
||||
Per ESKF-01 (state vector), ESKF-04 (initialization config), ESKF-05 (confidence tiers).
|
||||
</action>
|
||||
<verify>
|
||||
<automated>python -c "from gps_denied.schemas.eskf import ESKFState, ESKFConfig, ConfidenceTier, IMUMeasurement; print('OK')"</automated>
|
||||
</verify>
|
||||
<acceptance_criteria>
|
||||
- src/gps_denied/schemas/eskf.py contains `class ConfidenceTier(str, Enum)` with members HIGH, MEDIUM, LOW, FAILED
|
||||
- src/gps_denied/schemas/eskf.py contains `class IMUMeasurement(BaseModel)` with fields accel, gyro, timestamp
|
||||
- src/gps_denied/schemas/eskf.py contains `class ESKFConfig(BaseModel)` with fields accel_noise_density, gyro_noise_density, satellite_max_age, covariance_high_threshold
|
||||
- src/gps_denied/schemas/eskf.py contains `class ESKFState(BaseModel)` with fields position, velocity, quaternion, accel_bias, gyro_bias, covariance, timestamp, confidence
|
||||
- ESKFState.quaternion comment documents [w, x, y, z] convention
|
||||
- ESKFState.position comment documents NED meters from origin
|
||||
- `python -c "from gps_denied.schemas.eskf import ESKFState, ESKFConfig, ConfidenceTier, IMUMeasurement"` exits 0
|
||||
</acceptance_criteria>
|
||||
<done>All four ESKF schema classes importable, fields match solution.md spec exactly, ConfidenceTier has all four tiers</done>
|
||||
</task>
|
||||
|
||||
<task type="auto" tdd="true">
|
||||
<name>Task 2: Implement ESKF core algorithm</name>
|
||||
<files>src/gps_denied/core/eskf.py</files>
|
||||
<read_first>
|
||||
- src/gps_denied/schemas/eskf.py (just created — the contracts this implementation uses)
|
||||
- src/gps_denied/core/coordinates.py (CoordinateTransformer — used for NED/WGS84 conversion, gps_to_enu, enu_to_gps methods)
|
||||
- src/gps_denied/schemas/vo.py (RelativePose — the VO measurement input; fields: translation, rotation, confidence, scale_ambiguous)
|
||||
- src/gps_denied/schemas/__init__.py (GPSPoint — used for initialization from GPS position)
|
||||
- _docs/01_solution/solution.md (lines 127-178 for ESKF math, lines 244-253 for confidence tiers)
|
||||
- src/gps_denied/core/vo.py (to understand interface pattern: ABC + concrete class in same module)
|
||||
</read_first>
|
||||
<behavior>
|
||||
- Test: ESKF initialized with GPS position has state.position == gps_to_enu(gps), quaternion == [1,0,0,0], covariance diagonal matches config init vars
|
||||
- Test: After predict() with zero IMU and dt=0.01, position unchanged, covariance[0:3,0:3] trace increases
|
||||
- Test: After predict() with accel=[0,0,-9.81] (gravity in body NED) and dt=0.1, velocity change is near zero (gravity compensated)
|
||||
- Test: VO update with zero innovation leaves state unchanged, covariance does not increase
|
||||
- Test: VO update with 1m innovation in north reduces position covariance and shifts position toward measurement
|
||||
- Test: Satellite update with known position reduces position covariance below pre-update level
|
||||
- Test: Confidence is HIGH when last_satellite_time < 30s ago and position covariance trace < 400
|
||||
- Test: Confidence is MEDIUM when last_vo_time is recent but no satellite
|
||||
- Test: Confidence is LOW when only IMU (no VO, no satellite)
|
||||
- Test: Confidence is FAILED when get_confidence called with consecutive_failures >= 3
|
||||
</behavior>
|
||||
<action>
|
||||
Create `src/gps_denied/core/eskf.py` implementing the full 15-state Error-State Kalman Filter.
|
||||
|
||||
Module docstring: `"""Error-State Kalman Filter (ESKF) — 15-state sensor fusion (Component F17)."""`
|
||||
|
||||
Class `ESKF`:
|
||||
|
||||
**Constructor** `__init__(self, config: ESKFConfig | None = None)`:
|
||||
- Store config (default ESKFConfig())
|
||||
- Initialize: `_initialized: bool = False`, `_nominal_state: dict | None = None`, `_P: np.ndarray | None = None` (15x15 covariance), `_last_satellite_time: float | None = None`, `_last_vo_time: float | None = None`, `_last_timestamp: float | None = None`
|
||||
|
||||
**`initialize(self, position_ned: np.ndarray, timestamp: float, velocity: np.ndarray | None = None, quaternion: np.ndarray | None = None) -> None`**:
|
||||
- Set nominal state: position=position_ned, velocity=velocity or zeros(3), quaternion=quaternion or [1,0,0,0] (identity), accel_bias=zeros(3), gyro_bias=zeros(3)
|
||||
- Initialize P as 15x15 diagonal: [init_pos_var]*3 + [init_vel_var]*3 + [init_att_var]*3 + [init_accel_bias_var]*3 + [init_gyro_bias_var]*3
|
||||
- Set _initialized=True, _last_timestamp=timestamp
|
||||
|
||||
**`initialize_from_gps(self, gps: GPSPoint, altitude: float, timestamp: float, coord_transformer: CoordinateTransformer, flight_id: str) -> None`**:
|
||||
- Convert GPS to NED using coord_transformer.gps_to_enu(flight_id, gps) (note: existing code uses ENU, treat as [East, North, Up] -> convert to NED: [North, East, -Up] — BUT the existing codebase uses ENU throughout, so store as ENU to match CoordinateTransformer convention)
|
||||
- IMPORTANT: The existing CoordinateTransformer uses ENU (East, North, Up). Store position in ENU to maintain compatibility. Document this in a comment.
|
||||
- Call self.initialize(position_enu, timestamp)
|
||||
|
||||
Per ESKF-04: initialization from GLOBAL_POSITION_INT.
|
||||
|
||||
**`predict(self, imu: IMUMeasurement) -> None`** (per ESKF-01):
|
||||
- Compute dt = imu.timestamp - self._last_timestamp. If dt <= 0 or dt > 1.0, skip.
|
||||
- Extract corrected measurements: a_corrected = imu.accel - self._nominal_state["accel_bias"], w_corrected = imu.gyro - self._nominal_state["gyro_bias"]
|
||||
- Get rotation matrix R from quaternion: R = _quat_to_rotation_matrix(self._nominal_state["quaternion"])
|
||||
- Gravity vector g = np.array([0, 0, -9.81]) (in ENU, up is positive, gravity points down)
|
||||
- Propagate nominal state:
|
||||
- position += velocity * dt
|
||||
- velocity += (R @ a_corrected + g) * dt
|
||||
- quaternion = _quat_multiply(quaternion, _exp_quaternion(w_corrected * dt))
|
||||
- quaternion = quaternion / np.linalg.norm(quaternion) (normalize)
|
||||
- Biases unchanged (random walk modeled in Q)
|
||||
- Build 15x15 state transition matrix F:
|
||||
- F = I_15
|
||||
- F[0:3, 3:6] = I_3 * dt (position depends on velocity)
|
||||
- F[3:6, 6:9] = -R @ _skew_symmetric(a_corrected) * dt (velocity depends on attitude error)
|
||||
- F[3:6, 9:12] = -R * dt (velocity depends on accel bias)
|
||||
- F[6:9, 12:15] = -I_3 * dt (attitude depends on gyro bias)
|
||||
- Build 15x15 process noise Q:
|
||||
- Q = zeros(15, 15)
|
||||
- Q[3:6, 3:6] = (config.accel_noise_density**2) * dt * I_3 (velocity noise from accel)
|
||||
- Q[6:9, 6:9] = (config.gyro_noise_density**2) * dt * I_3 (attitude noise from gyro)
|
||||
- Q[9:12, 9:12] = (config.accel_random_walk**2) * dt * I_3 (accel bias random walk)
|
||||
- Q[12:15, 12:15] = (config.gyro_random_walk**2) * dt * I_3 (gyro bias random walk)
|
||||
- Propagate covariance: P = F @ P @ F.T + Q
|
||||
- Update _last_timestamp
|
||||
|
||||
**`update_vo(self, relative_position: np.ndarray, dt_vo: float) -> np.ndarray`** (per ESKF-02):
|
||||
- Predicted relative position = self._nominal_state["velocity"] * dt_vo
|
||||
- Innovation z = relative_position - predicted_relative_position (3,)
|
||||
- H_vo = zeros(3, 15); H_vo[0:3, 3:6] = I_3 * dt_vo (maps velocity error to relative position)
|
||||
- Alternative simpler H: H_vo[0:3, 0:3] = I_3 (direct position observation of relative displacement)
|
||||
- Use the simpler form: H_vo observes the position error directly since relative_position is integrated
|
||||
- R_vo = config.vo_position_noise**2 * I_3
|
||||
- S = H_vo @ P @ H_vo.T + R_vo
|
||||
- K = P @ H_vo.T @ np.linalg.inv(S)
|
||||
- delta_x = K @ z (15,)
|
||||
- Apply error-state correction to nominal state:
|
||||
- position += delta_x[0:3]
|
||||
- velocity += delta_x[3:6]
|
||||
- quaternion = _quat_multiply(quaternion, _exp_quaternion(delta_x[6:9]))
|
||||
- quaternion /= np.linalg.norm(quaternion)
|
||||
- accel_bias += delta_x[9:12]
|
||||
- gyro_bias += delta_x[12:15]
|
||||
- Reset error state: P = (I_15 - K @ H_vo) @ P
|
||||
- Update _last_vo_time = self._last_timestamp
|
||||
- Return innovation z
|
||||
|
||||
**`update_satellite(self, position_enu: np.ndarray, noise_meters: float) -> np.ndarray`** (per ESKF-03):
|
||||
- Innovation z = position_enu - self._nominal_state["position"] (3,)
|
||||
- H_sat = zeros(3, 15); H_sat[0:3, 0:3] = I_3 (directly observes position)
|
||||
- R_sat = noise_meters**2 * I_3
|
||||
- S = H_sat @ P @ H_sat.T + R_sat
|
||||
- K = P @ H_sat.T @ np.linalg.inv(S)
|
||||
- delta_x = K @ z (15,)
|
||||
- Apply same error-state correction as update_vo
|
||||
- P = (I_15 - K @ H_sat) @ P
|
||||
- Update _last_satellite_time = self._last_timestamp
|
||||
- Return innovation z
|
||||
|
||||
**`get_confidence(self, consecutive_failures: int = 0) -> ConfidenceTier`** (per ESKF-05):
|
||||
- If consecutive_failures >= 3: return FAILED
|
||||
- current_time = self._last_timestamp
|
||||
- pos_cov_trace = np.trace(self._P[0:3, 0:3])
|
||||
- If _last_satellite_time is not None and (current_time - _last_satellite_time) < config.satellite_max_age and pos_cov_trace < config.covariance_high_threshold: return HIGH
|
||||
- If _last_vo_time is not None and (current_time - _last_vo_time) < 5.0: return MEDIUM
|
||||
- return LOW
|
||||
|
||||
**`get_state(self) -> ESKFState`**:
|
||||
- Return ESKFState with all current nominal state fields, covariance=self._P.copy(), timestamp=self._last_timestamp, confidence=self.get_confidence()
|
||||
|
||||
**`@property initialized(self) -> bool`**: return self._initialized
|
||||
|
||||
**Helper functions** (module-level, private):
|
||||
|
||||
- `_quat_to_rotation_matrix(q: np.ndarray) -> np.ndarray`: Convert [w,x,y,z] quaternion to 3x3 rotation matrix using standard formula: R = (2w^2-1)I + 2(v*v^T) + 2w*skew(v) where v=[x,y,z]
|
||||
- `_quat_multiply(q1: np.ndarray, q2: np.ndarray) -> np.ndarray`: Hamilton product [w,x,y,z] convention
|
||||
- `_exp_quaternion(theta: np.ndarray) -> np.ndarray`: Convert rotation vector (3,) to quaternion: angle = ||theta||, if angle < 1e-10 return [1,0,0,0], else return [cos(angle/2), sin(angle/2)*theta/angle]
|
||||
- `_skew_symmetric(v: np.ndarray) -> np.ndarray`: 3x3 skew-symmetric matrix from 3-vector
|
||||
|
||||
All functions use numpy only. No scipy dependency needed.
|
||||
</action>
|
||||
<verify>
|
||||
<automated>python -c "
|
||||
import numpy as np
|
||||
from gps_denied.core.eskf import ESKF
|
||||
from gps_denied.schemas.eskf import ESKFConfig, IMUMeasurement
|
||||
eskf = ESKF()
|
||||
eskf.initialize(np.zeros(3), 0.0)
|
||||
assert eskf.initialized
|
||||
imu = IMUMeasurement(accel=np.array([0., 0., 9.81]), gyro=np.zeros(3), timestamp=0.01)
|
||||
eskf.predict(imu)
|
||||
state = eskf.get_state()
|
||||
assert state.position is not None
|
||||
assert state.covariance.shape == (15, 15)
|
||||
print('ESKF basic test OK')
|
||||
"</automated>
|
||||
</verify>
|
||||
<acceptance_criteria>
|
||||
- src/gps_denied/core/eskf.py contains `class ESKF:`
|
||||
- src/gps_denied/core/eskf.py contains `def predict(self, imu: IMUMeasurement)` with F matrix construction and P = F @ P @ F.T + Q
|
||||
- src/gps_denied/core/eskf.py contains `def update_vo(self, relative_position: np.ndarray` with Kalman gain K = P @ H.T @ inv(S)
|
||||
- src/gps_denied/core/eskf.py contains `def update_satellite(self, position_enu: np.ndarray` with H_sat[0:3, 0:3] = I_3
|
||||
- src/gps_denied/core/eskf.py contains `def get_confidence(self` returning ConfidenceTier with checks for satellite_max_age and covariance_high_threshold
|
||||
- src/gps_denied/core/eskf.py contains `def initialize_from_gps(self, gps: GPSPoint`
|
||||
- src/gps_denied/core/eskf.py contains `def _quat_to_rotation_matrix(`
|
||||
- src/gps_denied/core/eskf.py contains `def _skew_symmetric(`
|
||||
- Covariance matrix P is 15x15
|
||||
- Error-state vector is 15-dimensional: delta_x[0:3] position, [3:6] velocity, [6:9] attitude, [9:12] accel_bias, [12:15] gyro_bias
|
||||
- `python -c "from gps_denied.core.eskf import ESKF"` exits 0
|
||||
</acceptance_criteria>
|
||||
<done>ESKF class fully implements: IMU prediction with F/Q matrices and bias propagation (ESKF-01), VO measurement update with Kalman gain (ESKF-02), satellite measurement update with absolute position correction (ESKF-03), GPS initialization with high-uncertainty covariance (ESKF-04), and confidence tier computation (ESKF-05)</done>
|
||||
</task>
|
||||
|
||||
</tasks>
|
||||
|
||||
<verification>
|
||||
1. `python -c "from gps_denied.core.eskf import ESKF; from gps_denied.schemas.eskf import ESKFState, ESKFConfig, ConfidenceTier, IMUMeasurement; print('All imports OK')"`
|
||||
2. `python -c "import numpy as np; from gps_denied.core.eskf import ESKF; e = ESKF(); e.initialize(np.zeros(3), 0.0); s = e.get_state(); assert s.covariance.shape == (15,15); print('State shape OK')"`
|
||||
3. `ruff check src/gps_denied/core/eskf.py src/gps_denied/schemas/eskf.py` passes
|
||||
</verification>
|
||||
|
||||
<success_criteria>
|
||||
- ESKF class importable and instantiable with default config
|
||||
- predict() propagates nominal state and grows covariance
|
||||
- update_vo() applies Kalman correction from relative pose
|
||||
- update_satellite() applies Kalman correction from absolute position
|
||||
- get_confidence() returns correct tier based on timing and covariance
|
||||
- initialize_from_gps() sets state from GPS coordinates
|
||||
- All math uses numpy, no external filter library
|
||||
</success_criteria>
|
||||
|
||||
<output>
|
||||
After completion, create `.planning/phases/01-eskf-core/01-01-SUMMARY.md`
|
||||
</output>
|
||||
Reference in New Issue
Block a user