navigation: add fail-closed SITL estimator

libs/kaptan-navigation/Cargo.toml

@@ -0,0 +1,23 @@
+[package]
+name = "kaptan-navigation"
+edition.workspace = true
+license.workspace = true
+repository.workspace = true
+rust-version.workspace = true
+version.workspace = true
+
+[lints]
+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"]
+[dependencies]
+kaptan-math = { path = "../kaptan-math" }
+kaptan-sensor = { path = "../kaptan-sensor", optional = true }
+kaptan-time = { path = "../kaptan-time" }
+
+[dev-dependencies]
+kaptan-sensor = { path = "../kaptan-sensor" }

libs/kaptan-navigation/src/lib.rs

@@ -0,0 +1,41 @@
+//! 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.
+
+#![no_std]
+
+#[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 observation;
+
+pub use observation::{
+    AccelerationFrd, AngularRateFrd, CalibrationRevision, Covariance2, FrameId,
+    LocalPositionVelocityObservation, ObservationContext, ObservationSequence,
+    OpticalFlowObservation, PositionFrd, PositionNed, RangeObservation, ScalarVariance,
+    SensorExtrinsics, 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;

libs/kaptan-navigation/src/observation.rs

@@ -0,0 +1,540 @@
+//! 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_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);
+impl SourceId {
+    /// Creates a stable source identifier.
+    pub const fn new(value: u8) -> Self {
+        Self(value)
+    }
+    /// Returns the diagnostic identifier.
+    pub const fn get(self) -> u8 {
+        self.0
+    }
+}
+
+/// Physical coordinate frame identifier in the active calibration set.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct FrameId(u8);
+impl FrameId {
+    /// Aircraft FRD body frame.
+    pub const BODY_FRD: Self = Self(0);
+    /// Active launch-local navigation frame with north/east/down axes.
+    pub const LOCAL_NED: Self = Self(1);
+    /// Creates a configured sensor frame identifier.
+    pub const fn new(value: u8) -> Self {
+        Self(value)
+    }
+    /// Returns the diagnostic identifier.
+    pub const fn get(self) -> u8 {
+        self.0
+    }
+}
+
+/// 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.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct CalibrationRevision(u32);
+impl CalibrationRevision {
+    /// Creates a calibration revision token.
+    pub const fn new(value: u32) -> Self {
+        Self(value)
+    }
+    /// Returns its value for diagnostics.
+    pub const fn get(self) -> u32 {
+        self.0
+    }
+}
+
+/// Producer sequence number used to detect replayed or dropped observations.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct ObservationSequence(u32);
+impl ObservationSequence {
+    /// Creates a sequence token.
+    pub const fn new(value: u32) -> Self {
+        Self(value)
+    }
+    /// Returns the token value.
+    pub const fn get(self) -> u32 {
+        self.0
+    }
+}
+
+/// Driver validity state preserved through the navigation boundary.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum Validity {
+    /// The producer accepted this observation.
+    Valid,
+    /// The producer or adapter rejected it.
+    Invalid,
+}
+
+/// Common provenance for every navigation observation.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct ObservationContext {
+    /// Sensor measurement time synchronized into the controller monotonic domain.
+    pub measurement_time: Timestamp,
+    /// Controller receipt time, never compared with an external time directly.
+    pub receipt_time: Timestamp,
+    /// Statically configured source.
+    pub source: SourceId,
+    /// Frame of the value.
+    pub frame: FrameId,
+    /// Calibration/extrinsics revision.
+    pub calibration: CalibrationRevision,
+    /// Device quality, 0 through 255.
+    pub quality: u8,
+    /// Device/adapter validity.
+    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,
+        }
+    }
+}
+
+/// Positive finite scalar measurement variance in squared SI units.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct ScalarVariance(f32);
+impl ScalarVariance {
+    /// Validates a configured or reported scalar variance.
+    pub fn new(value: f32) -> Option<Self> {
+        if value.is_finite() && value > 0.0 {
+            Some(Self(value))
+        } else {
+            None
+        }
+    }
+    /// Returns the squared-SI value.
+    pub const fn get(self) -> f32 {
+        self.0
+    }
+}
+
+/// 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.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct PositionNed(Vector3<Ned>);
+impl PositionNed {
+    /// Creates a position in metres.
+    pub const fn metres(north: f32, east: f32, down: f32) -> Self {
+        Self(Vector3::new(north, east, down))
+    }
+    /// Returns its NED metre vector.
+    pub const fn vector(self) -> Vector3<Ned> {
+        self.0
+    }
+}
+
+/// NED velocity in metres per second.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct VelocityNed(Vector3<Ned>);
+impl VelocityNed {
+    /// Creates velocity in metres per second.
+    pub const fn metres_per_second(north: f32, east: f32, down: f32) -> Self {
+        Self(Vector3::new(north, east, down))
+    }
+    /// Returns its NED vector.
+    pub const fn vector(self) -> Vector3<Ned> {
+        self.0
+    }
+}
+
+/// 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.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct LocalPositionVelocityObservation {
+    /// Required source/time/frame provenance.
+    pub context: ObservationContext,
+    /// Absolute local-NED position in metres.
+    pub position: PositionNed,
+    /// Absolute local-NED velocity in metres per second.
+    pub velocity: VelocityNed,
+    /// Per-axis position measurement variance in m².
+    pub position_variance: ScalarVariance,
+    /// 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
+    }
+}
+
+/// Body FRD specific force in metres per second squared.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct AccelerationFrd(Vector3<Frd>);
+impl AccelerationFrd {
+    /// Creates specific force in metres per second squared.
+    pub const fn metres_per_second_squared(x: f32, y: f32, z: f32) -> Self {
+        Self(Vector3::new(x, y, z))
+    }
+    /// Returns the body vector.
+    pub const fn vector(self) -> Vector3<Frd> {
+        self.0
+    }
+}
+
+/// Body FRD angular rate in radians per second.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct AngularRateFrd(Vector3<Frd>);
+impl AngularRateFrd {
+    /// Creates angular rate in radians per second.
+    pub const fn radians_per_second(x: f32, y: f32, z: f32) -> Self {
+        Self(Vector3::new(x, y, z))
+    }
+    /// Returns the body vector.
+    pub const fn vector(self) -> Vector3<Frd> {
+        self.0
+    }
+}
+
+/// 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 {
+    /// Required observation provenance.
+    pub context: ObservationContext,
+    /// Ray distance in metres.
+    pub metres: f32,
+    /// Explicit measurement variance in m².
+    pub variance: ScalarVariance,
+}
+
+impl RangeObservation {
+    /// Converts a driver range value to metres at the navigation boundary.
+    #[cfg(any(test, feature = "sitl"))]
+    pub fn from_snapshot(
+        snapshot: RangeSnapshot,
+        mut context: ObservationContext,
+        variance: ScalarVariance,
+    ) -> Self {
+        context.measurement_time = snapshot.timestamp;
+        Self {
+            context,
+            metres: snapshot.distance_millimetres as f32 / 1_000.0,
+            variance,
+        }
+    }
+}
+
+/// 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};
+
+    fn context(frame: FrameId) -> ObservationContext {
+        ObservationContext::new(
+            Timestamp::from_microseconds(10),
+            Timestamp::from_microseconds(12),
+            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());
+    }
+
+    #[test]
+    fn provenance_validation() {
+        assert_eq!(SourceId::new(7).get(), 7);
+        assert_eq!(FrameId::new(8).get(), 8);
+        assert_eq!(CalibrationRevision::new(9).get(), 9);
+        assert_eq!(ObservationSequence::new(10).get(), 10);
+        assert!(ScalarVariance::new(0.0).is_none());
+        assert!(ScalarVariance::new(-1.0).is_none());
+        assert!(ScalarVariance::new(f32::NAN).is_none());
+    }
+
+    #[test]
+    fn snapshots_convert_at_boundary() {
+        let variance = ScalarVariance::new(0.25).unwrap();
+        let range = RangeObservation::from_snapshot(
+            RangeSnapshot::new(Timestamp::from_microseconds(20), 1_234),
+            context(FrameId::new(4)),
+            variance,
+        );
+        assert_eq!(
+            range.context.measurement_time,
+            Timestamp::from_microseconds(20)
+        );
+        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()
+                .y(),
+            2.0
+        );
+        assert_eq!(
+            AngularRateFrd::radians_per_second(1.0, 2.0, 3.0)
+                .vector()
+                .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)
+        );
+    }
+}