mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-23 03:16: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."""
|
"""Collection of geofence polygons."""
|
||||||
|
|
||||||
polygons: list[Polygon] = Field(default_factory=list)
|
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