mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-23 02:06:36 +00:00
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:
@@ -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
|
||||
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user