diff --git a/src/gps_denied/schemas/__init__.py b/src/gps_denied/schemas/__init__.py index 65964a3..6e84ca4 100644 --- a/src/gps_denied/schemas/__init__.py +++ b/src/gps_denied/schemas/__init__.py @@ -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 diff --git a/src/gps_denied/schemas/eskf.py b/src/gps_denied/schemas/eskf.py new file mode 100644 index 0000000..4cf3c28 --- /dev/null +++ b/src/gps_denied/schemas/eskf.py @@ -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