feat(firmware): validate configuration and migrate sensor-driven flight APIs

Validate configuration through its owning schemas and keep board
declarations separate from shared F722 resource ownership. Bound USB
transitions, isolate transport failures and retain restart-only activation.

Migrate navigation and flight consumers with the sensor and schema
contracts. Require explicit installation data and synchronous output
authority; board motor outputs remain inhibited.

Validation: assembled-prefix host tests, strict Clippy, platform/USB tests
and both ARM release builds. Final-source coverage and scoped Miri/Kani
evidence are retained separately from hardware qualification.

firmware/libs/kaptan-navigation/Cargo.toml

@@ -10,11 +10,10 @@ workspace = true
 
 [features]
 default = []
-# Enables the provisional raw-rate estimator used only by the internal SITL
-# executable. It is not part of a board build.
-sitl = ["dep:kaptan-sensor"]
+# Driver snapshot conversion into navigation observations. The estimator is
+# available independently of physical sensor adapters.
+sensor-snapshots = ["dep:kaptan-sensor"]
 [dependencies]
-kaptan-err-reportable = { path = "../kaptan-err-reportable" }
 kaptan-math = { path = "../kaptan-math" }
 kaptan-sensor = { path = "../kaptan-sensor", optional = true }
 kaptan-time = { path = "../kaptan-time" }

firmware/libs/kaptan-navigation/src/flight.rs

@@ -0,0 +1,868 @@
+//! Flight observation, uncertainty and bounded execution contracts.
+
+use crate::*;
+use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3};
+use kaptan_time::Timestamp;
+
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct OriginId(pub u32);
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct SensorReferenceId(pub u32);
+
+/// Mean calibrated force/rate over the declared preceding acquisition window.
+/// Interior propagation reconstructs that window with its constant average.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct FlightImuObservation {
+    pub context: ObservationContext,
+    pub acceleration: AccelerationFrd,
+    pub angular_rate: AngularRateFrd,
+    /// Positive duration in the endpoint clock, bounded by the configured IMU
+    /// interval. Both window endpoints must share the same timing model.
+    pub acquisition_duration_us: u32,
+}
+impl FlightImuObservation {
+    pub(crate) fn acquisition_start(self) -> Option<u64> {
+        if self.acquisition_duration_us == 0 {
+            return None;
+        }
+        self.context
+            .measurement_time
+            .as_microseconds()
+            .checked_sub(u64::from(self.acquisition_duration_us))
+    }
+}
+/// Timing of the latest valid admitted IMU window, possibly not yet propagated.
+/// This historical snapshot does not establish freshness, estimator health or
+/// output authority. It may remain available after a numerical failure.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct ImuCoverage {
+    pub end: Timestamp,
+    pub acquisition_duration_us: u32,
+    pub time_uncertainty_microseconds: u32,
+}
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct MagneticObservation {
+    pub context: ObservationContext,
+    pub microtesla_frd: Vector3<Frd>,
+    pub variance_microtesla_squared: [ScalarVariance; 3],
+}
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct PressureObservation {
+    pub context: ObservationContext,
+    pub pascals: f32,
+    pub variance_pascals_squared: ScalarVariance,
+}
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub enum SensorRecord {
+    Imu(FlightImuObservation),
+    LocalPositionVelocity(LocalPositionVelocityObservation),
+    Magnetic(MagneticObservation),
+    Pressure(PressureObservation),
+    Range(RangeObservation),
+}
+impl SensorRecord {
+    pub const fn context(self) -> ObservationContext {
+        match self {
+            Self::Imu(v) => v.context,
+            Self::LocalPositionVelocity(v) => v.context,
+            Self::Magnetic(v) => v.context,
+            Self::Pressure(v) => v.context,
+            Self::Range(v) => v.context,
+        }
+    }
+    pub const fn kind(self) -> u8 {
+        match self {
+            Self::Imu(_) => 1,
+            Self::LocalPositionVelocity(_) => 2,
+            Self::Magnetic(_) => 3,
+            Self::Pressure(_) => 4,
+            Self::Range(_) => 5,
+        }
+    }
+}
+
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+#[repr(u16)]
+pub enum ObservationRejection {
+    WrongEpoch = 1,
+    InvalidTime = 2,
+    TooOld = 3,
+    Replay = 4,
+    SourceMismatch = 5,
+    FrameMismatch = 6,
+    CalibrationMismatch = 7,
+    InvalidValue = 8,
+    InvalidQuality = 9,
+    QueueFull = 10,
+    RateExceeded = 11,
+    BehindFusionHorizon = 12,
+    ReferenceMismatch = 13,
+    Terminated = 14,
+}
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum ObservationAdmission {
+    Queued,
+    Rejected(ObservationRejection),
+}
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum NavigationFailure {
+    ImuGap,
+    QueueOverrun,
+    WorkBudget,
+    Numerical,
+    RequiredCapabilityLost,
+}
+impl From<kaptan_math::MathError> for NavigationFailure {
+    fn from(_: kaptan_math::MathError) -> Self {
+        Self::Numerical
+    }
+}
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum SourceStatus {
+    Unknown,
+    Healthy,
+    Stale,
+    Isolated,
+}
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct SensorStatus {
+    pub kind: u8,
+    pub source: SourceId,
+    pub integrity: SourceStatus,
+    pub last_received_measurement: Option<Timestamp>,
+    pub last_fused_measurement: Option<Timestamp>,
+    pub queued: u16,
+    pub rejected: u32,
+    pub fused: u32,
+    pub dropped: u32,
+}
+
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct LocalOrigin {
+    pub id: OriginId,
+    pub established_at: Timestamp,
+    pub reference: SensorReferenceId,
+    pub launch_in_reference: PositionNed,
+    pub launch_variance_m2: [f32; 3],
+}
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct CapabilityEvidence {
+    pub available: bool,
+    pub last_measurement_at: Option<Timestamp>,
+    pub valid_until: Option<Timestamp>,
+}
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct FlightCapabilities {
+    pub tilt: CapabilityEvidence,
+    pub heading: CapabilityEvidence,
+    pub horizontal_position: CapabilityEvidence,
+    pub horizontal_velocity: CapabilityEvidence,
+    pub height: CapabilityEvidence,
+    pub vertical_velocity: CapabilityEvidence,
+    pub ground_clearance: CapabilityEvidence,
+}
+impl FlightCapabilities {
+    pub fn flight_ready(self) -> bool {
+        self.tilt.available
+            && self.heading.available
+            && self.horizontal_position.available
+            && self.horizontal_velocity.available
+            && self.height.available
+            && self.vertical_velocity.available
+            && self.ground_clearance.available
+    }
+    pub fn valid_until(self) -> Option<Timestamp> {
+        if !self.flight_ready() {
+            return None;
+        }
+        [
+            self.tilt,
+            self.heading,
+            self.horizontal_position,
+            self.horizontal_velocity,
+            self.height,
+            self.vertical_velocity,
+            self.ground_clearance,
+        ]
+        .into_iter()
+        .map(|v| v.valid_until)
+        .try_fold(Timestamp::from_microseconds(u64::MAX), |a, b| {
+            Some(a.min(b?))
+        })
+    }
+}
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct GroundClearance {
+    pub measurement_at: Timestamp,
+    pub valid_until: Timestamp,
+    pub source: SourceId,
+    pub body_origin_height_metres: f32,
+    pub contact_clearance_metres: f32,
+    pub variance_m2: f32,
+    pub measurement_time_uncertainty_microseconds: u32,
+    pub upper_error_metres: f32,
+}
+impl GroundClearance {
+    /// Bound current clearance using current foot orientation and velocity
+    /// confidence. `upper_error_metres` bounds measured COM height, not a foot
+    /// projection frozen at the range timestamp.
+    pub fn upper_at(
+        self,
+        now: Timestamp,
+        estimate: &FlightEstimate,
+        contact: ContactGeometry,
+    ) -> Option<f32> {
+        let speed_bound =
+            estimate.velocity_error_bound().norm() + estimate.velocity.vector().norm();
+        let angle_bound = estimate.attitude_error_bound().norm();
+        if now >= self.valid_until
+            || estimate.at != now
+            || !speed_bound.is_finite()
+            || !angle_bound.is_finite()
+            || !contact.validate()
+        {
+            return None;
+        }
+        let earliest = self
+            .measurement_at
+            .as_microseconds()
+            .checked_sub(u64::from(self.measurement_time_uncertainty_microseconds))?;
+        let age = now.as_microseconds().checked_sub(earliest)? as f32 * 1e-6;
+        // Configuration checks the vehicle's free-flight acceleration bound.
+        let feet = contact.depth(estimate.attitude);
+        let upper = self.body_origin_height_metres
+            + self.upper_error_metres
+            + speed_bound * age
+            + 0.5 * MAXIMUM_FREE_ACCELERATION_MPS2 * age * age
+            - feet
+            + contact.rotation_radius_bound() * angle_bound;
+        upper.is_finite().then_some(upper)
+    }
+}
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct FlightEstimate {
+    pub at: Timestamp,
+    pub fused_through: Timestamp,
+    /// Latest complete IMU acquisition-window end, not its start or receipt.
+    pub last_imu_at: Timestamp,
+    pub origin: LocalOrigin,
+    pub attitude: UnitQuaternion<Frd, Ned>,
+    pub position: PositionNed,
+    pub velocity: VelocityNed,
+    /// Bias-corrected average of the latest complete declared window. This is not
+    /// an instantaneous angular-rate measurement at `at`.
+    pub angular_rate: AngularRateFrd,
+    pub position_variance_m2: [f32; 3],
+    pub velocity_variance_m2ps2: [f32; 3],
+    pub attitude_variance_rad2: [f32; 3],
+    pub timing_error_bound: TimingErrorBounds,
+    pub capabilities: FlightCapabilities,
+    pub ground_clearance: Option<GroundClearance>,
+}
+/// Deterministic absolute error radii from timing, separate from Gaussian P.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct TimingErrorBounds {
+    pub position_metres: [f32; 3],
+    pub velocity_mps: [f32; 3],
+    pub attitude_radians: [f32; 3],
+}
+impl TimingErrorBounds {
+    pub const ZERO: Self = Self {
+        position_metres: [0.0; 3],
+        velocity_mps: [0.0; 3],
+        attitude_radians: [0.0; 3],
+    };
+}
+impl FlightEstimate {
+    pub fn velocity_error_bound(&self) -> Vector3<Ned> {
+        let e: [f32; 3] = core::array::from_fn(|i| {
+            3.0 * kaptan_math::sqrt(self.velocity_variance_m2ps2[i])
+                + self.timing_error_bound.velocity_mps[i]
+        });
+        Vector3::new(e[0], e[1], e[2])
+    }
+    pub fn attitude_error_bound(&self) -> Vector3<Ned> {
+        let e: [f32; 3] = core::array::from_fn(|i| {
+            3.0 * kaptan_math::sqrt(self.attitude_variance_rad2[i])
+                + self.timing_error_bound.attitude_radians[i]
+        });
+        Vector3::new(e[0], e[1], e[2])
+    }
+}
+/// Four equal contact spheres at the corners of a body-FRD rectangle.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct ContactGeometry {
+    pub half_span_x_metres: f32,
+    pub half_span_y_metres: f32,
+    pub centre_z_metres: f32,
+    pub radius_metres: f32,
+}
+impl ContactGeometry {
+    pub(crate) fn validate(self) -> bool {
+        self.half_span_x_metres.is_finite()
+            && self.half_span_x_metres > 0.0
+            && self.half_span_y_metres.is_finite()
+            && self.half_span_y_metres > 0.0
+            && self.centre_z_metres.is_finite()
+            && self.radius_metres.is_finite()
+            && self.radius_metres >= 0.0
+            && (self.rotation_radius_bound() + self.radius_metres).is_finite()
+    }
+    fn rotation_radius_bound(self) -> f32 {
+        l1_bound(Vector3::new(
+            self.half_span_x_metres,
+            self.half_span_y_metres,
+            self.centre_z_metres,
+        ))
+    }
+    pub(crate) fn depth(self, q: UnitQuaternion<Frd, Ned>) -> f32 {
+        let mut depth = f32::NEG_INFINITY;
+        for x in [-self.half_span_x_metres, self.half_span_x_metres] {
+            for y in [-self.half_span_y_metres, self.half_span_y_metres] {
+                let foot = q.body_to_ned(Vector3::new(x, y, self.centre_z_metres)).z()
+                    + self.radius_metres;
+                if !foot.is_finite() {
+                    return f32::NAN;
+                }
+                depth = depth.max(foot);
+            }
+        }
+        depth
+    }
+}
+
+/// Selected source for each supported observation kind.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct NavigationSources {
+    pub imu: SourceId,
+    pub local_position_velocity: SourceId,
+    pub magnetic: SourceId,
+    pub pressure: SourceId,
+    pub range: SourceId,
+}
+impl NavigationSources {
+    pub(crate) const fn ordered(self) -> [SourceId; 5] {
+        [
+            self.imu,
+            self.local_position_velocity,
+            self.magnetic,
+            self.pressure,
+            self.range,
+        ]
+    }
+}
+
+/// Range-sensor extrinsics and deterministic quantization error.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct RangeInstallation {
+    pub frame: FrameId,
+    pub offset_frd_metres: Vector3<Frd>,
+    /// Finite nonzero beam direction, normalized once by navigation construction.
+    pub direction_frd: Vector3<Frd>,
+    pub quantization_error_metres: f32,
+}
+impl RangeInstallation {
+    fn validate(self) -> bool {
+        self.offset_frd_metres.is_finite()
+            && l1_bound(self.offset_frd_metres).is_finite()
+            && self.direction_frd.normalized().is_some()
+            && self.quantization_error_metres.is_finite()
+            && self.quantization_error_metres >= 0.0
+    }
+    pub(crate) fn lever_bound(self) -> f32 {
+        l1_bound(self.offset_frd_metres)
+    }
+}
+
+// The L1 norm bounds Euclidean radius. Round both additions outward.
+fn l1_bound(v: Vector3<Frd>) -> f32 {
+    ((v.x().abs() + v.y().abs()).next_up() + v.z().abs()).next_up()
+}
+
+/// Local isothermal pressure reference. The conversion accepts 0.95..1.05 of it.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct BarometricReference {
+    pub pressure_pascals: f32,
+    pub temperature_kelvin: f32,
+}
+impl BarometricReference {
+    pub(crate) fn height_scale(self) -> f32 {
+        287.05 * self.temperature_kelvin / GRAVITY_MPS2
+    }
+    fn validate(self) -> bool {
+        let lower = 0.95 * self.pressure_pascals;
+        let upper = 1.05 * self.pressure_pascals;
+        let scale = self.height_scale();
+        self.pressure_pascals.is_finite()
+            && self.pressure_pascals > 0.0
+            && self.temperature_kelvin.is_finite()
+            && self.temperature_kelvin > 0.0
+            && (self.pressure_pascals + upper).is_finite()
+            && scale.is_finite()
+            && (scale / lower).is_finite()
+            && scale / upper > 0.0
+    }
+}
+
+/// Required compiled installation. Calibration identifies one coherent set of
+/// mounting, calibration and uncertainty data, changed only by reinitialization.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct NavigationInstallation {
+    pub sources: NavigationSources,
+    pub range: RangeInstallation,
+    pub contact: ContactGeometry,
+    pub atmosphere: BarometricReference,
+    pub magnetic_field_ned_microtesla: Vector3<Ned>,
+    pub calibration: CalibrationRevision,
+    pub reference: SensorReferenceId,
+}
+impl NavigationInstallation {
+    fn validate(self) -> bool {
+        self.range.validate()
+            && self.contact.validate()
+            && self.atmosphere.validate()
+            && self.magnetic_field_ned_microtesla.is_finite()
+            && Vector3::<Ned>::new(
+                self.magnetic_field_ned_microtesla.x(),
+                self.magnetic_field_ned_microtesla.y(),
+                0.0,
+            )
+            .norm()
+                > 10.0
+            && self.magnetic_field_ned_microtesla.norm() < 100.0
+            && self.reference.0 != 0
+    }
+}
+
+/// Continuous process-noise densities in SI units.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct NoiseDensities {
+    pub gyro: f32,              // rad²/s
+    pub acceleration: f32,      // m²/s³
+    pub gyro_bias: f32,         // rad²/s³
+    pub acceleration_bias: f32, // m²/s⁵
+    pub barometric_datum: f32,  // m²/s
+}
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct NavigationConfig {
+    pub noise: NoiseDensities,
+    /// Acquisition-window, numerical-step and current-extrapolation ceiling.
+    pub maximum_imu_interval_microseconds: u64,
+    pub imu_freshness_microseconds: u64,
+    pub aiding_freshness_microseconds: u64,
+    pub maximum_time_uncertainty_microseconds: u32,
+    pub lag_microseconds: u64,
+    pub installation: NavigationInstallation,
+}
+impl NavigationConfig {
+    #[cfg(test)]
+    pub const fn engineering_fixture() -> Self {
+        Self {
+            noise: NoiseDensities {
+                gyro: 2.5e-7,
+                acceleration: 2.5e-5,
+                gyro_bias: 1e-10,
+                acceleration_bias: 1e-8,
+                barometric_datum: 1e-5,
+            },
+            maximum_imu_interval_microseconds: 20_000,
+            imu_freshness_microseconds: 20_000,
+            aiding_freshness_microseconds: 250_000,
+            maximum_time_uncertainty_microseconds: 2_000,
+            lag_microseconds: 100_000,
+            installation: NavigationInstallation {
+                sources: NavigationSources {
+                    imu: SourceId::new(1),
+                    local_position_velocity: SourceId::new(2),
+                    magnetic: SourceId::new(3),
+                    pressure: SourceId::new(4),
+                    range: SourceId::new(5),
+                },
+                range: RangeInstallation {
+                    frame: FrameId::new(3),
+                    offset_frd_metres: Vector3::new(0.0, 0.0, 0.04),
+                    direction_frd: Vector3::new(0.0, 0.0, 1.0),
+                    quantization_error_metres: 0.0005,
+                },
+                contact: ContactGeometry {
+                    half_span_x_metres: 0.12,
+                    half_span_y_metres: 0.12,
+                    centre_z_metres: 0.065,
+                    radius_metres: 0.015,
+                },
+                atmosphere: BarometricReference {
+                    pressure_pascals: 101_325.0,
+                    temperature_kelvin: 288.15,
+                },
+                magnetic_field_ned_microtesla: Vector3::new(20.0, 0.0, 45.0),
+                calibration: CalibrationRevision::new(1),
+                reference: SensorReferenceId(1),
+            },
+        }
+    }
+    pub fn validate(self) -> bool {
+        let n = self.noise;
+        [
+            n.gyro,
+            n.acceleration,
+            n.gyro_bias,
+            n.acceleration_bias,
+            n.barometric_datum,
+        ]
+        .iter()
+        .all(|v| v.is_finite() && *v > 0.0 && *v <= 1.0)
+            && (10_000..=20_000).contains(&self.maximum_imu_interval_microseconds)
+            && self.imu_freshness_microseconds >= self.maximum_imu_interval_microseconds
+            && self.imu_freshness_microseconds <= 20_000
+            && self.lag_microseconds == 100_000
+            && (200_000..=500_000).contains(&self.aiding_freshness_microseconds)
+            && self.maximum_time_uncertainty_microseconds <= 2_000
+            && self.installation.validate()
+    }
+}
+
+pub(crate) fn earliest(c: ObservationContext) -> Option<u64> {
+    c.measurement_time
+        .as_microseconds()
+        .checked_sub(u64::from(c.time_uncertainty_microseconds))
+}
+pub(crate) fn expiry(c: ObservationContext, age: u64) -> Option<Timestamp> {
+    Some(Timestamp::from_microseconds(earliest(c)?.checked_add(age)?))
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    fn contact() -> ContactGeometry {
+        NavigationConfig::engineering_fixture().installation.contact
+    }
+    fn time(v: u64) -> Timestamp {
+        Timestamp::from_microseconds(v)
+    }
+    fn estimate(at: Timestamp) -> FlightEstimate {
+        let capability = CapabilityEvidence {
+            available: true,
+            last_measurement_at: Some(at),
+            valid_until: Some(time(u64::MAX)),
+        };
+        FlightEstimate {
+            at,
+            fused_through: at,
+            last_imu_at: at,
+            origin: LocalOrigin {
+                id: OriginId(1),
+                established_at: at,
+                reference: SensorReferenceId(1),
+                launch_in_reference: PositionNed::metres(0.0, 0.0, 0.0),
+                launch_variance_m2: [0.0; 3],
+            },
+            attitude: UnitQuaternion::identity(),
+            position: PositionNed::metres(0.0, 0.0, 0.0),
+            velocity: VelocityNed::metres_per_second(0.1, 0.0, 0.0),
+            angular_rate: AngularRateFrd::radians_per_second(0.0, 0.0, 0.0),
+            position_variance_m2: [0.0; 3],
+            velocity_variance_m2ps2: [0.0; 3],
+            attitude_variance_rad2: [0.0; 3],
+            timing_error_bound: TimingErrorBounds::ZERO,
+            capabilities: FlightCapabilities {
+                tilt: capability,
+                heading: capability,
+                horizontal_position: capability,
+                horizontal_velocity: capability,
+                height: capability,
+                vertical_velocity: capability,
+                ground_clearance: capability,
+            },
+            ground_clearance: None,
+        }
+    }
+    #[test]
+    fn capability_expiry_requires_every_requested_capability() {
+        let evidence = CapabilityEvidence {
+            available: true,
+            last_measurement_at: Some(time(10)),
+            valid_until: Some(time(100)),
+        };
+        let healthy = FlightCapabilities {
+            tilt: evidence,
+            heading: evidence,
+            horizontal_position: evidence,
+            horizontal_velocity: evidence,
+            height: evidence,
+            vertical_velocity: evidence,
+            ground_clearance: evidence,
+        };
+        assert_eq!(healthy.valid_until(), Some(time(100)));
+        for index in 0..7 {
+            let mut c = healthy;
+            [
+                &mut c.tilt,
+                &mut c.heading,
+                &mut c.horizontal_position,
+                &mut c.horizontal_velocity,
+                &mut c.height,
+                &mut c.vertical_velocity,
+                &mut c.ground_clearance,
+            ]
+            .into_iter()
+            .nth(index)
+            .unwrap()
+            .available = false;
+            assert!(!c.flight_ready());
+            assert_eq!(c.valid_until(), None);
+        }
+        let mut c = healthy;
+        c.ground_clearance.valid_until = None;
+        assert_eq!(c.valid_until(), None);
+        c = healthy;
+        c.heading.valid_until = Some(time(50));
+        assert_eq!(c.valid_until(), Some(time(50)));
+    }
+    #[test]
+    fn clearance_bound_uses_earliest_capture_and_rejects_invalid_arithmetic() {
+        let c = GroundClearance {
+            measurement_at: time(10_000),
+            valid_until: time(60_000),
+            source: SourceId::new(5),
+            body_origin_height_metres: 0.08,
+            contact_clearance_metres: 0.0,
+            variance_m2: 4e-6,
+            measurement_time_uncertainty_microseconds: 1_000,
+            upper_error_metres: 0.01,
+        };
+        let e = estimate(time(20_000));
+        assert!((c.upper_at(time(20_000), &e, contact()).unwrap() - 0.012_612_5).abs() < 1e-7);
+        assert_eq!(
+            c.upper_at(time(60_000), &estimate(time(60_000)), contact()),
+            None
+        );
+        for speed in [f32::NAN, f32::INFINITY] {
+            let mut bad = e;
+            bad.velocity = VelocityNed::metres_per_second(speed, 0.0, 0.0);
+            assert_eq!(c.upper_at(time(20_000), &bad, contact()), None);
+        }
+        assert_eq!(
+            c.upper_at(time(8_000), &estimate(time(8_000)), contact()),
+            None
+        );
+        assert_eq!(c.upper_at(time(20_001), &e, contact()), None);
+        let mut bad_angle = e;
+        bad_angle.attitude_variance_rad2[0] = f32::NAN;
+        assert_eq!(c.upper_at(time(20_000), &bad_angle, contact()), None);
+        let mut invalid_contact = contact();
+        invalid_contact.radius_metres = -0.01;
+        assert_eq!(c.upper_at(time(20_000), &e, invalid_contact), None);
+        assert_eq!(
+            GroundClearance {
+                measurement_time_uncertainty_microseconds: 20_000,
+                ..c
+            }
+            .upper_at(time(20_000), &e, contact()),
+            None
+        );
+        assert_eq!(
+            GroundClearance {
+                valid_until: time(3_000_000),
+                ..c
+            }
+            .upper_at(
+                time(2_000_000),
+                &FlightEstimate {
+                    at: time(2_000_000),
+                    velocity: VelocityNed::metres_per_second(f32::MAX, 0.0, 0.0),
+                    ..e
+                },
+                contact(),
+            ),
+            None
+        );
+    }
+    #[test]
+    fn current_foot_geometry_and_velocity_confidence_bound_landing_clearance() {
+        // Independent rigid geometry: fixed COM 0.132 m; foot-sphere centres
+        // (+/-0.12,+/-0.12,0.065), radius 0.015. Roll changes 0.2 -> 0.
+        let old_depth = 0.12_f64 * 0.2_f64.sin() + 0.065 * 0.2_f64.cos() + 0.015;
+        let measured_contact = 0.132 - old_depth;
+        let old_bound = measured_contact + 0.0075 + 0.2 * 0.020 + 0.5 * 25.0 * 0.020_f64.powi(2);
+        assert!(old_bound < 0.05);
+        let actual_now = 0.132 - (0.065 + 0.015);
+        let c = GroundClearance {
+            measurement_at: time(10_000),
+            valid_until: time(70_000),
+            source: SourceId::new(5),
+            body_origin_height_metres: 0.132,
+            contact_clearance_metres: measured_contact as f32,
+            variance_m2: 4e-6,
+            measurement_time_uncertainty_microseconds: 0,
+            upper_error_metres: 0.0075,
+        };
+        let mut e = estimate(time(30_000));
+        e.velocity = VelocityNed::metres_per_second(0.0, 0.0, 0.0);
+        let upper = c.upper_at(e.at, &e, contact()).unwrap();
+        assert!(f64::from(upper) >= actual_now);
+        assert!(upper > 0.05); // neither cap entry nor contact dwell may use the old bound
+        e.velocity_variance_m2ps2 = [0.04; 3];
+        let uncertain = c.upper_at(e.at, &e, contact()).unwrap();
+        assert!(uncertain > upper + 0.020); // 3*sqrt(trace(Pv))*20 ms, not fixed +0.2 m/s
+    }
+    #[test]
+    fn contact_geometry() {
+        for geometry in [
+            contact(),
+            ContactGeometry {
+                half_span_x_metres: 0.3,
+                half_span_y_metres: 0.2,
+                centre_z_metres: 0.15,
+                radius_metres: 0.025,
+            },
+        ] {
+            let x = f64::from(geometry.half_span_x_metres);
+            let y = f64::from(geometry.half_span_y_metres);
+            let z = f64::from(geometry.centre_z_metres);
+            assert!(f64::from(geometry.rotation_radius_bound()) >= (x * x + y * y + z * z).sqrt());
+            for (roll, pitch) in [(-0.5_f64, 0.2_f64), (0.0, 0.0), (0.4, -0.3)] {
+                let (sr, cr) = ((roll / 2.0).sin(), (roll / 2.0).cos());
+                let (sp, cp) = ((pitch / 2.0).sin(), (pitch / 2.0).cos());
+                let q = UnitQuaternion::from_components(
+                    (cp * cr) as f32,
+                    (cp * sr) as f32,
+                    (sp * cr) as f32,
+                    (-sp * sr) as f32,
+                )
+                .unwrap();
+                let expected = x * pitch.sin().abs()
+                    + y * (pitch.cos() * roll.sin()).abs()
+                    + z * pitch.cos() * roll.cos()
+                    + f64::from(geometry.radius_metres);
+                assert!((f64::from(geometry.depth(q)) - expected).abs() < 1e-7);
+                let e = FlightEstimate {
+                    attitude: q,
+                    ..estimate(time(10_000))
+                };
+                let c = GroundClearance {
+                    measurement_at: e.at,
+                    valid_until: time(70_000),
+                    source: SourceId::new(90),
+                    body_origin_height_metres: 1.0,
+                    contact_clearance_metres: 0.0,
+                    variance_m2: 1e-6,
+                    measurement_time_uncertainty_microseconds: 0,
+                    upper_error_metres: 0.001,
+                };
+                assert!(f64::from(c.upper_at(e.at, &e, geometry).unwrap()) >= 1.0 - expected);
+            }
+        }
+    }
+    #[test]
+    fn installation_validation() {
+        let fixture = NavigationConfig::engineering_fixture();
+        for field in 0..9 {
+            for bad in [f32::NAN, f32::INFINITY, -1.0] {
+                let mut config = fixture;
+                let i = &mut config.installation;
+                match field {
+                    0 => i.contact.half_span_x_metres = bad,
+                    1 => i.contact.half_span_y_metres = bad,
+                    2 => i.contact.radius_metres = bad,
+                    3 => i.range.quantization_error_metres = bad,
+                    4 => i.atmosphere.pressure_pascals = bad,
+                    5 => i.atmosphere.temperature_kelvin = bad,
+                    6 if !bad.is_finite() => i.contact.centre_z_metres = bad,
+                    7 if !bad.is_finite() => {
+                        i.range.offset_frd_metres = Vector3::new(bad, 0.0, 0.0)
+                    }
+                    8 if !bad.is_finite() => i.range.direction_frd = Vector3::new(0.0, 0.0, bad),
+                    _ => continue,
+                }
+                assert!(Navigation::new(config).is_err());
+            }
+        }
+        for field in 0..6 {
+            let mut config = fixture;
+            let i = &mut config.installation;
+            match field {
+                0 => i.range.direction_frd = Vector3::zero(),
+                1 => i.contact.half_span_x_metres = 0.0,
+                2 => i.contact.half_span_y_metres = 0.0,
+                3 => i.atmosphere.pressure_pascals = 0.0,
+                4 => i.atmosphere.temperature_kelvin = 0.0,
+                _ => i.atmosphere.pressure_pascals = f32::MAX,
+            }
+            assert!(Navigation::new(config).is_err());
+        }
+        let mut config = fixture;
+        config.installation.contact.half_span_x_metres = f32::MAX;
+        config.installation.contact.half_span_y_metres = f32::MAX;
+        assert!(Navigation::new(config).is_err());
+        config = fixture;
+        config.installation.range.offset_frd_metres = Vector3::new(f32::MAX, f32::MAX, 0.0);
+        assert!(Navigation::new(config).is_err());
+        for (pressure_pascals, temperature_kelvin) in [
+            (101_325.0, f32::MAX),
+            (f32::MIN_POSITIVE, 288.15),
+            (1e20, f32::from_bits(1)),
+        ] {
+            config = fixture;
+            config.installation.atmosphere = BarometricReference {
+                pressure_pascals,
+                temperature_kelvin,
+            };
+            assert!(Navigation::new(config).is_err());
+        }
+    }
+    #[test]
+    fn navigation_configuration_rejects_each_invalid_domain() {
+        let factory = NavigationConfig::engineering_fixture();
+        assert!(factory.validate());
+        for index in 0..5 {
+            for bad in [f32::NAN, 0.0, -1.0, 1.01] {
+                let mut c = factory;
+                *[
+                    &mut c.noise.gyro,
+                    &mut c.noise.acceleration,
+                    &mut c.noise.gyro_bias,
+                    &mut c.noise.acceleration_bias,
+                    &mut c.noise.barometric_datum,
+                ]
+                .into_iter()
+                .nth(index)
+                .unwrap() = bad;
+                assert!(!c.validate());
+                assert!(crate::Navigation::new(c).is_err());
+            }
+        }
+        let mut c = factory;
+        c.maximum_imu_interval_microseconds = 9_999;
+        assert!(!c.validate());
+        c = factory;
+        c.imu_freshness_microseconds = 19_999;
+        assert!(!c.validate());
+        c = factory;
+        c.imu_freshness_microseconds = 20_001;
+        assert!(!c.validate());
+        c = factory;
+        c.lag_microseconds = 0;
+        assert!(!c.validate());
+        c = factory;
+        c.aiding_freshness_microseconds = 100_000;
+        assert!(!c.validate());
+        c = factory;
+        c.maximum_time_uncertainty_microseconds = 2_001;
+        assert!(!c.validate());
+        c = factory;
+        c.installation.magnetic_field_ned_microtesla = Vector3::new(f32::NAN, 0.0, 45.0);
+        assert!(!c.validate());
+        c = factory;
+        c.installation.magnetic_field_ned_microtesla = Vector3::new(10.0, 0.0, 45.0);
+        assert!(!c.validate());
+        c = factory;
+        c.installation.magnetic_field_ned_microtesla = Vector3::new(20.0, f32::MAX, 45.0);
+        assert!(!c.validate());
+        c = factory;
+        c.installation.reference = SensorReferenceId(0);
+        assert!(!c.validate());
+        c = factory;
+        c.installation.magnetic_field_ned_microtesla = Vector3::new(0.0, 20.0, 45.0);
+        assert!(c.validate());
+    }
+}

firmware/libs/kaptan-navigation/src/lib.rs

@@ -1,41 +1,27 @@
-//! Navigation observation contracts and a SITL-only raw-rate ESKF.
-//!
-//! Navigation owns the conversion from driver integer observations into SI
-//! `f32` values. Board and platform crates must not manufacture navigation
-//! observations or duplicate their frame/calibration metadata.
+//! Allocation-free sensor-driven navigation shared by host and MCU endpoints.
 
 #![no_std]
+#![forbid(unsafe_code)]
 
-#[cfg(all(feature = "sitl", target_os = "none"))]
-compile_error!(
-    "kaptan-navigation's raw-rate ESKF is restricted to the SITL executable; \
-     it is not available to a board target"
-);
-
-#[cfg(any(test, feature = "sitl"))]
-mod eskf;
+mod filter;
+mod flight;
+mod navigation;
 mod observation;
+mod timing;
+
+pub use flight::*;
+pub use navigation::Navigation;
 
 pub use observation::{
-    AccelerationFrd, AngularRateFrd, CalibrationRevision, Covariance2, FrameId,
-    LocalPositionVelocityObservation, ObservationContext, ObservationSequence,
-    OpticalFlowObservation, PositionFrd, PositionNed, RangeObservation, ScalarVariance,
-    SensorExtrinsics, SourceId, Validity, VelocityNed,
+    AccelerationFrd, AngularRateFrd, CalibrationRevision, FrameId,
+    LocalPositionVelocityObservation, ObservationContext, ObservationSequence, PositionNed,
+    RangeObservation, ScalarVariance, SourceId, Validity, VelocityNed,
 };
 
-/// Internal-SITL API for the provisional raw-rate ESKF.
-///
-/// It supports the host simulator only. It cannot compile for a bare-metal
-/// target and must not be used as a flight-estimator API.
-#[cfg(any(test, feature = "sitl"))]
-pub mod sitl {
-    pub use crate::eskf::{
-        Eskf, EstimatorCapabilities, EstimatorConfig, EstimatorHealth, FusionError, FusionResult,
-        InertialEskf, IntegrityConfig, LocalTerrainEskf, NominalState, PropagationError,
-        SourceStatus, StationaryInitializer,
-    };
-    pub use crate::observation::{ImuNoise, ImuObservation};
-}
-
 /// Standard gravity in metres per second squared.
 pub const GRAVITY_MPS2: f32 = 9.806_65;
+
+/// Supported free-flight acceleration bound in m/s², including gravity
+/// and maximum collective thrust. Flight configuration checks T_max/m + g
+/// against this bound. It does not bound contact impulses.
+pub const MAXIMUM_FREE_ACCELERATION_MPS2: f32 = 25.0;

firmware/libs/kaptan-navigation/src/observation.rs

@@ -1,15 +1,10 @@
 //! Typed SI observations and their calibration/frame provenance.
 
-use kaptan_math::{Frd, Ned, Sensor, UnitQuaternion, Vector2, Vector3};
-#[cfg(any(test, feature = "sitl"))]
-use kaptan_sensor::imu::ImuSnapshot;
-#[cfg(any(test, feature = "sitl"))]
-use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
+use kaptan_math::{Frd, Ned, Vector3};
+#[cfg(any(test, feature = "sensor-snapshots"))]
+use kaptan_sensor::range::RangeSnapshot;
 use kaptan_time::Timestamp;
 
-#[cfg(any(test, feature = "sitl"))]
-use crate::GRAVITY_MPS2;
-
 /// Origin of one observation stream, allocated by a static board configuration.
 #[derive(Clone, Copy, Debug, Eq, PartialEq)]
 pub struct SourceId(u8);
@@ -32,6 +27,7 @@ impl FrameId {
     pub const BODY_FRD: Self = Self(0);
     /// Active launch-local navigation frame with north/east/down axes.
     pub const LOCAL_NED: Self = Self(1);
+    pub const SCALAR: Self = Self(2);
     /// Creates a configured sensor frame identifier.
     pub const fn new(value: u8) -> Self {
         Self(value)
@@ -44,11 +40,9 @@ impl FrameId {
 
 /// Monotonic revision of the calibration/extrinsics used for an observation.
 ///
-/// TODO(configuration): this is a provenance token, not a calibration store.
-/// The future configuration service must atomically install sensor mounting,
-/// scale/bias calibration, uncertainty, and this revision while navigation is
-/// stopped, then require reinitialization before admitting observations from
-/// the new revision.
+/// This token identifies a coherent mounting, calibration and uncertainty set.
+/// Changing the set requires navigation to stop and reinitialize before it
+/// accepts observations carrying the new revision.
 #[derive(Clone, Copy, Debug, Eq, PartialEq)]
 pub struct CalibrationRevision(u32);
 impl CalibrationRevision {
@@ -86,6 +80,13 @@ pub enum Validity {
 }
 
 /// Common provenance for every navigation observation.
+///
+/// The caller must map physical sample time into the controller clock, accounting
+/// for sensor delay. A snapshot's request or reception timestamp is insufficient.
+/// Without that mapping, withhold the observation or mark it [`Validity::Invalid`].
+/// Receipt time must be at or after the snapshot timestamp in the same clock domain.
+/// Snapshot conversion preserves these times and changes only units. The estimator
+/// checks ordering and freshness on admission.
 #[derive(Clone, Copy, Debug, Eq, PartialEq)]
 pub struct ObservationContext {
     /// Sensor measurement time synchronized into the controller monotonic domain.
@@ -104,32 +105,8 @@ pub struct ObservationContext {
     pub validity: Validity,
     /// Source sequence number.
     pub sequence: ObservationSequence,
-}
-
-impl ObservationContext {
-    /// Creates provenance shared by a concrete observation value.
-    #[allow(clippy::too_many_arguments)]
-    pub const fn new(
-        measurement_time: Timestamp,
-        receipt_time: Timestamp,
-        source: SourceId,
-        frame: FrameId,
-        calibration: CalibrationRevision,
-        quality: u8,
-        validity: Validity,
-        sequence: ObservationSequence,
-    ) -> Self {
-        Self {
-            measurement_time,
-            receipt_time,
-            source,
-            frame,
-            calibration,
-            quality,
-            validity,
-            sequence,
-        }
-    }
+    /// Hard absolute clock-mapping error, not a statistical standard deviation.
+    pub time_uncertainty_microseconds: u32,
 }
 
 /// Positive finite scalar measurement variance in squared SI units.
@@ -150,29 +127,13 @@ impl ScalarVariance {
     }
 }
 
-/// Symmetric two-dimensional covariance in squared SI units.
-#[derive(Clone, Copy, Debug, PartialEq)]
-pub struct Covariance2 {
-    /// xx variance.
-    pub xx: ScalarVariance,
-    /// xy covariance.
-    pub xy: f32,
-    /// yy variance.
-    pub yy: ScalarVariance,
-}
-impl Covariance2 {
-    /// Validates finite covariance and a positive determinant.
-    pub fn new(xx: ScalarVariance, xy: f32, yy: ScalarVariance) -> Option<Self> {
-        let determinant = xx.get() * yy.get() - xy * xy;
-        if xy.is_finite() && determinant.is_finite() && determinant > 0.0 {
-            Some(Self { xx, xy, yy })
-        } else {
-            None
-        }
-    }
-}
-
 /// NED position in metres.
+///
+/// Position and velocity remain distinct even in the same coordinate frame.
+/// ```compile_fail,E0308
+/// use kaptan_navigation::{PositionNed, VelocityNed};
+/// let position: PositionNed = VelocityNed::metres_per_second(0.0, 0.0, 0.0);
+/// ```
 #[derive(Clone, Copy, Debug, PartialEq)]
 pub struct PositionNed(Vector3<Ned>);
 impl PositionNed {
@@ -203,8 +164,7 @@ impl VelocityNed {
 /// Complete position and velocity aiding in the active local-NED frame.
 ///
 /// Position and velocity share one source timestamp and sequence because the
-/// producer measures the same vehicle state. Their diagonal variances remain
-/// explicit rather than being hidden in a simulator or transport adapter.
+/// producer measures the same vehicle state. Variances use squared SI units.
 #[derive(Clone, Copy, Debug, PartialEq)]
 pub struct LocalPositionVelocityObservation {
     /// Required source/time/frame provenance.
@@ -214,23 +174,11 @@ pub struct LocalPositionVelocityObservation {
     /// Absolute local-NED velocity in metres per second.
     pub velocity: VelocityNed,
     /// Per-axis position measurement variance in m².
-    pub position_variance: ScalarVariance,
+    pub position_variance: [ScalarVariance; 3],
     /// Per-axis velocity measurement variance in (m/s)².
-    pub velocity_variance: ScalarVariance,
-}
-
-/// Body FRD position offset in metres.
-#[derive(Clone, Copy, Debug, PartialEq)]
-pub struct PositionFrd(Vector3<Frd>);
-impl PositionFrd {
-    /// Creates a body-frame offset in metres.
-    pub const fn metres(forward: f32, right: f32, down: f32) -> Self {
-        Self(Vector3::new(forward, right, down))
-    }
-    /// Returns its body vector.
-    pub const fn vector(self) -> Vector3<Frd> {
-        self.0
-    }
+    pub velocity_variance: [ScalarVariance; 3],
+    /// Fixed sensor-reference coordinates. Navigation alone subtracts launch.
+    pub reference: crate::SensorReferenceId,
 }
 
 /// Body FRD specific force in metres per second squared.
@@ -261,57 +209,6 @@ impl AngularRateFrd {
     }
 }
 
-/// Configured IMU white-noise variances in SI squared units.
-#[derive(Clone, Copy, Debug, PartialEq)]
-#[cfg(any(test, feature = "sitl"))]
-pub struct ImuNoise {
-    /// Gyro rate variance, (rad/s)^2.
-    pub gyro_rate: ScalarVariance,
-    /// Accelerometer specific-force variance, (m/s²)^2.
-    pub acceleration: ScalarVariance,
-}
-
-/// IMU observation expressed in the calibrated aircraft FRD frame.
-#[derive(Clone, Copy, Debug, PartialEq)]
-#[cfg(any(test, feature = "sitl"))]
-pub struct ImuObservation {
-    /// Required observation provenance.
-    pub context: ObservationContext,
-    /// Specific force in m/s².
-    pub acceleration: AccelerationFrd,
-    /// Angular rate in rad/s.
-    pub angular_rate: AngularRateFrd,
-    /// Explicit configured uncertainty.
-    pub noise: ImuNoise,
-}
-
-#[cfg(any(test, feature = "sitl"))]
-impl ImuObservation {
-    /// Converts the existing integer IMU contract to SI exactly at the
-    /// navigation boundary. The board has already aligned this snapshot to FRD.
-    pub fn from_snapshot(
-        snapshot: ImuSnapshot,
-        mut context: ObservationContext,
-        noise: ImuNoise,
-    ) -> Self {
-        context.measurement_time = snapshot.timestamp;
-        Self {
-            context,
-            acceleration: AccelerationFrd::metres_per_second_squared(
-                f32::from(snapshot.acceleration_milligravity[0]) * GRAVITY_MPS2 / 1_000.0,
-                f32::from(snapshot.acceleration_milligravity[1]) * GRAVITY_MPS2 / 1_000.0,
-                f32::from(snapshot.acceleration_milligravity[2]) * GRAVITY_MPS2 / 1_000.0,
-            ),
-            angular_rate: AngularRateFrd::radians_per_second(
-                f32::from(snapshot.angular_rate_milliradians_per_second[0]) / 1_000.0,
-                f32::from(snapshot.angular_rate_milliradians_per_second[1]) / 1_000.0,
-                f32::from(snapshot.angular_rate_milliradians_per_second[2]) / 1_000.0,
-            ),
-            noise,
-        }
-    }
-}
-
 /// Range observation in metres along the sensor's positive z axis.
 #[derive(Clone, Copy, Debug, PartialEq)]
 pub struct RangeObservation {
@@ -325,13 +222,14 @@ pub struct RangeObservation {
 
 impl RangeObservation {
     /// Converts a driver range value to metres at the navigation boundary.
-    #[cfg(any(test, feature = "sitl"))]
+    ///
+    /// Uses the caller's timestamps under the [`ObservationContext`] timing contract.
+    #[cfg(any(test, feature = "sensor-snapshots"))]
     pub fn from_snapshot(
         snapshot: RangeSnapshot,
-        mut context: ObservationContext,
+        context: ObservationContext,
         variance: ScalarVariance,
     ) -> Self {
-        context.measurement_time = snapshot.timestamp;
         Self {
             context,
             metres: snapshot.distance_millimetres as f32 / 1_000.0,
@@ -340,115 +238,23 @@ impl RangeObservation {
     }
 }
 
-/// Ground-referenced XY velocity reported in a named sensor frame.
-#[derive(Clone, Copy, Debug, PartialEq)]
-pub struct OpticalFlowObservation {
-    /// Required observation provenance.
-    pub context: ObservationContext,
-    /// Sensor-frame x/y velocity in m/s.
-    pub velocity: Vector2<Sensor>,
-    /// Ground distance in metres; retained for plausibility/diagnostics.
-    pub ground_distance_metres: f32,
-    /// Explicit XY measurement covariance in (m/s)².
-    pub covariance: Covariance2,
-}
-
-impl OpticalFlowObservation {
-    /// Converts the existing MTF-01-independent driver value into SI.
-    #[cfg(any(test, feature = "sitl"))]
-    pub fn from_snapshot(
-        snapshot: OpticalFlowSnapshot,
-        mut context: ObservationContext,
-        covariance: Covariance2,
-    ) -> Self {
-        context.measurement_time = snapshot.timestamp;
-        context.quality = snapshot.quality;
-        Self {
-            context,
-            velocity: Vector2::new(
-                snapshot.velocity_millimetres_per_second[0] as f32 / 1_000.0,
-                snapshot.velocity_millimetres_per_second[1] as f32 / 1_000.0,
-            ),
-            ground_distance_metres: snapshot.ground_distance_millimetres as f32 / 1_000.0,
-            covariance,
-        }
-    }
-}
-
-/// Static calibrated transform from a sensor frame into aircraft FRD.
-///
-/// TODO(configuration): this remains a build-time value until the
-/// configuration service can stage the full sensor calibration bundle. Do not
-/// expose setters for individual fields: source identity, frame, calibration
-/// revision, rotation, and lever arm must change together and only while the
-/// estimator and actuators are inhibited.
-#[derive(Clone, Copy, Debug, PartialEq)]
-pub struct SensorExtrinsics {
-    /// Statically configured source expected for this physical device.
-    pub source: SourceId,
-    /// Frame ID expected in every observation from this device.
-    pub sensor_frame: FrameId,
-    /// Calibration/extrinsics revision represented by this transform.
-    pub calibration: CalibrationRevision,
-    /// Rotation mapping sensor vectors into FRD.
-    pub sensor_to_body: UnitQuaternion<Sensor, Frd>,
-    /// Sensor origin relative to aircraft origin, in FRD metres.
-    pub position_body: PositionFrd,
-}
-
-impl SensorExtrinsics {
-    /// Maps a sensor vector into FRD at the sole sensor-calibration boundary.
-    pub fn sensor_to_body_vector(self, value: Vector3<Sensor>) -> Vector3<Frd> {
-        self.sensor_to_body.rotate(value)
-    }
-}
-
 #[cfg(test)]
 mod tests {
     use super::*;
-    use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
+    use kaptan_sensor::range::RangeSnapshot;
 
     fn context(frame: FrameId) -> ObservationContext {
-        ObservationContext::new(
-            Timestamp::from_microseconds(10),
-            Timestamp::from_microseconds(12),
-            SourceId::new(2),
+        ObservationContext {
+            measurement_time: Timestamp::from_microseconds(95),
+            receipt_time: Timestamp::from_microseconds(120),
+            source: SourceId::new(2),
             frame,
-            CalibrationRevision::new(3),
-            250,
-            Validity::Valid,
-            ObservationSequence::new(9),
-        )
-    }
-
-    #[test]
-    fn imu_snapshot_is_si() {
-        let noise = ImuNoise {
-            gyro_rate: ScalarVariance::new(0.01).unwrap(),
-            acceleration: ScalarVariance::new(0.02).unwrap(),
-        };
-        let sample = ImuObservation::from_snapshot(
-            ImuSnapshot::new(
-                Timestamp::from_microseconds(10),
-                [1_000, 0, -1_000],
-                [1_000, 0, 0],
-                0,
-            ),
-            context(FrameId::BODY_FRD),
-            noise,
-        );
-        assert!((sample.acceleration.vector().x() - GRAVITY_MPS2).abs() < 1.0e-5);
-        assert!((sample.angular_rate.vector().x() - 1.0).abs() < 1.0e-6);
-        assert_eq!(sample.context.calibration, CalibrationRevision::new(3));
-    }
-
-    #[test]
-    fn covariance_validation() {
-        let variance = ScalarVariance::new(1.0).unwrap();
-        assert!(Covariance2::new(variance, 1.0, variance).is_none());
-        let large = ScalarVariance::new(f32::MAX).unwrap();
-        assert!(Covariance2::new(large, 0.0, large).is_none());
-        assert!(Covariance2::new(variance, f32::NAN, variance).is_none());
+            calibration: CalibrationRevision::new(3),
+            quality: 250,
+            validity: Validity::Valid,
+            sequence: ObservationSequence::new(9),
+            time_uncertainty_microseconds: 0,
+        }
     }
 
     #[test]
@@ -463,55 +269,24 @@ mod tests {
     }
 
     #[test]
-    fn snapshots_convert_at_boundary() {
+    fn range_conversion_preserves_established_times() {
         let variance = ScalarVariance::new(0.25).unwrap();
         let range = RangeObservation::from_snapshot(
-            RangeSnapshot::new(Timestamp::from_microseconds(20), 1_234),
+            RangeSnapshot::new(Timestamp::from_microseconds(100), 1_234),
             context(FrameId::new(4)),
             variance,
         );
-        assert_eq!(
-            range.context.measurement_time,
-            Timestamp::from_microseconds(20)
-        );
+        assert_eq!(range.context, context(FrameId::new(4)));
         assert!((range.metres - 1.234).abs() < 1.0e-6);
-
-        let covariance = Covariance2::new(variance, 0.01, variance).unwrap();
-        let flow = OpticalFlowObservation::from_snapshot(
-            OpticalFlowSnapshot::new(Timestamp::from_microseconds(21), [-500, 250], 2_500, 42),
-            context(FrameId::new(5)),
-            covariance,
-        );
-        assert_eq!(
-            flow.context.measurement_time,
-            Timestamp::from_microseconds(21)
-        );
-        assert_eq!(flow.context.quality, 42);
-        assert_eq!((flow.velocity.x(), flow.velocity.y()), (-0.5, 0.25));
-        assert_eq!(flow.ground_distance_metres, 2.5);
     }
 
     #[test]
     fn observation_frames() {
-        let noise = ImuNoise {
-            gyro_rate: ScalarVariance::new(0.01).unwrap(),
-            acceleration: ScalarVariance::new(0.02).unwrap(),
-        };
-        let sample = ImuObservation::from_snapshot(
-            ImuSnapshot::new(Timestamp::from_microseconds(10), [0; 3], [0; 3], 0),
-            context(FrameId::BODY_FRD),
-            noise,
-        );
-        assert_eq!(
-            sample.context.measurement_time,
-            Timestamp::from_microseconds(10)
-        );
         assert_eq!(PositionNed::metres(1.0, 2.0, 3.0).vector().z(), 3.0);
         assert_eq!(
             VelocityNed::metres_per_second(1.0, 2.0, 3.0).vector().y(),
             2.0
         );
-        assert_eq!(PositionFrd::metres(1.0, 2.0, 3.0).vector().x(), 1.0);
         assert_eq!(
             AccelerationFrd::metres_per_second_squared(1.0, 2.0, 3.0)
                 .vector()
@@ -524,17 +299,5 @@ mod tests {
                 .z(),
             3.0
         );
-
-        let extrinsics = SensorExtrinsics {
-            source: SourceId::new(2),
-            sensor_frame: FrameId::new(5),
-            calibration: CalibrationRevision::new(3),
-            sensor_to_body: UnitQuaternion::identity(),
-            position_body: PositionFrd::metres(0.0, 0.0, 0.0),
-        };
-        assert_eq!(
-            extrinsics.sensor_to_body_vector(Vector3::new(1.0, 2.0, 3.0)),
-            Vector3::new(1.0, 2.0, 3.0)
-        );
     }
 }

firmware/libs/kaptan-navigation/src/timing.rs

@@ -0,0 +1,335 @@
+//! Correlated, bounded timing-model error, separate from Gaussian covariance.
+//!
+//! `shape` encloses deterministic error in an ellipsoid. Linear transformations
+//! preserve state correlations. Minkowski addition covers arbitrary correlation
+//! between old and new bounded errors, including one persistent clock offset.
+//! It is NOT independent measurement noise and is never added to Gaussian P.
+
+use crate::{
+    NavigationFailure,
+    filter::{Matrix, N, ZERO, sandwich},
+};
+use kaptan_math::sqrt;
+
+#[derive(Clone, Copy)]
+pub(crate) struct TimingEnvelope {
+    shape: Matrix,
+}
+
+impl TimingEnvelope {
+    pub const ZERO: Self = Self { shape: ZERO };
+
+    pub fn valid(&self) -> bool {
+        self.shape.iter().flatten().all(|v| v.is_finite())
+            && (0..N).all(|i| self.shape[i][i] >= 0.0)
+    }
+
+    pub fn radius(&self, axis: usize) -> f32 {
+        sqrt(self.shape[axis][axis])
+    }
+
+    pub fn norm_bound(&self, start: usize, count: usize) -> f32 {
+        // trace bounds the largest eigenvalue, so this bounds the Euclidean
+        // norm of the requested projection of the ellipsoid, not a sigma.
+        sqrt((start..start + count).map(|i| self.shape[i][i]).sum())
+    }
+
+    pub fn projected(&self, h: &[f32; N]) -> Result<f32, NavigationFailure> {
+        let mut squared = 0.0;
+        for (i, row) in self.shape.iter().enumerate() {
+            for (j, value) in row.iter().enumerate() {
+                squared += h[i] * value * h[j];
+            }
+        }
+        if !squared.is_finite() || squared < 0.0 {
+            return Err(NavigationFailure::Numerical);
+        }
+        Ok(sqrt(squared))
+    }
+
+    pub fn transform(&mut self, a: &Matrix) -> Result<(), NavigationFailure> {
+        if self.shape == ZERO {
+            return Ok(());
+        }
+        // Reject nonfinite shape before skipping zero coefficients, which would
+        // otherwise hide 0*Inf/NaN failures.
+        if !self.shape.iter().flatten().all(|v| v.is_finite()) {
+            return Err(NavigationFailure::Numerical);
+        }
+        let mut next = sandwich(a, &self.shape);
+        let columns: [f32; N] = core::array::from_fn(|k| a.iter().map(|r| r[k].abs()).sum());
+        for (i, row) in next.iter_mut().enumerate() {
+            let mut absolute_products = [0.0; N];
+            for (j, aj) in a[i].iter().enumerate() {
+                if *aj != 0.0 {
+                    for (k, ap) in absolute_products.iter_mut().enumerate() {
+                        *ap += aj.abs() * self.shape[j][k].abs();
+                    }
+                }
+            }
+            let mut absolute_row = 0.0;
+            for (ap, column) in absolute_products.iter().zip(&columns) {
+                absolute_row += ap * column;
+            }
+            // Outward arithmetic enclosure, not a repair of Gaussian P. The
+            // 16-term products need < gamma_64; 256 eps covers the computed
+            // majorant and diagonal addition too. The absolute term also
+            // dominates underflow/flush-to-zero at these fixed matrix sizes.
+            row[i] = (row[i] + absolute_row * (256.0 * f32::EPSILON) + 1e-30).next_up();
+        }
+        self.commit(next)
+    }
+
+    pub fn add_generator(&mut self, g: [f32; N]) -> Result<(), NavigationFailure> {
+        if !g.iter().all(|v| v.is_finite()) {
+            return Err(NavigationFailure::Numerical);
+        }
+        if g.iter().all(|v| *v == 0.0) {
+            return Ok(());
+        }
+        let old_trace: f32 = (0..N).map(|i| self.shape[i][i]).sum();
+        let new_trace: f32 = g.iter().map(|v| v * v).sum();
+        if !old_trace.is_finite() || !new_trace.is_finite() {
+            return Err(NavigationFailure::Numerical);
+        }
+        let (old_weight, new_weight) = if old_trace == 0.0 {
+            (0.0, 1.0)
+        } else {
+            // Every positive beta is a valid Minkowski enclosure. This trace
+            // choice in the declared SI coordinates reduces its size. Limiting
+            // beta is numerical conditioning, not a limit on admitted timing.
+            let old_norm = sqrt(old_trace);
+            let new_norm = sqrt(new_trace.max(1e-30));
+            let beta = if new_norm > old_norm * 1e6 {
+                1e6
+            } else if new_norm < old_norm * 1e-6 {
+                1e-6
+            } else {
+                new_norm / old_norm
+            };
+            (1.0 + beta, 1.0 + 1.0 / beta)
+        };
+        let mut next = ZERO;
+        for (i, row) in next.iter_mut().enumerate() {
+            let mut magnitude = 0.0;
+            for (j, value) in row.iter_mut().enumerate() {
+                let left = old_weight * self.shape[i][j];
+                let right = new_weight * g[i] * g[j];
+                *value = left + right;
+                magnitude += left.abs() + right.abs();
+            }
+            row[i] = (row[i] + magnitude * (32.0 * f32::EPSILON) + 1e-30).next_up();
+        }
+        self.commit(next)
+    }
+
+    fn commit(&mut self, mut next: Matrix) -> Result<(), NavigationFailure> {
+        for i in 0..N {
+            for j in 0..i {
+                let value = next[i][j] * 0.5 + next[j][i] * 0.5;
+                next[i][j] = value;
+                next[j][i] = value;
+            }
+        }
+        let candidate = Self { shape: next };
+        if !candidate.valid() {
+            return Err(NavigationFailure::Numerical);
+        }
+        *self = candidate;
+        Ok(())
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::filter::identity;
+
+    impl TimingEnvelope {
+        fn former_transform(&mut self, a: &Matrix) -> Result<(), NavigationFailure> {
+            if self.shape == ZERO {
+                return Ok(());
+            }
+            let mut next = sandwich(a, &self.shape);
+            let columns: [f32; N] = core::array::from_fn(|k| a.iter().map(|r| r[k].abs()).sum());
+            for (i, row) in next.iter_mut().enumerate() {
+                let mut absolute_row = 0.0;
+                for (k, column) in columns.iter().enumerate() {
+                    let mut ap = 0.0;
+                    for (j, aj) in a[i].iter().enumerate() {
+                        ap += aj.abs() * self.shape[j][k].abs();
+                    }
+                    absolute_row += ap * column;
+                }
+                // Outward arithmetic enclosure, not a repair of Gaussian P. The
+                // 16-term products need < gamma_64; 256 eps covers the computed
+                // majorant and diagonal addition too. The absolute term also
+                // dominates underflow/flush-to-zero at these fixed matrix sizes.
+                row[i] = (row[i] + absolute_row * (256.0 * f32::EPSILON) + 1e-30).next_up();
+            }
+            self.commit(next)
+        }
+    }
+
+    #[test]
+    fn sparse_absolute_products_preserve_the_dense_enclosure_and_refusal() {
+        fn compare(shape: Matrix, a: Matrix) {
+            let mut previous = TimingEnvelope { shape };
+            let mut current = previous;
+            assert_eq!(current.transform(&a), previous.former_transform(&a));
+            // Includes the retained input on refusal. Check signs of zero and
+            // original nonfinite payloads as well as every finite result bit.
+            for (actual, expected) in current
+                .shape
+                .iter()
+                .flatten()
+                .zip(previous.shape.iter().flatten())
+            {
+                assert_eq!(actual.to_bits(), expected.to_bits());
+            }
+        }
+        let mut sparse = identity();
+        sparse[0][3] = -0.02;
+        sparse[3][7] = 0.196133;
+        sparse[6][9] = -0.02;
+        let dense = core::array::from_fn(|i| {
+            core::array::from_fn(|j| ((i * 7 + j * 3) as f32 - 47.0) * 0.03125)
+        });
+        for scale in [
+            -0.0,
+            f32::from_bits(1),
+            f32::MIN_POSITIVE,
+            1e-20,
+            0.25,
+            1e20,
+            f32::MAX,
+        ] {
+            let shape = core::array::from_fn(|i| {
+                core::array::from_fn(|j| if i == j { scale } else { scale * 0.01 })
+            });
+            for a in [ZERO, identity(), sparse, dense] {
+                compare(shape, a);
+            }
+        }
+        for bad in [f32::NAN, f32::INFINITY, f32::NEG_INFINITY] {
+            for (row, column) in [(0, 0), (12, 4)] {
+                let mut shape = identity();
+                shape[row][column] = bad;
+                for a in [ZERO, identity(), sparse, dense] {
+                    compare(shape, a);
+                }
+            }
+            let mut a = identity();
+            a[0][1] = bad;
+            compare(ZERO, a); // The existing zero-shape early return stays first.
+            compare(identity(), a);
+        }
+        let mut huge = identity();
+        huge[0][0] = f32::MAX;
+        compare(identity(), huge);
+    }
+
+    #[test]
+    fn correlated_generators_and_linear_motion_enclose_independent_supports() {
+        // The exact set is a sum of line segments. Its support in any direction
+        // is sum(|h dot g|), regardless of whether clock offsets are correlated.
+        let generators: [[f32; N]; 3] = core::array::from_fn(|j| {
+            core::array::from_fn(|i| ((i * 7 + j * 11) as f32 - 50.0) * 0.0001)
+        });
+        let mut envelope = TimingEnvelope::ZERO;
+        for g in generators {
+            envelope.add_generator(g).unwrap();
+        }
+        let mut a = identity();
+        a[0][3] = 0.02;
+        a[3][7] = -0.196133;
+        a[6][9] = -0.02;
+        a[15][2] = 1.0;
+        envelope.transform(&a).unwrap();
+        for trial in 0..128 {
+            let h: [f32; N] =
+                core::array::from_fn(|i| (((trial + 1) * (i + 3) * 17 % 103) as f32 - 51.0) / 51.0);
+            let exact: f64 = generators
+                .iter()
+                .map(|g| {
+                    (0..N)
+                        .map(|i| {
+                            f64::from(h[i])
+                                * (0..N)
+                                    .map(|j| f64::from(a[i][j]) * f64::from(g[j]))
+                                    .sum::<f64>()
+                        })
+                        .sum::<f64>()
+                        .abs()
+                })
+                .sum();
+            assert!(f64::from(envelope.projected(&h).unwrap()) >= exact);
+        }
+        assert!(envelope.norm_bound(0, 3) >= envelope.radius(0));
+
+        // A common position/datum offset cancels in the pressure observation.
+        // Diagonal independent-error treatment would lose this information.
+        let mut correlated = TimingEnvelope::ZERO;
+        let mut g = [0.0; N];
+        g[2] = 0.01;
+        g[15] = 0.01;
+        correlated.add_generator(g).unwrap();
+        let mut h = [0.0; N];
+        h[2] = -1.0;
+        h[15] = 1.0;
+        assert!(correlated.projected(&h).unwrap() < 0.0001);
+    }
+
+    #[test]
+    fn disparate_and_subnormal_generators_remain_outward() {
+        for (first, second) in [(1e-8, 1.0), (1.0, 1e-8), (1e-40, 1e-40)] {
+            let mut envelope = TimingEnvelope::ZERO;
+            let mut g = [0.0; N];
+            g[0] = first;
+            envelope.add_generator(g).unwrap();
+            g[0] = second;
+            envelope.add_generator(g).unwrap();
+            assert!(f64::from(envelope.radius(0)) >= f64::from(first) + f64::from(second));
+        }
+        let mut zero = TimingEnvelope::ZERO;
+        zero.add_generator([0.0; N]).unwrap();
+        zero.transform(&identity()).unwrap();
+        assert_eq!(zero.shape, ZERO);
+    }
+
+    #[test]
+    fn numerical_failures_preserve_the_timing_enclosure() {
+        let mut envelope = TimingEnvelope::ZERO;
+        let mut g = [0.0; N];
+        g[0] = 0.002;
+        envelope.add_generator(g).unwrap();
+        let before = envelope.shape;
+        for bad in [f32::NAN, f32::INFINITY, f32::MAX] {
+            g[0] = bad;
+            assert_eq!(envelope.add_generator(g), Err(NavigationFailure::Numerical));
+            assert_eq!(envelope.shape, before);
+        }
+        let mut a = identity();
+        a[0][0] = f32::MAX;
+        assert_eq!(envelope.transform(&a), Err(NavigationFailure::Numerical));
+        assert_eq!(envelope.shape, before);
+        let mut h = [0.0; N];
+        h[0] = f32::MAX;
+        assert_eq!(envelope.projected(&h), Err(NavigationFailure::Numerical));
+
+        // Exercise the finite screen independently of callers' Gaussian checks.
+        let mut invalid = TimingEnvelope { shape: identity() };
+        invalid.shape[0][0] = -1.0;
+        assert!(!invalid.valid());
+        h[0] = 1.0;
+        assert_eq!(invalid.projected(&h), Err(NavigationFailure::Numerical));
+        invalid.shape[0][0] = f32::INFINITY;
+        assert!(!invalid.valid());
+        invalid.shape = identity();
+        for (i, row) in invalid.shape.iter_mut().enumerate() {
+            row[i] = f32::MAX;
+        }
+        g[0] = 0.002;
+        assert_eq!(invalid.add_generator(g), Err(NavigationFailure::Numerical));
+    }
+}