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

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