feat(eskf): add ESKF schema contracts (ESKF-01, ESKF-04, ESKF-05)

ConfidenceTier, IMUMeasurement, ESKFConfig, ESKFState Pydantic models.
Re-exported from schemas/__init__.py.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
Yuzviak
2026-04-01 23:37:14 +03:00
parent bf9bef19c8
commit 57c7a6b80a
2 changed files with 71 additions and 0 deletions
+3
View File
@@ -37,3 +37,6 @@ class Geofences(BaseModel):
"""Collection of geofence polygons."""
polygons: list[Polygon] = Field(default_factory=list)
from gps_denied.schemas.eskf import ConfidenceTier, ESKFConfig, ESKFState, IMUMeasurement # noqa: E402
+68
View File
@@ -0,0 +1,68 @@
"""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,) ENU meters from origin (East, North, Up)
velocity: np.ndarray # (3,) ENU m/s
quaternion: np.ndarray # (4,) [w, x, y, z] body-to-ENU
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