gps-denied-onboard/.planning/phases/01-eskf-core/01-01-PLAN.md

---
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>