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

phase, plan, type, wave, depends_on, files_modified, autonomous, requirements, must_haves

phase	plan	type	wave	depends_on	files_modified	autonomous	requirements	must_haves
01-eskf-core	01	execute	1		src/gps_denied/core/eskf.py src/gps_denied/schemas/eskf.py	true	ESKF-01 ESKF-02 ESKF-03 ESKF-04 ESKF-05	truths artifacts key_links 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 path provides min_lines exports src/gps_denied/core/eskf.py 15-state ESKF implementation 250 ESKF path provides exports src/gps_denied/schemas/eskf.py ESKF data contracts ESKFState ESKFConfig ConfidenceTier IMUMeasurement from to via pattern src/gps_denied/core/eskf.py src/gps_denied/schemas/eskf.py import ESKFState, ESKFConfig, ConfidenceTier, IMUMeasurement from gps_denied.schemas.eskf import from to via pattern src/gps_denied/core/eskf.py numpy matrix math for F, Q, P, K, H import numpy as np

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

<execution_context> @$HOME/.claude/get-shit-done/workflows/execute-plan.md @$HOME/.claude/get-shit-done/templates/summary.md </execution_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 Task 1: Create ESKF schema contracts src/gps_denied/schemas/eskf.py - 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) Create `src/gps_denied/schemas/eskf.py` with these exact types:

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

from gps_denied.schemas.eskf import ConfidenceTier, ESKFConfig, ESKFState, IMUMeasurement

Per ESKF-01 (state vector), ESKF-04 (initialization config), ESKF-05 (confidence tiers). python -c "from gps_denied.schemas.eskf import ESKFState, ESKFConfig, ConfidenceTier, IMUMeasurement; print('OK')" <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> All four ESKF schema classes importable, fields match solution.md spec exactly, ConfidenceTier has all four tiers

Task 2: Implement ESKF core algorithm src/gps_denied/core/eskf.py - 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) - 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 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(vv^T) + 2wskew(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. 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') " <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> 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)

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

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

After completion, create `.planning/phases/01-eskf-core/01-01-SUMMARY.md`