feat(tests): add comprehensive ESKF + coordinate chain tests (ESKF-01..06)

test_eskf.py: 18 tests covering initialization, IMU prediction, VO/satellite
updates, confidence tiers, and full fusion integration.
test_coordinates.py: 17 new tests for K matrix, ray-ground intersection,
pixel-GPS roundtrips, and cv2.perspectiveTransform homography.

All 35 tests pass.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

tests/test_coordinates.py

@@ -1,8 +1,15 @@
-"""Tests for CoordinateTransformer (F13)."""
+"""Tests for CoordinateTransformer (F13) — Real coordinate chain (ESKF-06)."""
 
+import numpy as np
 import pytest
 
-from gps_denied.core.coordinates import CoordinateTransformer, OriginNotSetError
+from gps_denied.core.coordinates import (
+    CoordinateTransformer,
+    OriginNotSetError,
+    _build_intrinsic_matrix,
+    _cam_to_body_rotation,
+    _quat_to_rotation_matrix,
+)
 from gps_denied.schemas import CameraParameters, GPSPoint
 
 
@@ -58,7 +65,7 @@ def test_pixel_to_gps_flow(transformer):
     fid = "flight_123"
     origin = GPSPoint(lat=48.0, lon=37.0)
     transformer.set_enu_origin(fid, origin)
-    
+
     cam = CameraParameters(
         focal_length=25.0,
         sensor_width=23.5,
@@ -66,19 +73,317 @@ def test_pixel_to_gps_flow(transformer):
         resolution_width=4000,
         resolution_height=3000,
     )
-    
+
-    # Image center should yield the frame center (mock implementation logic)
+    # Image center should yield approximately the frame center
-    pixel = (2000.0, 1500.0)
+    cx = cam.resolution_width / 2.0
+    cy = cam.resolution_height / 2.0
+    pixel = (cx, cy)
     pose = {"position": [0, 0, 0]}
-    
+    q_identity = np.array([1.0, 0.0, 0.0, 0.0])
-    gps_res = transformer.pixel_to_gps(fid, pixel, pose, cam, 100.0)
+
-    assert gps_res.lat == origin.lat
+    gps_res = transformer.pixel_to_gps(fid, pixel, pose, cam, 100.0, quaternion=q_identity)
-    assert gps_res.lon == origin.lon
+    assert pytest.approx(gps_res.lat, abs=1e-4) == origin.lat
-    
+    assert pytest.approx(gps_res.lon, abs=1e-4) == origin.lon
-    # Inverse must match pixel (mock implementations match)
+
-    pix_res = transformer.gps_to_pixel(fid, gps_res, pose, cam, 100.0)
+    # Inverse must match pixel
-    assert pix_res == pixel
+    pix_res = transformer.gps_to_pixel(fid, gps_res, pose, cam, 100.0, quaternion=q_identity)
+    assert abs(pix_res[0] - pixel[0]) < 1.0
+    assert abs(pix_res[1] - pixel[1]) < 1.0
 
     # And image_object_to_gps should work
-    obj_gps = transformer.image_object_to_gps(fid, 1, pixel)
+    obj_gps = transformer.image_object_to_gps(
-    assert obj_gps.lat == origin.lat
+        fid, 1, pixel, frame_pose=pose, camera_params=cam, altitude=100.0, quaternion=q_identity
+    )
+    assert pytest.approx(obj_gps.lat, abs=1e-4) == origin.lat
+
+
+# ============================================================================
+# ESKF-06: Real Coordinate Chain Tests
+# ============================================================================
+
+
+class TestIntrinsicMatrix:
+    """Tests for camera intrinsic matrix construction."""
+
+    def test_build_intrinsic_matrix_adti_20l_v1(self):
+        """Test K matrix for ADTI 20L V1 camera with 16mm lens."""
+        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)
+
+        # Expected focal lengths
+        fx_expected = 16.0 * 5456 / 23.2
+        fy_expected = 16.0 * 3632 / 15.4
+
+        assert K.shape == (3, 3)
+        assert abs(K[0, 0] - fx_expected) < 1.0  # fx
+        assert abs(K[1, 1] - fy_expected) < 1.0  # fy
+        assert abs(K[0, 2] - 2728.0) < 1.0  # cx (center)
+        assert abs(K[1, 2] - 1816.0) < 1.0  # cy (center)
+        assert K[2, 2] == 1.0
+
+    def test_build_intrinsic_matrix_custom_principal_point(self):
+        """Test K matrix with custom principal point."""
+        cam = CameraParameters(
+            focal_length=16.0,
+            sensor_width=23.2,
+            sensor_height=15.4,
+            resolution_width=5456,
+            resolution_height=3632,
+            principal_point=(2700.0, 1800.0),
+        )
+        K = _build_intrinsic_matrix(cam)
+
+        assert abs(K[0, 2] - 2700.0) < 0.1  # cx
+        assert abs(K[1, 2] - 1800.0) < 0.1  # cy
+
+
+class TestCameraToBodyRotation:
+    """Tests for camera-to-body rotation matrix."""
+
+    def test_cam_to_body_is_180_x_rotation(self):
+        """Test that camera-to-body rotation is Rx(180deg)."""
+        R_cam_body = _cam_to_body_rotation()
+
+        # Should flip Y and Z axes, keep X
+        assert R_cam_body.shape == (3, 3)
+        assert R_cam_body[0, 0] == 1.0
+        assert R_cam_body[1, 1] == -1.0
+        assert R_cam_body[2, 2] == -1.0
+        assert np.allclose(np.linalg.det(R_cam_body), 1.0)  # det = 1 (proper rotation)
+
+
+class TestQuaternionRotation:
+    """Tests for quaternion to rotation matrix conversion."""
+
+    def test_identity_quaternion(self):
+        """Test identity quaternion [1, 0, 0, 0] produces identity matrix."""
+        q = np.array([1.0, 0.0, 0.0, 0.0])
+        R = _quat_to_rotation_matrix(q)
+
+        assert np.allclose(R, np.eye(3))
+
+    def test_90_degree_z_rotation(self):
+        """Test 90-degree rotation around Z axis."""
+        # q = [cos(45deg), 0, 0, sin(45deg)] for 90-degree rotation around Z
+        angle = np.pi / 4  # 45 degrees
+        q = np.array([np.cos(angle), 0.0, 0.0, np.sin(angle)])
+        R = _quat_to_rotation_matrix(q)
+
+        # Should rotate [1, 0, 0] to approximately [0, 1, 0]
+        x_axis = np.array([1.0, 0.0, 0.0])
+        rotated = R @ x_axis
+        expected = np.array([0.0, 1.0, 0.0])
+        assert np.allclose(rotated, expected, atol=0.01)
+
+
+class TestPixelToGPSChain:
+    """Tests for full pixel-to-GPS projection chain (ESKF-06)."""
+
+    def test_image_center_nadir_projection(self):
+        """Test that image center pixel projects to UAV nadir position."""
+        transformer = CoordinateTransformer()
+        origin = GPSPoint(lat=48.0, lon=37.0)
+        transformer.set_enu_origin("f1", origin)
+
+        cam = CameraParameters(
+            focal_length=16.0,
+            sensor_width=23.2,
+            sensor_height=15.4,
+            resolution_width=5456,
+            resolution_height=3632,
+        )
+
+        # Center pixel with identity quaternion (level flight)
+        pixel = (2728.0, 1816.0)
+        pose = {"position": [0.0, 0.0, 0.0]}
+        q_identity = np.array([1.0, 0.0, 0.0, 0.0])
+
+        gps = transformer.pixel_to_gps(
+            "f1", pixel, pose, cam, altitude=600.0, quaternion=q_identity
+        )
+
+        # Center pixel should project to UAV nadir (origin)
+        assert abs(gps.lat - 48.0) < 0.001
+        assert abs(gps.lon - 37.0) < 0.001
+
+    def test_pixel_offset_produces_ground_offset(self):
+        """Test that off-center pixel offsets project to corresponding ground distance."""
+        transformer = CoordinateTransformer()
+        origin = GPSPoint(lat=48.0, lon=37.0)
+        transformer.set_enu_origin("f1", origin)
+
+        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.0, 0.0]}
+        q_identity = np.array([1.0, 0.0, 0.0, 0.0])
+        altitude = 600.0
+
+        # Center pixel
+        center_pixel = (2728.0, 1816.0)
+        center_gps = transformer.pixel_to_gps(
+            "f1", center_pixel, pose, cam, altitude=altitude, quaternion=q_identity
+        )
+
+        # Offset pixel (100 pixels to the right)
+        offset_pixel = (2828.0, 1816.0)
+        offset_gps = transformer.pixel_to_gps(
+            "f1", offset_pixel, pose, cam, altitude=altitude, quaternion=q_identity
+        )
+
+        # Longitude difference should be non-zero (right = east = longer)
+        assert offset_gps.lon != center_gps.lon
+
+    def test_altitude_scaling_is_linear(self):
+        """Test that doubling altitude doubles ground distance from nadir."""
+        transformer = CoordinateTransformer()
+        origin = GPSPoint(lat=48.0, lon=37.0)
+        transformer.set_enu_origin("f1", origin)
+
+        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.0, 0.0]}
+        q_identity = np.array([1.0, 0.0, 0.0, 0.0])
+        pixel = (2728.0 + 100, 1816.0)  # Offset pixel
+
+        # Project at altitude 300m
+        gps_300 = transformer.pixel_to_gps(
+            "f1", pixel, pose, cam, altitude=300.0, quaternion=q_identity
+        )
+
+        # Project at altitude 600m
+        gps_600 = transformer.pixel_to_gps(
+            "f1", pixel, pose, cam, altitude=600.0, quaternion=q_identity
+        )
+
+        # Distance from origin should scale with altitude (roughly)
+        # At 600m, ground distance should be ~2x that at 300m
+        dist_300 = abs(gps_300.lon - 37.0)
+        dist_600 = abs(gps_600.lon - 37.0)
+        ratio = dist_600 / dist_300 if dist_300 > 0 else 1.0
+        assert 1.8 < ratio < 2.2  # Allow some numerical tolerance
+
+
+class TestGPSToPixelInverse:
+    """Tests for GPS-to-pixel projection (inverse of pixel-to-GPS)."""
+
+    def test_gps_to_pixel_is_inverse(self):
+        """Test that gps_to_pixel is the exact inverse of pixel_to_gps."""
+        transformer = CoordinateTransformer()
+        origin = GPSPoint(lat=48.0, lon=37.0)
+        transformer.set_enu_origin("f1", origin)
+
+        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.0, 0.0]}
+        q_identity = np.array([1.0, 0.0, 0.0, 0.0])
+        original_pixel = (2728.0 + 50, 1816.0 - 30)
+
+        # pixel -> GPS
+        gps = transformer.pixel_to_gps(
+            "f1", original_pixel, pose, cam, altitude=600.0, quaternion=q_identity
+        )
+
+        # GPS -> pixel
+        recovered_pixel = transformer.gps_to_pixel(
+            "f1", gps, pose, cam, altitude=600.0, quaternion=q_identity
+        )
+
+        # Should recover original pixel
+        assert abs(recovered_pixel[0] - original_pixel[0]) < 1.0
+        assert abs(recovered_pixel[1] - original_pixel[1]) < 1.0
+
+    def test_roundtrip_with_quaternion(self):
+        """Test pixel-GPS-pixel roundtrip with non-identity quaternion."""
+        transformer = CoordinateTransformer()
+        origin = GPSPoint(lat=48.0, lon=37.0)
+        transformer.set_enu_origin("f1", origin)
+
+        cam = CameraParameters(
+            focal_length=16.0,
+            sensor_width=23.2,
+            sensor_height=15.4,
+            resolution_width=5456,
+            resolution_height=3632,
+        )
+
+        pose = {"position": [100.0, 50.0, 0.0]}
+        # Small 30-degree rotation around Z
+        angle = np.radians(30)
+        q = np.array([np.cos(angle / 2), 0, 0, np.sin(angle / 2)])
+        original_pixel = (2728.0, 1816.0)  # Center
+
+        gps = transformer.pixel_to_gps(
+            "f1", original_pixel, pose, cam, altitude=600.0, quaternion=q
+        )
+
+        recovered_pixel = transformer.gps_to_pixel(
+            "f1", gps, pose, cam, altitude=600.0, quaternion=q
+        )
+
+        # Center pixel should still recover to center
+        assert abs(recovered_pixel[0] - 2728.0) < 10.0
+        assert abs(recovered_pixel[1] - 1816.0) < 10.0
+
+
+class TestTransformPoints:
+    """Tests for homography transform via cv2.perspectiveTransform."""
+
+    def test_transform_points_identity(self):
+        """Test homography transform with identity matrix."""
+        transformer = CoordinateTransformer()
+
+        H = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]  # Identity
+        points = [(10.0, 20.0), (30.0, 40.0)]
+
+        result = transformer.transform_points(points, H)
+
+        assert len(result) == 2
+        assert abs(result[0][0] - 10.0) < 0.1
+        assert abs(result[0][1] - 20.0) < 0.1
+        assert abs(result[1][0] - 30.0) < 0.1
+        assert abs(result[1][1] - 40.0) < 0.1
+
+    def test_transform_points_translation(self):
+        """Test homography with translation."""
+        transformer = CoordinateTransformer()
+
+        # Translate by (100, 50)
+        H = [[1, 0, 100], [0, 1, 50], [0, 0, 1]]
+        points = [(0.0, 0.0)]
+
+        result = transformer.transform_points(points, H)
+
+        assert len(result) == 1
+        assert abs(result[0][0] - 100.0) < 0.1
+        assert abs(result[0][1] - 50.0) < 0.1
+
+    def test_transform_points_empty(self):
+        """Test transform_points with empty point list."""
+        transformer = CoordinateTransformer()
+        H = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
+        result = transformer.transform_points([], H)
+        assert result == []

tests/test_eskf.py

@@ -0,0 +1,317 @@
+"""Tests for ESKF (F17) — Error-State Kalman Filter."""
+
+import numpy as np
+import pytest
+
+from gps_denied.core.coordinates import CoordinateTransformer
+from gps_denied.core.eskf import ESKF
+from gps_denied.schemas import GPSPoint
+from gps_denied.schemas.eskf import ConfidenceTier, ESKFConfig, IMUMeasurement
+
+
+@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()
+
+
+class TestESKFInitialization:
+    """Tests for ESKF-04: Initialization."""
+
+    def test_initialization_default(self, config):
+        """Test basic initialization with position and timestamp."""
+        e = ESKF()
+        e.initialize(np.array([100.0, 200.0, 0.0]), timestamp=0.0)
+        assert e.initialized
+        state = e.get_state()
+        assert np.allclose(state.position, [100.0, 200.0, 0.0])
+        assert np.allclose(state.velocity, [0.0, 0.0, 0.0])
+        assert np.allclose(state.quaternion, [1.0, 0.0, 0.0, 0.0])
+        assert state.covariance.shape == (15, 15)
+        assert state.covariance[0, 0] == config.init_pos_var
+        assert state.covariance[3, 3] == config.init_vel_var
+        assert state.covariance[6, 6] == config.init_att_var
+
+    def test_initialization_from_gps(self):
+        """Test initialization from GPS position."""
+        ct = CoordinateTransformer()
+        ct.set_enu_origin("f1", GPSPoint(lat=48.0, lon=37.0))
+
+        e = ESKF()
+        e.initialize_from_gps(
+            GPSPoint(lat=48.001, lon=37.001),
+            altitude=600.0,
+            timestamp=0.0,
+            coord_transformer=ct,
+            flight_id="f1",
+        )
+        assert e.initialized
+        state = e.get_state()
+        assert not np.allclose(state.position, [0.0, 0.0, 0.0])
+        assert np.linalg.norm(state.position[:2]) > 50  # ~111m per 0.001 deg
+
+
+class TestESKFPrediction:
+    """Tests for ESKF-01: IMU prediction."""
+
+    def test_predict_covariance_grows(self, eskf):
+        """Test that covariance grows during IMU-only prediction."""
+        P_before = eskf.get_state().covariance.copy()
+        trace_before = np.trace(P_before[0:3, 0:3])
+
+        imu = IMUMeasurement(
+            accel=np.array([0.0, 0.0, 9.81]),
+            gyro=np.zeros(3),
+            timestamp=0.01,
+        )
+        eskf.predict(imu)
+
+        P_after = eskf.get_state().covariance
+        trace_after = np.trace(P_after[0:3, 0:3])
+        assert trace_after > trace_before
+
+    def test_predict_gravity_compensation(self, eskf):
+        """Test that gravity is properly compensated (stationary accel has no velocity growth)."""
+        # 100 steps at 0.01s = 1 second with gravity-aligned acceleration
+        for i in range(100):
+            imu = IMUMeasurement(
+                accel=np.array([0.0, 0.0, 9.81]),
+                gyro=np.zeros(3),
+                timestamp=0.01 * (i + 1),
+            )
+            eskf.predict(imu)
+
+        state = eskf.get_state()
+        # Velocity should remain near zero (gravity compensated)
+        assert np.linalg.norm(state.velocity) < 1.0
+
+    def test_predict_with_acceleration(self, eskf):
+        """Test velocity integration from applied acceleration."""
+        # 100 steps at 0.01s = 1 second with 1 m/s^2 forward + gravity
+        for i in range(100):
+            imu = IMUMeasurement(
+                accel=np.array([1.0, 0.0, 9.81]),
+                gyro=np.zeros(3),
+                timestamp=0.01 * (i + 1),
+            )
+            eskf.predict(imu)
+
+        state = eskf.get_state()
+        # Velocity[0] should be approximately 1.0 m/s
+        assert abs(state.velocity[0] - 1.0) < 0.5
+
+    def test_predict_position_propagation(self):
+        """Test position propagation from velocity."""
+        e = ESKF()
+        e.initialize(np.zeros(3), timestamp=0.0, velocity=np.array([10.0, 0.0, 0.0]))
+
+        for i in range(100):
+            imu = IMUMeasurement(
+                accel=np.array([0.0, 0.0, 9.81]),
+                gyro=np.zeros(3),
+                timestamp=0.01 * (i + 1),
+            )
+            e.predict(imu)
+
+        state = e.get_state()
+        # Position[0] should be approximately 10.0 meters after 1 second at 10 m/s
+        assert abs(state.position[0] - 10.0) < 2.0
+
+
+class TestESKFVOUpdate:
+    """Tests for ESKF-02: VO measurement update."""
+
+    def test_vo_update_reduces_uncertainty(self, eskf):
+        """Test that VO update reduces position covariance."""
+        # Grow covariance first
+        for _ in range(5):
+            imu = IMUMeasurement(
+                accel=np.zeros(3),
+                gyro=np.zeros(3),
+                timestamp=eskf._last_timestamp + 0.01,
+            )
+            eskf.predict(imu)
+
+        trace_before = np.trace(eskf.get_state().covariance[0:3, 0:3])
+
+        eskf.update_vo(np.zeros(3), dt_vo=0.1)
+
+        trace_after = np.trace(eskf.get_state().covariance[0:3, 0:3])
+        assert trace_after < trace_before
+
+    def test_vo_update_corrects_position(self, eskf):
+        """Test that VO update shifts position toward measurement."""
+        # Predict to drift
+        for i in range(10):
+            imu = IMUMeasurement(
+                accel=np.array([0.0, 0.0, 9.81]),
+                gyro=np.zeros(3),
+                timestamp=0.01 * (i + 1),
+            )
+            eskf.predict(imu)
+
+        pos_before = eskf.get_state().position.copy()
+
+        # VO measurement with 1m north offset
+        eskf.update_vo(np.array([0.0, 1.0, 0.0]), dt_vo=0.1)
+
+        pos_after = eskf.get_state().position
+        assert not np.allclose(pos_after, pos_before)
+
+    def test_vo_update_returns_innovation(self, eskf):
+        """Test that VO update returns innovation vector."""
+        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
+
+
+class TestESKFSatelliteUpdate:
+    """Tests for ESKF-03: Satellite measurement update."""
+
+    def test_satellite_update_corrects_position(self, eskf):
+        """Test that satellite update moves position toward measurement."""
+        # Predict to drift
+        for i in range(10):
+            imu = IMUMeasurement(
+                accel=np.array([0.0, 0.0, 9.81]),
+                gyro=np.zeros(3),
+                timestamp=0.01 * (i + 1),
+            )
+            eskf.predict(imu)
+
+        pos_before = eskf.get_state().position.copy()
+        target_pos = np.array([50.0, 50.0, 0.0])
+
+        eskf.update_satellite(target_pos, noise_meters=10.0)
+
+        pos_after = eskf.get_state().position
+        # Position should move toward target
+        dist_before = np.linalg.norm(pos_before - target_pos)
+        dist_after = np.linalg.norm(pos_after - target_pos)
+        assert dist_after < dist_before
+
+    def test_satellite_update_tightens_covariance(self, eskf):
+        """Test that satellite update reduces position covariance."""
+        trace_before = np.trace(eskf.get_state().covariance[0:3, 0:3])
+
+        eskf.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
+
+    def test_satellite_update_covariance_bounded_by_noise(self, eskf):
+        """Test that final covariance is bounded by satellite noise level."""
+        eskf.update_satellite(np.zeros(3), noise_meters=5.0)
+
+        pos_cov_trace = np.trace(eskf.get_state().covariance[0:3, 0:3])
+        # Covariance should be bounded by roughly 3*noise^2 (3 dimensions)
+        assert pos_cov_trace < 3 * (5.0 ** 2)
+
+
+class TestESKFConfidenceTiers:
+    """Tests for ESKF-05: Confidence tier computation."""
+
+    def test_confidence_high(self, eskf, config):
+        """Test HIGH confidence with recent satellite and low covariance."""
+        eskf.update_satellite(np.zeros(3), noise_meters=5.0)
+
+        # Advance time slightly (still within satellite_max_age)
+        imu = IMUMeasurement(
+            accel=np.array([0.0, 0.0, 9.81]),
+            gyro=np.zeros(3),
+            timestamp=1.0,
+        )
+        eskf.predict(imu)
+
+        assert eskf.get_confidence() == ConfidenceTier.HIGH
+
+    def test_confidence_medium(self):
+        """Test MEDIUM confidence with recent VO but no satellite."""
+        e = ESKF()
+        e.initialize(np.zeros(3), timestamp=0.0)
+
+        e.update_vo(np.zeros(3), dt_vo=0.1)
+        assert e.get_confidence() == ConfidenceTier.MEDIUM
+
+    def test_confidence_low(self, eskf):
+        """Test LOW confidence with no recent measurements."""
+        # Many predictions without any update
+        for i in range(100):
+            imu = IMUMeasurement(
+                accel=np.zeros(3),
+                gyro=np.zeros(3),
+                timestamp=0.01 * (i + 1),
+            )
+            eskf.predict(imu)
+
+        assert eskf.get_confidence() == ConfidenceTier.LOW
+
+    def test_confidence_failed(self, eskf):
+        """Test FAILED confidence with 3+ consecutive failures."""
+        assert eskf.get_confidence(consecutive_failures=3) == ConfidenceTier.FAILED
+        assert eskf.get_confidence(consecutive_failures=0) != ConfidenceTier.FAILED
+
+
+class TestESKFStateAccess:
+    """Tests for ESKF state accessor."""
+
+    def test_get_state_returns_eskf_state(self, eskf):
+        """Test that get_state returns properly formed ESKFState."""
+        state = eskf.get_state()
+        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)
+        assert isinstance(state.confidence, ConfidenceTier)
+
+
+class TestESKFFusion:
+    """Integration tests for full ESKF fusion sequence."""
+
+    def test_full_fusion_sequence(self):
+        """Test complete IMU → VO → satellite fusion."""
+        e = ESKF()
+        e.initialize(np.zeros(3), timestamp=0.0)
+
+        # IMU prediction phase
+        for i in range(10):
+            imu = IMUMeasurement(
+                accel=np.array([0.0, 0.0, 9.81]),
+                gyro=np.zeros(3),
+                timestamp=0.01 * (i + 1),
+            )
+            e.predict(imu)
+
+        P_before_vo = np.trace(e.get_state().covariance[0:3, 0:3])
+
+        # VO update
+        e.update_vo(np.array([0.1, 0.0, 0.0]), dt_vo=0.1)
+
+        P_after_vo = np.trace(e.get_state().covariance[0:3, 0:3])
+        assert P_after_vo < P_before_vo
+
+        # More IMU prediction
+        for i in range(10):
+            imu = IMUMeasurement(
+                accel=np.array([0.0, 0.0, 9.81]),
+                gyro=np.zeros(3),
+                timestamp=0.01 * (i + 11),
+            )
+            e.predict(imu)
+
+        # Satellite update
+        e.update_satellite(np.array([1.0, 0.0, 0.0]), noise_meters=5.0)
+
+        state = e.get_state()
+        # Position should be close to satellite measurement
+        assert np.linalg.norm(state.position[:2] - [1.0, 0.0]) < 10.0
+        assert state.confidence == ConfidenceTier.HIGH