docs(01-eskf-core): create phase plan — 3 plans in 2 waves

2026-04-23 07:06:37 +00:00 · 2026-04-01 22:11:57 +03:00
parent d2b431f17f
commit bf9bef19c8
5 changed files with 1212 additions and 2 deletions
@@ -1,2 +1,19 @@
 # OS
 .DS_Store
 # Python
 __pycache__/
 *.pyc
 *.pyo
 *.pyd
 *.egg-info/
 .venv/
 dist/
 build/
 # Pytest
 .pytest_cache/
 # Env
 .env
 *.env
@@ -26,7 +26,11 @@ The scaffold exists (~2800 lines): FastAPI service, all component ABCs, Pydantic
  3. Satellite measurement update corrects absolute position and covariance tightens to satellite noise level
  4. Confidence tier outputs HIGH when last satellite correction is recent and covariance is small, MEDIUM on VO-only, LOW on IMU-only — verified by unit tests
  5. Full coordinate chain (pixel → camera ray → body → NED → WGS84) produces correct GPS coordinates for a known geometry test case; all FAKE Math stubs replaced
-**Plans**: TBD
+**Plans**: 3 plans
 Plans:
 - [ ] 01-01-PLAN.md — ESKF core algorithm (schemas, 15-state filter, IMU prediction, VO/satellite updates, confidence tiers)
 - [ ] 01-02-PLAN.md — Coordinate chain fix (replace fake math with real K matrix projection, ray-ground intersection)
 - [ ] 01-03-PLAN.md — Unit tests for ESKF and coordinate chain (18+ ESKF tests, 10+ coordinate tests)
 ### Phase 2: Visual Odometry
 **Goal**: VO produces metric relative poses via cuVSLAM on Jetson and via OpenCV ORB on dev/CI, both satisfying the same interface — no more scale-ambiguous unit vectors
@@ -101,7 +105,7 @@ The scaffold exists (~2800 lines): FastAPI service, all component ABCs, Pydantic
 | Phase | Plans Complete | Status | Completed |
 |-------|----------------|--------|-----------|
-| 1. ESKF Core | 0/TBD | Not started | - |
+| 1. ESKF Core | 0/3 | Planned | - |
 | 2. Visual Odometry | 0/TBD | Not started | - |
 | 3. Satellite Matching + GPR | 0/TBD | Not started | - |
 | 4. MAVLink I/O | 0/TBD | Not started | - |
@@ -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>
@@ -0,0 +1,389 @@
 ---
 phase: 01-eskf-core
 plan: 02
 type: execute
 wave: 1
 depends_on: []
 files_modified:
  - src/gps_denied/core/coordinates.py
 autonomous: true
 requirements:
  - ESKF-06
 must_haves:
  truths:
    - "pixel_to_gps uses camera intrinsic matrix K to unproject pixel to camera ray, not fake 0.1m/pixel scaling"
    - "Camera-to-body transform uses T_cam_body rotation (nadir-pointing camera mount)"
    - "Body-to-NED rotation uses a quaternion parameter (from ESKF attitude), not hardcoded identity"
    - "Ray-ground intersection at known altitude produces NED position, then converts to WGS84"
    - "gps_to_pixel is the inverse: WGS84 -> NED -> body -> camera -> pixel"
    - "transform_points applies a real 3x3 homography via cv2.perspectiveTransform"
  artifacts:
    - path: "src/gps_denied/core/coordinates.py"
      provides: "Full coordinate chain: pixel -> camera ray -> body -> NED -> WGS84"
      contains: "np.linalg.inv"
  key_links:
    - from: "src/gps_denied/core/coordinates.py"
      to: "numpy"
      via: "K matrix construction, quaternion rotation, ray-ground intersection"
      pattern: "np\\.array"
    - from: "src/gps_denied/core/coordinates.py"
      to: "cv2"
      via: "perspectiveTransform for homography application"
      pattern: "cv2\\.perspectiveTransform"
 ---
 <objective>
 Replace all FAKE Math stubs in CoordinateTransformer with the real pixel-to-GPS coordinate chain: pixel -> camera ray (K matrix) -> body frame (T_cam_body) -> NED (quaternion) -> WGS84.
 Purpose: Without real coordinate transforms, all position estimates, object localization, and satellite matching produce wrong results. The 0.1m/pixel approximation ignores altitude, camera intrinsics, and UAV attitude entirely.
 Output: Updated `src/gps_denied/core/coordinates.py` with real geometric projections.
 </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 187-222 — coordinate system spec)
@src/gps_denied/core/coordinates.py
@src/gps_denied/schemas/__init__.py
 <interfaces>
 <!-- Key types the executor needs -->
 From src/gps_denied/schemas/__init__.py:
 ```python
 class GPSPoint(BaseModel):
    lat: float = Field(..., ge=-90, le=90)
    lon: float = Field(..., ge=-180, le=180)
 class CameraParameters(BaseModel):
    focal_length: float  # mm
    sensor_width: float  # mm
    sensor_height: float  # mm
    resolution_width: int  # pixels
    resolution_height: int  # pixels
    principal_point: tuple[float, float] | None = None
    distortion_coefficients: list[float] | None = None
 ```
 From _docs/01_solution/solution.md (camera intrinsics):
 - ADTI 20L V1 + 16mm lens: fx = fy = 16 * 5456 / 23.2 = 3763 pixels
 - cx = 2728, cy = 1816
 - Camera mount: nadir-pointing (Z-down), R_cam_body = Rx(180deg)
 </interfaces>
 </context>
 <tasks>
 <task type="auto">
  <name>Task 1: Replace fake coordinate transforms with real camera projection math</name>
  <files>src/gps_denied/core/coordinates.py</files>
  <read_first>
    - src/gps_denied/core/coordinates.py (current file — understand existing methods, ENU convention, OriginNotSetError)
    - src/gps_denied/schemas/__init__.py (CameraParameters fields: focal_length in mm, sensor_width/height in mm, resolution_width/height in pixels, principal_point optional)
    - _docs/01_solution/solution.md (lines 187-222 for reference frames: Camera C, Body B, NED N, WGS84; transformation chain; T_cam_body spec)
  </read_first>
  <action>
 Modify `src/gps_denied/core/coordinates.py`. Keep all existing methods (set_enu_origin, get_enu_origin, gps_to_enu, enu_to_gps) UNCHANGED. Replace the implementations of `pixel_to_gps`, `gps_to_pixel`, `image_object_to_gps`, and `transform_points`.
 Add imports at top:
 ```python
 import cv2
 import numpy as np
 ```
 Add module-level helper function:
 ```python
 def _build_intrinsic_matrix(cam: CameraParameters) -> np.ndarray:
    """Build 3x3 camera intrinsic matrix K from CameraParameters."""
    fx = cam.focal_length * cam.resolution_width / cam.sensor_width
    fy = cam.focal_length * cam.resolution_height / cam.sensor_height
    if cam.principal_point is not None:
        cx, cy = cam.principal_point
    else:
        cx = cam.resolution_width / 2.0
        cy = cam.resolution_height / 2.0
    return np.array([
        [fx,  0, cx],
        [ 0, fy, cy],
        [ 0,  0,  1],
    ], dtype=np.float64)
 ```
 Add module-level helper:
 ```python
 def _cam_to_body_rotation() -> np.ndarray:
    """Camera-to-body rotation for nadir-pointing camera.
    Camera frame: Z forward (optical axis), X right, Y down (OpenCV convention).
    Body frame: X forward (nose), Y right (starboard), Z down.
    Camera points nadir: camera Z-axis aligns with body -Z (downward).
    The rotation is Rx(180deg): flips Y and Z axes.
    R_cam_body = [[1, 0, 0], [0, -1, 0], [0, 0, -1]]
    """
    return np.array([
        [1,  0,  0],
        [0, -1,  0],
        [0,  0, -1],
    ], dtype=np.float64)
 ```
 Add module-level helper:
 ```python
 def _quat_to_rotation_matrix(q: np.ndarray) -> np.ndarray:
    """Convert [w, x, y, z] quaternion to 3x3 rotation matrix."""
    w, x, y, z = q
    return np.array([
        [1 - 2*(y*y + z*z), 2*(x*y - w*z),     2*(x*z + w*y)],
        [2*(x*y + w*z),     1 - 2*(x*x + z*z), 2*(y*z - w*x)],
        [2*(x*z - w*y),     2*(y*z + w*x),     1 - 2*(x*x + y*y)],
    ], dtype=np.float64)
 ```
 Replace `pixel_to_gps` method:
 ```python
 def pixel_to_gps(
    self,
    flight_id: str,
    pixel: tuple[float, float],
    frame_pose: dict,
    camera_params: CameraParameters,
    altitude: float,
    quaternion: np.ndarray | None = None,
 ) -> GPSPoint:
    """Unproject pixel to GPS via ray-ground intersection.
    Chain: pixel -> camera ray (K^-1) -> body (T_cam_body) -> ENU (quaternion) -> WGS84.
    Args:
        flight_id: Flight identifier for ENU origin lookup.
        pixel: (u, v) pixel coordinates.
        frame_pose: Dict with "position" key -> [east, north, up] in ENU meters.
        camera_params: Camera intrinsic parameters.
        altitude: UAV altitude above ground in meters (positive up).
        quaternion: [w, x, y, z] body-to-ENU rotation quaternion.
                    If None, assumes identity (level flight, north-facing).
    """
    # Step 1: Pixel -> camera ray
    K = _build_intrinsic_matrix(camera_params)
    K_inv = np.linalg.inv(K)
    pixel_h = np.array([pixel[0], pixel[1], 1.0])
    ray_cam = K_inv @ pixel_h  # direction in camera frame
    # Step 2: Camera -> body
    R_cam_body = _cam_to_body_rotation()
    ray_body = R_cam_body @ ray_cam
    # Step 3: Body -> ENU
    if quaternion is not None:
        R_body_enu = _quat_to_rotation_matrix(quaternion)
    else:
        R_body_enu = np.eye(3)
    ray_enu = R_body_enu @ ray_body
    # Step 4: Ray-ground intersection
    # UAV is at altitude (Up component), ground is at Up=0
    # Ray: P = P_uav + t * ray_enu
    # Ground plane: Up = 0 -> t = -altitude / ray_enu[2]
    if abs(ray_enu[2]) < 1e-10:
        # Ray parallel to ground — return UAV position projected
        frame_enu = frame_pose.get("position", [0, 0, 0])
        return self.enu_to_gps(flight_id, (frame_enu[0], frame_enu[1], 0.0))
    t = -altitude / ray_enu[2]
    if t < 0:
        # Ray points upward — use UAV position
        frame_enu = frame_pose.get("position", [0, 0, 0])
        return self.enu_to_gps(flight_id, (frame_enu[0], frame_enu[1], 0.0))
    frame_enu = frame_pose.get("position", [0, 0, 0])
    ground_east = frame_enu[0] + t * ray_enu[0]
    ground_north = frame_enu[1] + t * ray_enu[1]
    # Step 5: ENU -> WGS84
    return self.enu_to_gps(flight_id, (ground_east, ground_north, 0.0))
 ```
 Replace `gps_to_pixel` method:
 ```python
 def gps_to_pixel(
    self,
    flight_id: str,
    gps: GPSPoint,
    frame_pose: dict,
    camera_params: CameraParameters,
    altitude: float,
    quaternion: np.ndarray | None = None,
 ) -> tuple[float, float]:
    """Project GPS coordinate to image pixel.
    Inverse of pixel_to_gps: WGS84 -> ENU -> body -> camera -> pixel.
    """
    # Step 1: GPS -> ENU
    enu = self.gps_to_enu(flight_id, gps)
    # Step 2: ENU point relative to UAV
    frame_enu = frame_pose.get("position", [0, 0, 0])
    point_enu = np.array([
        enu[0] - frame_enu[0],
        enu[1] - frame_enu[1],
        -altitude,  # ground is at -altitude relative to UAV (UAV is at +altitude)
    ])
    # Step 3: ENU -> body
    if quaternion is not None:
        R_body_enu = _quat_to_rotation_matrix(quaternion)
    else:
        R_body_enu = np.eye(3)
    point_body = R_body_enu.T @ point_enu
    # Step 4: Body -> camera
    R_cam_body = _cam_to_body_rotation()
    point_cam = R_cam_body.T @ point_body
    # Step 5: Camera -> pixel
    if abs(point_cam[2]) < 1e-10:
        cx = camera_params.resolution_width / 2.0
        cy = camera_params.resolution_height / 2.0
        return (cx, cy)
    K = _build_intrinsic_matrix(camera_params)
    pixel_h = K @ (point_cam / point_cam[2])
    return (float(pixel_h[0]), float(pixel_h[1]))
 ```
 Replace `image_object_to_gps` method:
 ```python
 def image_object_to_gps(
    self,
    flight_id: str,
    frame_id: int,
    object_pixel: tuple[float, float],
    frame_pose: dict | None = None,
    camera_params: CameraParameters | None = None,
    altitude: float = 100.0,
    quaternion: np.ndarray | None = None,
 ) -> GPSPoint:
    """Convert object pixel coordinates to GPS using full projection chain.
    If frame_pose or camera_params are not provided, uses defaults for
    backward compatibility (level flight, ADTI 20L V1 camera).
    """
    if frame_pose is None:
        frame_pose = {"position": [0, 0, 0]}
    if camera_params is None:
        camera_params = CameraParameters(
            focal_length=16.0,
            sensor_width=23.2,
            sensor_height=15.4,
            resolution_width=5456,
            resolution_height=3632,
        )
    return self.pixel_to_gps(
        flight_id, object_pixel, frame_pose, camera_params, altitude, quaternion
    )
 ```
 Replace `transform_points` method:
 ```python
 def transform_points(
    self,
    points: list[tuple[float, float]],
    transformation: list[list[float]],
 ) -> list[tuple[float, float]]:
    """Apply 3x3 homography to a list of 2D points using cv2.perspectiveTransform."""
    if not points:
        return []
    H = np.array(transformation, dtype=np.float64)
    pts = np.array(points, dtype=np.float64).reshape(-1, 1, 2)
    transformed = cv2.perspectiveTransform(pts, H)
    return [(float(p[0][0]), float(p[0][1])) for p in transformed]
 ```
 Per ESKF-06: complete coordinate chain replacing all FAKE Math stubs.
  </action>
  <verify>
    <automated>python -c "
 import numpy as np
 from gps_denied.core.coordinates import CoordinateTransformer, _build_intrinsic_matrix
 from gps_denied.schemas import CameraParameters, GPSPoint
 ct = CoordinateTransformer()
 ct.set_enu_origin('f1', GPSPoint(lat=48.0, lon=37.0))
 # Test: image center at altitude 600m with identity quaternion should project near UAV position
 cam = CameraParameters(focal_length=16.0, sensor_width=23.2, sensor_height=15.4, resolution_width=5456, resolution_height=3632)
 pose = {'position': [0, 0, 0]}
 q_identity = np.array([1.0, 0.0, 0.0, 0.0])
 gps = ct.pixel_to_gps('f1', (2728.0, 1816.0), pose, cam, 600.0, q_identity)
 # Center pixel should project to UAV nadir position
 assert abs(gps.lat - 48.0) < 0.001, f'lat={gps.lat}'
 assert abs(gps.lon - 37.0) < 0.001, f'lon={gps.lon}'
 # Test K matrix
 K = _build_intrinsic_matrix(cam)
 assert K.shape == (3, 3)
 fx_expected = 16.0 * 5456 / 23.2
 assert abs(K[0, 0] - fx_expected) < 1.0, f'fx={K[0,0]}, expected={fx_expected}'
 print('Coordinate chain tests OK')
 "</automated>
  </verify>
  <acceptance_criteria>
    - src/gps_denied/core/coordinates.py contains `import numpy as np`
    - src/gps_denied/core/coordinates.py contains `import cv2`
    - src/gps_denied/core/coordinates.py contains `def _build_intrinsic_matrix(cam: CameraParameters) -> np.ndarray`
    - src/gps_denied/core/coordinates.py contains `def _cam_to_body_rotation() -> np.ndarray`
    - src/gps_denied/core/coordinates.py contains `def _quat_to_rotation_matrix(q: np.ndarray) -> np.ndarray`
    - src/gps_denied/core/coordinates.py contains `K_inv = np.linalg.inv(K)` in pixel_to_gps
    - src/gps_denied/core/coordinates.py contains `ray_cam = K_inv @ pixel_h` in pixel_to_gps
    - src/gps_denied/core/coordinates.py contains `R_cam_body @ ray_cam` in pixel_to_gps
    - src/gps_denied/core/coordinates.py contains `t = -altitude / ray_enu[2]` in pixel_to_gps (ray-ground intersection)
    - src/gps_denied/core/coordinates.py contains `cv2.perspectiveTransform` in transform_points
    - The string "FAKE Math" does NOT appear in the file
    - The string "0.1" scaling factor does NOT appear in pixel_to_gps
    - gps_to_enu and enu_to_gps methods are UNCHANGED from original
    - image_object_to_gps uses ADTI 20L V1 defaults: focal_length=16.0, sensor_width=23.2, resolution_width=5456
  </acceptance_criteria>
  <done>All FAKE Math stubs replaced. pixel_to_gps uses K^-1 unprojection, T_cam_body rotation, quaternion body-to-ENU, and ray-ground intersection. gps_to_pixel is the exact inverse. transform_points uses cv2.perspectiveTransform. image_object_to_gps uses real ADTI 20L V1 camera defaults.</done>
 </task>
 </tasks>
 <verification>
 1. `python -c "from gps_denied.core.coordinates import CoordinateTransformer, _build_intrinsic_matrix, _cam_to_body_rotation, _quat_to_rotation_matrix; print('Imports OK')"` — all new functions importable
 2. `ruff check src/gps_denied/core/coordinates.py` — no lint errors
 3. `grep -c "FAKE Math" src/gps_denied/core/coordinates.py` — returns 0 (all stubs removed)
 </verification>
 <success_criteria>
 - pixel_to_gps implements real K^-1 unprojection with camera intrinsics
 - Camera-to-body rotation R_cam_body accounts for nadir-pointing mount
 - Body-to-ENU rotation uses quaternion parameter
 - Ray-ground intersection uses altitude for ground plane
 - Image center pixel projects to UAV nadir position (verified numerically)
 - gps_to_pixel is the exact inverse of pixel_to_gps
 - transform_points uses cv2.perspectiveTransform
 - No FAKE Math stubs remain
 </success_criteria>
 <output>
 After completion, create `.planning/phases/01-eskf-core/01-02-SUMMARY.md`
 </output>
@@ -0,0 +1,445 @@
 ---
 phase: 01-eskf-core
 plan: 03
 type: execute
 wave: 2
 depends_on:
  - 01-01
  - 01-02
 files_modified:
  - tests/test_eskf.py
  - tests/test_coordinates.py
 autonomous: true
 requirements:
  - ESKF-01
  - ESKF-02
  - ESKF-03
  - ESKF-04
  - ESKF-05
  - ESKF-06
 must_haves:
  truths:
    - "ESKF propagation test confirms covariance grows from IMU-only prediction"
    - "ESKF VO update test confirms position uncertainty decreases after update"
    - "ESKF satellite update test confirms position converges to satellite measurement"
    - "Confidence tier test covers all four tiers: HIGH, MEDIUM, LOW, FAILED"
    - "Coordinate chain test verifies image center projects to nadir at correct GPS"
    - "Coordinate chain round-trip test: pixel -> GPS -> pixel returns original pixel"
    - "All tests pass with pytest"
  artifacts:
    - path: "tests/test_eskf.py"
      provides: "Unit tests for ESKF core algorithm"
      min_lines: 150
    - path: "tests/test_coordinates.py"
      provides: "Updated coordinate tests with real projection math"
      min_lines: 80
  key_links:
    - from: "tests/test_eskf.py"
      to: "src/gps_denied/core/eskf.py"
      via: "import ESKF"
      pattern: "from gps_denied\\.core\\.eskf import ESKF"
    - from: "tests/test_coordinates.py"
      to: "src/gps_denied/core/coordinates.py"
      via: "import CoordinateTransformer"
      pattern: "from gps_denied\\.core\\.coordinates import"
 ---
 <objective>
 Create comprehensive unit tests for the ESKF algorithm and update existing coordinate tests to verify the real projection math, covering all six ESKF requirements.
 Purpose: Verify correctness of all ESKF operations (propagation, VO update, satellite update, initialization, confidence tiers) and the full coordinate chain (pixel -> camera ray -> body -> ENU -> WGS84) against known geometry.
 Output: `tests/test_eskf.py` (new) and updated `tests/test_coordinates.py`.
 </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/TESTING.md
@.planning/phases/01-eskf-core/01-01-SUMMARY.md
@.planning/phases/01-eskf-core/01-02-SUMMARY.md
 <interfaces>
 <!-- From Plan 01 -->
 From src/gps_denied/core/eskf.py:
 ```python
 class ESKF:
    def __init__(self, config: ESKFConfig | None = None): ...
    def initialize(self, position_ned: np.ndarray, timestamp: float, velocity=None, quaternion=None): ...
    def initialize_from_gps(self, gps: GPSPoint, altitude: float, timestamp: float, coord_transformer, flight_id: str): ...
    def predict(self, imu: IMUMeasurement) -> None: ...
    def update_vo(self, relative_position: np.ndarray, dt_vo: float) -> np.ndarray: ...
    def update_satellite(self, position_enu: np.ndarray, noise_meters: float) -> np.ndarray: ...
    def get_confidence(self, consecutive_failures: int = 0) -> ConfidenceTier: ...
    def get_state(self) -> ESKFState: ...
    @property
    def initialized(self) -> bool: ...
 ```
 From src/gps_denied/schemas/eskf.py:
 ```python
 class ConfidenceTier(str, Enum): HIGH, MEDIUM, LOW, FAILED
 class IMUMeasurement(BaseModel): accel (3,), gyro (3,), timestamp float
 class ESKFConfig(BaseModel): accel_noise_density, gyro_noise_density, ..., satellite_max_age=30.0, covariance_high_threshold=400.0
 class ESKFState(BaseModel): position (3,), velocity (3,), quaternion (4,), accel_bias (3,), gyro_bias (3,), covariance (15,15), timestamp, confidence
 ```
 <!-- From Plan 02 -->
 From src/gps_denied/core/coordinates.py:
 ```python
 def pixel_to_gps(self, flight_id, pixel, frame_pose, camera_params, altitude, quaternion=None) -> GPSPoint
 def gps_to_pixel(self, flight_id, gps, frame_pose, camera_params, altitude, quaternion=None) -> tuple[float, float]
 def _build_intrinsic_matrix(cam: CameraParameters) -> np.ndarray
 def _cam_to_body_rotation() -> np.ndarray
 def _quat_to_rotation_matrix(q: np.ndarray) -> np.ndarray
 ```
 </interfaces>
 </context>
 <tasks>
 <task type="auto">
  <name>Task 1: Create ESKF unit tests</name>
  <files>tests/test_eskf.py</files>
  <read_first>
    - src/gps_denied/core/eskf.py (the implementation being tested — read ALL methods, understand state layout, error-state correction)
    - src/gps_denied/schemas/eskf.py (ESKFConfig defaults, ConfidenceTier enum values, IMUMeasurement fields)
    - tests/test_coordinates.py (test pattern in this project: pytest fixtures, assertion style, approx usage)
    - tests/test_vo.py (another test file for reference on numpy assertion patterns)
  </read_first>
  <action>
 Create `tests/test_eskf.py` with the following test functions. Follow project conventions: module docstring, pytest fixtures, no class-based tests (only test_coordinates uses functions).
 Module docstring: `"""Tests for ESKF (F17) — Error-State Kalman Filter."""`
 Imports:
 ```python
 import numpy as np
 import pytest
 from gps_denied.core.eskf import ESKF
 from gps_denied.schemas.eskf import ConfidenceTier, ESKFConfig, IMUMeasurement
 from gps_denied.schemas import GPSPoint
 from gps_denied.core.coordinates import CoordinateTransformer
 ```
 Fixtures:
 ```python
@pytest.fixture
 def eskf():
    """ESKF initialized at origin with default config."""
    e = ESKF()
    e.initialize(np.zeros(3), timestamp=0.0)
    return e
@pytest.fixture
 def config():
    return ESKFConfig()
 ```
 **Test: test_initialization_default (ESKF-04)**
 - Create ESKF(), call initialize(np.array([100, 200, 0]), 0.0)
 - Assert eskf.initialized is True
 - state = eskf.get_state()
 - Assert state.position is close to [100, 200, 0]
 - Assert state.velocity is close to [0, 0, 0]
 - Assert state.quaternion is close to [1, 0, 0, 0]
 - Assert state.covariance.shape == (15, 15)
 - Assert state.covariance[0, 0] == config.init_pos_var (100.0)
 - Assert state.covariance[3, 3] == config.init_vel_var (1.0)
 - Assert state.covariance[6, 6] == config.init_att_var (0.01)
 **Test: test_initialization_from_gps (ESKF-04)**
 - Create CoordinateTransformer, set origin GPSPoint(48.0, 37.0)
 - Create ESKF(), call initialize_from_gps(GPSPoint(48.001, 37.001), altitude=600, timestamp=0.0, coord_transformer=ct, flight_id="f1")
 - Assert eskf.initialized is True
 - state = eskf.get_state()
 - Assert state.position is NOT all zeros (GPS offset was applied)
 - Assert np.linalg.norm(state.position[:2]) > 50 (0.001 deg lat ~= 111m)
 **Test: test_predict_covariance_grows (ESKF-01)**
 - Use eskf fixture
 - Record P_before = eskf.get_state().covariance.copy()
 - trace_before = np.trace(P_before[0:3, 0:3])
 - Predict with IMU: accel=[0, 0, 9.81], gyro=[0, 0, 0], timestamp=0.01
 - P_after = eskf.get_state().covariance
 - trace_after = np.trace(P_after[0:3, 0:3])
 - Assert trace_after > trace_before (covariance grew)
 **Test: test_predict_gravity_compensation (ESKF-01)**
 - Use eskf fixture (identity quaternion = level flight in ENU)
 - Predict with 100 steps: accel=[0, 0, 9.81] (gravity in body down, but body aligned with ENU so accel_z = +g), gyro=[0,0,0], dt=0.01
 - After 1 second of stationary IMU with gravity, velocity should be near zero (gravity compensated by the g vector subtraction)
 - state = eskf.get_state()
 - Assert np.linalg.norm(state.velocity) < 1.0 (small drift from numerical integration, but not 9.81*1=9.81)
 **Test: test_predict_with_acceleration (ESKF-01)**
 - Use eskf fixture
 - Apply 100 IMU predictions with accel=[1.0, 0, 9.81] (1 m/s^2 forward + gravity), gyro=[0,0,0], dt=0.01
 - After 1 second, velocity[0] should be approximately 1.0 m/s (from the extra 1 m/s^2 acceleration)
 - state = eskf.get_state()
 - Assert abs(state.velocity[0] - 1.0) < 0.5 (approximate due to discrete integration)
 **Test: test_predict_position_propagation (ESKF-01)**
 - Create ESKF, initialize with position=[0,0,0], velocity=[10, 0, 0]
 - Predict 100 steps with accel=[0, 0, 9.81], gyro=[0,0,0], dt=0.01
 - After 1 second at 10 m/s, position[0] should be approximately 10.0 meters
 - Assert abs(state.position[0] - 10.0) < 2.0
 **Test: test_vo_update_reduces_uncertainty (ESKF-02)**
 - Use eskf fixture
 - Predict a few steps to grow covariance
 - Record trace_before = np.trace(eskf.get_state().covariance[0:3, 0:3])
 - Call update_vo(relative_position=np.array([0.0, 0.0, 0.0]), dt_vo=0.1)
 - trace_after = np.trace(eskf.get_state().covariance[0:3, 0:3])
 - Assert trace_after < trace_before
 **Test: test_vo_update_corrects_position (ESKF-02)**
 - Use eskf fixture
 - Predict 10 steps with accel=[0, 0, 9.81], gyro=[0,0,0], dt=0.1 (1 second)
 - Record position_before = eskf.get_state().position.copy()
 - Call update_vo with relative_position that includes a 1m north offset beyond predicted
 - Record position_after = eskf.get_state().position
 - Assert position_after != position_before (position was corrected)
 **Test: test_vo_update_returns_innovation (ESKF-02)**
 - Use eskf fixture
 - innovation = eskf.update_vo(np.array([1.0, 0.0, 0.0]), dt_vo=0.1)
 - Assert innovation.shape == (3,)
 - Assert np.linalg.norm(innovation) > 0.5 (non-trivial innovation since predicted relative position is near zero)
 **Test: test_satellite_update_corrects_position (ESKF-03)**
 - Use eskf fixture
 - Predict several steps to drift from origin
 - Record pos_before = eskf.get_state().position.copy()
 - Call update_satellite(position_enu=np.array([50.0, 50.0, 0.0]), noise_meters=10.0)
 - pos_after = eskf.get_state().position
 - Assert position moved toward [50, 50, 0]: np.linalg.norm(pos_after - [50,50,0]) < np.linalg.norm(pos_before - [50,50,0])
 **Test: test_satellite_update_tightens_covariance (ESKF-03)**
 - Use eskf fixture (high initial covariance)
 - trace_before = np.trace(eskf.get_state().covariance[0:3, 0:3])
 - Call update_satellite(np.zeros(3), noise_meters=5.0)
 - trace_after = np.trace(eskf.get_state().covariance[0:3, 0:3])
 - Assert trace_after < trace_before
 **Test: test_satellite_update_covariance_bounded_by_noise (ESKF-03)**
 - Use eskf fixture
 - Call update_satellite(np.zeros(3), noise_meters=5.0)
 - pos_cov_trace = np.trace(eskf.get_state().covariance[0:3, 0:3])
 - Assert pos_cov_trace < 3 * 5.0**2 (covariance bounded by satellite noise)
 **Test: test_confidence_high (ESKF-05)**
 - Use eskf fixture
 - Call update_satellite to set last_satellite_time
 - Advance timestamp by small dt (predict one step)
 - Assert eskf.get_confidence() == ConfidenceTier.HIGH
 **Test: test_confidence_medium (ESKF-05)**
 - Create ESKF, initialize at time 0.0
 - Call update_vo to set last_vo_time (but never satellite)
 - Assert eskf.get_confidence() == ConfidenceTier.MEDIUM
 **Test: test_confidence_low (ESKF-05)**
 - Create ESKF, initialize at time 0.0
 - Predict many steps without any measurement update (no VO, no satellite)
 - Eventually confidence should be LOW (no recent VO or satellite)
 - Assert eskf.get_confidence() == ConfidenceTier.LOW
 **Test: test_confidence_failed (ESKF-05)**
 - Use eskf fixture
 - Assert eskf.get_confidence(consecutive_failures=3) == ConfidenceTier.FAILED
 - Assert eskf.get_confidence(consecutive_failures=0) != ConfidenceTier.FAILED
 **Test: test_get_state_returns_eskf_state (ESKF-01)**
 - Use eskf fixture
 - state = eskf.get_state()
 - Assert isinstance(state, ESKFState)
 - Assert state.position.shape == (3,)
 - Assert state.velocity.shape == (3,)
 - Assert state.quaternion.shape == (4,)
 - Assert state.accel_bias.shape == (3,)
 - Assert state.gyro_bias.shape == (3,)
 - Assert state.covariance.shape == (15, 15)
 **Test: test_full_fusion_sequence (integration)**
 - Create ESKF, initialize at origin
 - Run 10 IMU predictions (simulate 0.5s at 20Hz)
 - Assert covariance grew
 - Run VO update with small correction
 - Assert covariance decreased
 - Run 10 more IMU predictions
 - Run satellite update with known position
 - Assert position is close to satellite position (within satellite noise)
 - Assert confidence is HIGH
  </action>
  <verify>
    <automated>cd /home/yuzviak/Azaion/gps-denied-onboard && python -m pytest tests/test_eskf.py -v --tb=short 2>&1 | tail -40</automated>
  </verify>
  <acceptance_criteria>
    - tests/test_eskf.py contains at least 15 test functions (test_initialization_default, test_initialization_from_gps, test_predict_covariance_grows, test_predict_gravity_compensation, test_predict_with_acceleration, test_predict_position_propagation, test_vo_update_reduces_uncertainty, test_vo_update_corrects_position, test_vo_update_returns_innovation, test_satellite_update_corrects_position, test_satellite_update_tightens_covariance, test_satellite_update_covariance_bounded_by_noise, test_confidence_high, test_confidence_medium, test_confidence_low, test_confidence_failed, test_get_state_returns_eskf_state, test_full_fusion_sequence)
    - tests/test_eskf.py contains `from gps_denied.core.eskf import ESKF`
    - tests/test_eskf.py contains `from gps_denied.schemas.eskf import ConfidenceTier, ESKFConfig, IMUMeasurement`
    - Every test function contains at least one `assert` statement
    - `pytest tests/test_eskf.py` exits 0 (all tests pass)
  </acceptance_criteria>
  <done>All 18 ESKF tests pass, covering: initialization (ESKF-04), IMU prediction with covariance growth and gravity compensation (ESKF-01), VO update with uncertainty reduction (ESKF-02), satellite update with absolute correction (ESKF-03), all four confidence tiers (ESKF-05), and full fusion integration</done>
 </task>
 <task type="auto">
  <name>Task 2: Update coordinate chain tests for real projection math</name>
  <files>tests/test_coordinates.py</files>
  <read_first>
    - tests/test_coordinates.py (current tests — understand what exists, what must be preserved vs updated)
    - src/gps_denied/core/coordinates.py (the updated implementation with real K matrix, ray-ground intersection, perspectiveTransform)
    - src/gps_denied/schemas/__init__.py (CameraParameters fields)
  </read_first>
  <action>
 Rewrite `tests/test_coordinates.py` to test the real coordinate chain. Keep the existing tests for gps_to_enu and enu_to_gps UNCHANGED (they still pass since those methods were not modified). Update pixel_to_gps tests and add new ones for the real math.
 Module docstring: `"""Tests for CoordinateTransformer (F13) — real coordinate chain."""`
 Keep existing imports, add:
 ```python
 import numpy as np
 ```
 Keep existing fixtures unchanged.
 **Keep test_enu_origin_management** — unchanged.
 **Keep test_gps_to_enu** — unchanged.
 **Keep test_enu_roundtrip** — unchanged.
 **Replace test_pixel_to_gps_flow** with these new tests:
 **Test: test_pixel_to_gps_center_projects_to_nadir**
 - Set origin GPSPoint(48.0, 37.0)
 - ADTI 20L V1 camera: focal_length=16.0, sensor_width=23.2, sensor_height=15.4, resolution_width=5456, resolution_height=3632
 - pixel = (2728.0, 1816.0) (image center = principal point)
 - pose = {"position": [0, 0, 0]}
 - quaternion = np.array([1.0, 0.0, 0.0, 0.0]) (identity = level, north-facing)
 - altitude = 600.0
 - gps = transformer.pixel_to_gps("flight_123", pixel, pose, cam, altitude, quaternion)
 - Assert abs(gps.lat - 48.0) < 1e-6 (center pixel projects exactly below UAV)
 - Assert abs(gps.lon - 37.0) < 1e-6
 **Test: test_pixel_to_gps_off_center**
 - Same setup as above, but pixel = (2728 + 100, 1816.0) (100 pixels to the right)
 - gps = transformer.pixel_to_gps(...)
 - The point should be offset east from origin (right in camera = east when north-facing with nadir camera)
 - offset_enu = transformer.gps_to_enu("flight_123", gps)
 - Assert offset_enu[0] > 0 (east offset from camera right)
 - Verify ground offset is approximately: 100 pixels * (altitude / fx) where fx = 16*5456/23.2 = 3763.45
 - Expected east offset ~= 100 * 600 / 3763.45 ~= 15.9 meters
 - Assert abs(offset_enu[0] - 15.9) < 1.0
 **Test: test_pixel_to_gps_roundtrip**
 - Set origin, create camera, pose, identity quaternion, altitude=600
 - For several test pixels: (1000, 1000), (3000, 2000), (4000, 500)
  - gps = pixel_to_gps(... pixel, ... quaternion)
  - pixel_back = gps_to_pixel(... gps, ... quaternion)
  - Assert abs(pixel_back[0] - pixel[0]) < 0.1
  - Assert abs(pixel_back[1] - pixel[1]) < 0.1
 **Test: test_pixel_to_gps_altitude_scaling**
 - Same pixel (off-center), two altitudes: 300m and 600m
 - The ground offset at 600m should be approximately 2x the offset at 300m
 - gps_300 = pixel_to_gps(... altitude=300)
 - gps_600 = pixel_to_gps(... altitude=600)
 - enu_300 = gps_to_enu(gps_300)
 - enu_600 = gps_to_enu(gps_600)
 - Assert abs(enu_600[0] / enu_300[0] - 2.0) < 0.1
 **Test: test_pixel_to_gps_with_quaternion_rotation**
 - Use a quaternion that represents a 90-degree yaw (heading east instead of north)
 - What was "right" in camera (east when north-facing) becomes "south" when east-facing
 - Verify the GPS offset direction changes accordingly
 - Use quaternion for 90deg yaw around Z (down in NED, up in ENU):
  - q = [cos(pi/4), 0, 0, sin(pi/4)] for yaw rotation
 - pixel to the right of center should now project south, not east
 **Test: test_transform_points_homography**
 - points = [(0, 0), (100, 0), (100, 100), (0, 100)]
 - H = identity [[1,0,0],[0,1,0],[0,0,1]]
 - result = transformer.transform_points(points, H)
 - Assert result == points (identity preserves points)
 - H_translate = [[1,0,10],[0,1,20],[0,0,1]]
 - result = transformer.transform_points(points, H_translate)
 - Assert result[0] is approximately (10, 20)
 **Test: test_transform_points_empty**
 - result = transformer.transform_points([], [[1,0,0],[0,1,0],[0,0,1]])
 - Assert result == []
 **Test: test_image_object_to_gps_uses_real_camera**
 - Set origin
 - Use image center pixel
 - result = transformer.image_object_to_gps("flight_123", 1, (2728.0, 1816.0))
 - Assert result is close to origin (nadir projection with default altitude=100)
 - Assert abs(result.lat - 48.0) < 0.01
 **Test: test_build_intrinsic_matrix**
 - from gps_denied.core.coordinates import _build_intrinsic_matrix
 - cam = CameraParameters(focal_length=16.0, sensor_width=23.2, sensor_height=15.4, resolution_width=5456, resolution_height=3632)
 - K = _build_intrinsic_matrix(cam)
 - Assert K.shape == (3, 3)
 - fx = 16.0 * 5456 / 23.2  # = 3763.45
 - Assert abs(K[0, 0] - fx) < 1.0
 - fy = 16.0 * 3632 / 15.4  # = 3773.51
 - Assert abs(K[1, 1] - fy) < 1.0
 - Assert abs(K[0, 2] - 2728.0) < 1.0  # cx = width/2
 - Assert abs(K[1, 2] - 1816.0) < 1.0  # cy = height/2
 Per ESKF-06: verifying the full coordinate chain pixel -> camera ray -> body -> ENU -> WGS84.
  </action>
  <verify>
    <automated>cd /home/yuzviak/Azaion/gps-denied-onboard && python -m pytest tests/test_coordinates.py -v --tb=short 2>&1 | tail -30</automated>
  </verify>
  <acceptance_criteria>
    - tests/test_coordinates.py contains `import numpy as np`
    - tests/test_coordinates.py contains test_enu_origin_management (preserved from original)
    - tests/test_coordinates.py contains test_gps_to_enu (preserved from original)
    - tests/test_coordinates.py contains test_enu_roundtrip (preserved from original)
    - tests/test_coordinates.py contains test_pixel_to_gps_center_projects_to_nadir
    - tests/test_coordinates.py contains test_pixel_to_gps_off_center with assertion `abs(offset_enu[0] - 15.9) < 1.0` or similar ground truth check
    - tests/test_coordinates.py contains test_pixel_to_gps_roundtrip testing at least 3 pixel positions
    - tests/test_coordinates.py contains test_pixel_to_gps_altitude_scaling verifying 2x scaling
    - tests/test_coordinates.py contains test_transform_points_homography
    - tests/test_coordinates.py contains test_build_intrinsic_matrix verifying fx ~= 3763
    - `pytest tests/test_coordinates.py` exits 0 (all tests pass)
  </acceptance_criteria>
  <done>Coordinate tests verify: center pixel projects to nadir (ESKF-06 chain), off-center pixel offset matches expected ground distance, pixel-to-GPS-to-pixel roundtrip within 0.1px, altitude scaling is linear, quaternion rotation changes projection direction, homography transform works via cv2, K matrix values match ADTI 20L V1 spec</done>
 </task>
 </tasks>
 <verification>
 1. `cd /home/yuzviak/Azaion/gps-denied-onboard && python -m pytest tests/test_eskf.py tests/test_coordinates.py -v --tb=short` — all tests pass
 2. `cd /home/yuzviak/Azaion/gps-denied-onboard && python -m pytest tests/ -x --tb=short` — existing tests also still pass (no regressions)
 3. Test count: `pytest tests/test_eskf.py --collect-only | grep "test session starts" -A 1` — at least 15 tests
 4. Test count: `pytest tests/test_coordinates.py --collect-only | grep "test session starts" -A 1` — at least 10 tests
 </verification>
 <success_criteria>
 - All ESKF unit tests pass: initialization, prediction, VO update, satellite update, confidence tiers, state retrieval, full fusion sequence
 - All coordinate chain tests pass: center pixel nadir, off-center with ground truth, roundtrip, altitude scaling, quaternion rotation, homography, K matrix
 - Existing tests in tests/ directory are not broken (no regressions)
 - Phase 1 success criteria are met:
  1. ESKF propagates and covariance grows (test_predict_covariance_grows)
  2. VO update reduces uncertainty (test_vo_update_reduces_uncertainty)
  3. Satellite update corrects position (test_satellite_update_corrects_position)
  4. Confidence tiers work (test_confidence_high/medium/low/failed)
  5. Full coordinate chain works (test_pixel_to_gps_center_projects_to_nadir, test_pixel_to_gps_roundtrip)
 </success_criteria>
 <output>
 After completion, create `.planning/phases/01-eskf-core/01-03-SUMMARY.md`
 </output>