From b25a14f22481c07c2bba1c63ee69aa0f0f87d3ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20=C3=96zkan?= Date: Sun, 9 Aug 2026 13:58:21 +0300 Subject: [PATCH] navigation: add fail-closed SITL estimator --- libs/kaptan-navigation/Cargo.toml | 23 + libs/kaptan-navigation/src/eskf.rs | 4192 +++++++++++++++++++++ libs/kaptan-navigation/src/lib.rs | 41 + libs/kaptan-navigation/src/observation.rs | 540 +++ 4 files changed, 4796 insertions(+) create mode 100644 libs/kaptan-navigation/Cargo.toml create mode 100644 libs/kaptan-navigation/src/eskf.rs create mode 100644 libs/kaptan-navigation/src/lib.rs create mode 100644 libs/kaptan-navigation/src/observation.rs diff --git a/libs/kaptan-navigation/Cargo.toml b/libs/kaptan-navigation/Cargo.toml new file mode 100644 index 0000000..c0c91b6 --- /dev/null +++ b/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" } diff --git a/libs/kaptan-navigation/src/eskf.rs b/libs/kaptan-navigation/src/eskf.rs new file mode 100644 index 0000000..52a4461 --- /dev/null +++ b/libs/kaptan-navigation/src/eskf.rs @@ -0,0 +1,4192 @@ +//! Fixed-size multiplicative error-state Kalman filter. + +use kaptan_math::{Frd, MathError, Ned, Sensor, UnitQuaternion, Vector3, sqrt}; +use kaptan_time::Timestamp; + +use crate::observation::{ImuNoise, ImuObservation}; +use crate::{ + AccelerationFrd, AngularRateFrd, CalibrationRevision, FrameId, GRAVITY_MPS2, + LocalPositionVelocityObservation, ObservationSequence, OpticalFlowObservation, PositionNed, + RangeObservation, ScalarVariance, SensorExtrinsics, SourceId, Validity, VelocityNed, +}; + +const INERTIAL_DIM: usize = 15; +const TERRAIN_DIM: usize = 16; +const MAX_DIM: usize = TERRAIN_DIM; +const MATRIX: usize = MAX_DIM * MAX_DIM; +const POSITION: usize = 0; +const VELOCITY: usize = 3; +const ATTITUDE: usize = 6; +const GYRO_BIAS: usize = 9; +const ACCEL_BIAS: usize = 12; +const TERRAIN: usize = 15; + +// `UnitQuaternion::from_small_rotation_vector` is a bounded polynomial. This +// keeps every estimator call in its accurate operating region even if a board +// deliberately configures an IMU timeout longer than the default. +const MAXIMUM_ROTATION_INCREMENT_RADIANS_SQUARED: f32 = 4.0; + +/// Configured process, integrity, and plausibility limits for one estimator. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct EstimatorConfig { + /// Down-positive gravity used in NED propagation, m/s². + pub gravity_mps2: f32, + /// Continuous gyro white-noise variance, (rad/s)²/s. + pub gyro_noise_variance: f32, + /// Continuous accelerometer white-noise variance, (m/s²)²/s. + pub acceleration_noise_variance: f32, + /// Gyro-bias random-walk variance, (rad/s)²/s. + pub gyro_bias_walk_variance: f32, + /// Accelerometer-bias random-walk variance, (m/s²)²/s. + pub acceleration_bias_walk_variance: f32, + /// Terrain-down random-walk variance, m²/s. + pub terrain_walk_variance: f32, + /// Largest accepted IMU interval. Larger gaps require reinitialization. + pub maximum_imu_interval_microseconds: u64, + /// Minimum cosine of a downward range ray in NED. + pub minimum_range_down_cosine: f32, + /// Inclusive physical range lower bound in metres. + pub minimum_range_metres: f32, + /// Inclusive physical range upper bound in metres. + pub maximum_range_metres: f32, + /// Minimum quality for range fusion. + pub minimum_range_quality: u8, + /// Minimum quality for optical-flow fusion. + pub minimum_flow_quality: u8, + /// Largest plausible ground speed accepted from optical flow, m/s. + pub maximum_flow_speed_metres_per_second: f32, + /// One-degree-of-freedom chi-square gate. + pub range_gate_chi_square: f32, + /// Two-degree-of-freedom chi-square gate. + pub flow_gate_chi_square: f32, + /// Largest local-NED coordinate magnitude accepted from a position source. + pub maximum_local_position_metres: f32, + /// Largest local-NED speed accepted from a position source. + pub maximum_local_speed_metres_per_second: f32, + /// One-degree-of-freedom chi-square gate for local position/velocity axes. + pub local_state_gate_chi_square: f32, +} + +impl Default for EstimatorConfig { + fn default() -> Self { + Self { + gravity_mps2: GRAVITY_MPS2, + gyro_noise_variance: 2.5e-5, + acceleration_noise_variance: 2.5e-3, + gyro_bias_walk_variance: 1.0e-8, + acceleration_bias_walk_variance: 1.0e-6, + terrain_walk_variance: 1.0e-4, + maximum_imu_interval_microseconds: 20_000, + minimum_range_down_cosine: 0.5, + minimum_range_metres: 0.05, + maximum_range_metres: 40.0, + minimum_range_quality: 1, + minimum_flow_quality: 30, + maximum_flow_speed_metres_per_second: 25.0, + range_gate_chi_square: 6.635, + flow_gate_chi_square: 9.210, + maximum_local_position_metres: 10_000.0, + maximum_local_speed_metres_per_second: 100.0, + local_state_gate_chi_square: 16.0, + } + } +} + +impl EstimatorConfig { + fn is_valid(self) -> bool { + self.gravity_mps2.is_finite() + && self.gravity_mps2 > 0.0 + && self.gyro_noise_variance.is_finite() + && self.gyro_noise_variance >= 0.0 + && self.acceleration_noise_variance.is_finite() + && self.acceleration_noise_variance >= 0.0 + && self.gyro_bias_walk_variance.is_finite() + && self.gyro_bias_walk_variance >= 0.0 + && self.acceleration_bias_walk_variance.is_finite() + && self.acceleration_bias_walk_variance >= 0.0 + && self.terrain_walk_variance.is_finite() + && self.terrain_walk_variance >= 0.0 + && self.maximum_imu_interval_microseconds > 0 + && self.minimum_range_down_cosine.is_finite() + && self.minimum_range_down_cosine > 0.0 + && self.minimum_range_down_cosine <= 1.0 + && self.minimum_range_metres.is_finite() + && self.minimum_range_metres > 0.0 + && self.maximum_range_metres.is_finite() + && self.maximum_range_metres >= self.minimum_range_metres + && self.maximum_flow_speed_metres_per_second.is_finite() + && self.maximum_flow_speed_metres_per_second > 0.0 + && self.range_gate_chi_square.is_finite() + && self.range_gate_chi_square > 0.0 + && self.flow_gate_chi_square.is_finite() + && self.flow_gate_chi_square > 0.0 + && self.maximum_local_position_metres.is_finite() + && self.maximum_local_position_metres > 0.0 + && self.maximum_local_speed_metres_per_second.is_finite() + && self.maximum_local_speed_metres_per_second > 0.0 + && self.local_state_gate_chi_square.is_finite() + && self.local_state_gate_chi_square > 0.0 + } +} + +/// Bounded source-isolation policy. Four sources cover the current supported +/// sensor paths without allocating a map in a flight path. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct IntegrityConfig { + /// Maximum receipt-time age for an aided capability. + pub maximum_age_microseconds: u64, + /// Consecutive invalid/plausibility/gate failures before isolation. + pub rejection_limit: u8, + /// Consecutive gated observations needed to re-admit an isolated source. + pub readmission_accepts: u8, +} + +impl Default for IntegrityConfig { + fn default() -> Self { + Self { + maximum_age_microseconds: 250_000, + rejection_limit: 3, + readmission_accepts: 2, + } + } +} + +impl IntegrityConfig { + fn is_valid(self) -> bool { + self.maximum_age_microseconds > 0 + && self.rejection_limit > 0 + && self.readmission_accepts > 0 + } +} + +/// Current acceptance state for a configured aiding source. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum SourceStatus { + /// Source has not yet published an accepted observation. + Unknown, + /// Source has a fresh accepted observation. + Healthy, + /// Source is older than its configured freshness bound. + Stale, + /// Source is withheld after repeated invalid or gated observations. + Isolated, +} + +/// Truthful estimator capability report. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct EstimatorCapabilities { + /// Attitude is initialized and its covariance is finite. + pub attitude: bool, + /// Terrain-relative vertical position has fresh accepted range aiding. + pub terrain_relative_height: bool, + /// Terrain-relative horizontal velocity has fresh accepted flow aiding. + pub terrain_relative_velocity: bool, + /// Absolute launch-local position has fresh aiding. + pub local_position: bool, + /// Absolute launch-local velocity has fresh aiding. + pub local_velocity: bool, +} + +/// Estimator state and integrity summary for safety/diagnostics. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct EstimatorHealth { + /// No state/covariance component is non-finite. + pub finite: bool, + /// Estimator has passed stationary initialization. + pub initialized: bool, + /// Source status for the most recent range source. + pub range: SourceStatus, + /// Source status for the most recent optical-flow source. + pub optical_flow: SourceStatus, + /// Source status for the most recent local position/velocity source. + pub local_position_velocity: SourceStatus, + /// Capabilities that are actually observable with fresh inputs. + pub capabilities: EstimatorCapabilities, +} + +/// Propagation failure that leaves the nominal state unchanged. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum PropagationError { + /// An IMU observation was invalid or was not calibrated into FRD. + InvalidImu, + /// IMU source differs from the stream bound during stationary initialization. + ImuSourceMismatch, + /// IMU calibration differs from the stream bound during stationary initialization. + ImuCalibrationMismatch, + /// IMU sequence repeated or moved backwards within the bound stream. + ReplayedImu, + /// IMU measurement time did not advance strictly. + NonMonotonicTimestamp, + /// The interval exceeded the bounded propagation contract. + ImuGapTooLarge, + /// The IMU rate and interval exceed the bounded quaternion increment. + RotationIncrementTooLarge, + /// Input or resulting state was non-finite. + NonFinite, + /// The candidate state or covariance failed validation; reinitialize before reuse. + FilterInvalid, +} + +/// Aiding fusion refusal with no nominal-state update. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum FusionError { + /// Observation is not at this filter's current history time. + TimestampMismatch, + /// Observation was device-invalid. + InvalidObservation, + /// Observation source does not identify the configured physical device. + SourceMismatch, + /// Observation frame does not match the configured extrinsics. + FrameMismatch, + /// Observation calibration revision does not match the configured transform. + CalibrationMismatch, + /// The fixed source-integrity table has no entry for this source. + SourceCapacityExhausted, + /// A source repeated or moved backwards from its accepted sequence stream. + ReplayedObservation, + /// This estimator profile has no terrain state. + TerrainProfileRequired, + /// Optical flow from an offset sensor needs a correlated IMU-noise update + /// that this profile does not implement. + UnsupportedOpticalFlowLeverArm, + /// Measurement fails physical range or velocity plausibility bounds. + Implausible, + /// Range ray is too tilted or does not intersect terrain below the sensor. + InvalidRangeGeometry, + /// Innovation covariance was singular/non-finite. + SingularInnovation, + /// Innovation exceeds the configured chi-square gate. + InnovationRejected, + /// The candidate state or covariance failed validation; reinitialize before reuse. + FilterInvalid, +} + +/// Fusion outcome when the observation was valid and within its gate. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum FusionResult { + /// State and covariance were updated using Joseph form. + Fused, + /// Source remains isolated while it accumulates re-admission evidence. + SourceIsolated, +} + +#[derive(Clone, Copy)] +struct SourceEntry { + used: bool, + source: SourceId, + last_accepted_receipt: Option, + last_sequence: Option, + rejections: u8, + probes: u8, + isolated: bool, +} + +impl SourceEntry { + const EMPTY: Self = Self { + used: false, + source: SourceId::new(0), + last_accepted_receipt: None, + last_sequence: None, + rejections: 0, + probes: 0, + isolated: false, + }; +} + +#[derive(Clone, Copy)] +struct SourceIntegrity { + entries: [SourceEntry; 4], + config: IntegrityConfig, +} + +impl SourceIntegrity { + fn new(config: IntegrityConfig) -> Self { + Self { + entries: [SourceEntry::EMPTY; 4], + config, + } + } + + /// Reserves one static entry before source state is mutated. A full table + /// is a visible fusion refusal; it never replaces unrelated sensor state. + fn reserve(&mut self, source: SourceId) -> bool { + if self + .entries + .iter() + .any(|entry| entry.used && entry.source == source) + { + return true; + } + let Some(entry) = self.entries.iter_mut().find(|entry| !entry.used) else { + return false; + }; + *entry = SourceEntry { + used: true, + source, + ..SourceEntry::EMPTY + }; + true + } + + fn entry_mut(&mut self, source: SourceId) -> Option<&mut SourceEntry> { + self.entries + .iter_mut() + .find(|entry| entry.used && entry.source == source) + } + + fn accepts_sequence(&mut self, source: SourceId, sequence: ObservationSequence) -> bool { + let Some(entry) = self.entry_mut(source) else { + return false; + }; + let Some(previous) = entry.last_sequence else { + entry.last_sequence = Some(sequence); + return true; + }; + let distance = sequence.get().wrapping_sub(previous.get()); + if distance == 0 || distance >= (1 << 31) { + return false; + } + entry.last_sequence = Some(sequence); + true + } + + fn status(&self, source: Option, now: Timestamp) -> SourceStatus { + let Some(source) = source else { + return SourceStatus::Unknown; + }; + let Some(entry) = self + .entries + .iter() + .find(|entry| entry.used && entry.source == source) + else { + return SourceStatus::Unknown; + }; + if entry.isolated { + return SourceStatus::Isolated; + } + let Some(receipt) = entry.last_accepted_receipt else { + return SourceStatus::Unknown; + }; + if now + .as_microseconds() + .saturating_sub(receipt.as_microseconds()) + > self.config.maximum_age_microseconds + { + SourceStatus::Stale + } else { + SourceStatus::Healthy + } + } + + fn reject(&mut self, source: SourceId) { + let rejection_limit = self.config.rejection_limit; + let Some(entry) = self.entry_mut(source) else { + return; + }; + entry.probes = 0; + entry.rejections = entry.rejections.saturating_add(1); + if entry.rejections >= rejection_limit { + entry.isolated = true; + } + } + + /// Records a valid gated observation and reports whether it may update. + fn accept(&mut self, source: SourceId, receipt: Timestamp) -> bool { + let readmission_accepts = self.config.readmission_accepts; + let Some(entry) = self.entry_mut(source) else { + return false; + }; + entry.last_accepted_receipt = Some(receipt); + if entry.isolated { + entry.probes = entry.probes.saturating_add(1); + if entry.probes < readmission_accepts { + return false; + } + entry.isolated = false; + } + entry.probes = 0; + entry.rejections = 0; + true + } +} + +/// Nominal navigation state. Values have intentionally private storage so +/// state changes always pass through propagation or an error-state injection. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct NominalState { + position: PositionNed, + velocity: VelocityNed, + attitude: UnitQuaternion, + gyro_bias: AngularRateFrd, + acceleration_bias: AccelerationFrd, + terrain_down_metres: f32, +} + +impl NominalState { + /// Returns local NED position in metres. + pub const fn position(self) -> PositionNed { + self.position + } + /// Returns local NED velocity in metres per second. + pub const fn velocity(self) -> VelocityNed { + self.velocity + } + /// Returns the normalized body-to-NED attitude. + pub const fn attitude(self) -> UnitQuaternion { + self.attitude + } + /// Returns gyro bias in body rad/s. + pub const fn gyro_bias(self) -> AngularRateFrd { + self.gyro_bias + } + /// Returns accelerometer bias in body m/s². + pub const fn acceleration_bias(self) -> AccelerationFrd { + self.acceleration_bias + } + /// Returns terrain down position for the terrain profile only. + pub fn terrain_down_metres(self) -> Option { + if N == TERRAIN_DIM { + Some(self.terrain_down_metres) + } else { + None + } + } +} + +fn state_is_valid(state: NominalState) -> bool { + // UnitQuaternion keeps its representation private and every constructor + // normalizes finite input, so an unnormalized attitude has no safe path + // into NominalState. + state.position.vector().is_finite() + && state.velocity.vector().is_finite() + && state.gyro_bias.vector().is_finite() + && state.acceleration_bias.vector().is_finite() + && (N != TERRAIN_DIM || state.terrain_down_metres.is_finite()) +} + +/// Stationary initialization accumulator. It is bounded by `SAMPLES` and +/// assumes a level aircraft in FRD; arbitrary initial attitude needs an aiding +/// source and is intentionally not guessed. +const MAXIMUM_INITIAL_GYRO_BIAS_RAD_S: f32 = 0.1; +const MAXIMUM_INITIAL_ACCELERATION_RESIDUAL_MPS2: f32 = 0.75; +const MAXIMUM_INITIAL_GYRO_VARIANCE_RAD2_S2: f32 = 0.0025; +const MAXIMUM_INITIAL_ACCELERATION_VARIANCE_M2PS4: f32 = 0.16; + +pub struct StationaryInitializer { + count: usize, + gyro_sum: Vector3, + acceleration_sum: Vector3, + gyro_squared_sum: f32, + acceleration_squared_sum: f32, + source: Option, + calibration: Option, + last_time: Option, + last_sequence: Option, +} + +#[derive(Clone, Copy)] +struct ImuStream { + source: SourceId, + calibration: CalibrationRevision, + last_sequence: ObservationSequence, +} + +fn sequence_strictly_after(next: ObservationSequence, previous: ObservationSequence) -> bool { + let distance = next.get().wrapping_sub(previous.get()); + distance != 0 && distance < (1 << 31) +} + +impl StationaryInitializer { + /// Creates an empty level stationary initialization window. + pub const fn new() -> Self { + Self { + count: 0, + gyro_sum: Vector3::zero(), + acceleration_sum: Vector3::zero(), + gyro_squared_sum: 0.0, + acceleration_squared_sum: 0.0, + source: None, + calibration: None, + last_time: None, + last_sequence: None, + } + } + + /// Adds one source-bound, monotonic FRD IMU observation. Any malformed, + /// mixed-source, or non-monotonic sample resets the window so a level + /// estimate can never average unrelated or reordered motion. + pub fn push(&mut self, imu: ImuObservation) -> bool { + if self.count == SAMPLES { + return false; + } + if (imu.context.validity != Validity::Valid) + || (imu.context.frame != FrameId::BODY_FRD) + || !imu.acceleration.vector().is_finite() + || !imu.angular_rate.vector().is_finite() + { + self.reset(); + return false; + } + if self + .source + .is_some_and(|source| source != imu.context.source) + || self + .calibration + .is_some_and(|calibration| calibration != imu.context.calibration) + || self + .last_time + .is_some_and(|time| imu.context.measurement_time <= time) + || self + .last_sequence + .is_some_and(|sequence| !sequence_strictly_after(imu.context.sequence, sequence)) + { + self.reset(); + return false; + } + let gyro_squared = imu.angular_rate.vector().norm_squared(); + let acceleration_squared = imu.acceleration.vector().norm_squared(); + let gyro_sum = self.gyro_sum.add(imu.angular_rate.vector()); + let acceleration_sum = self.acceleration_sum.add(imu.acceleration.vector()); + let gyro_squared_sum = self.gyro_squared_sum + gyro_squared; + let acceleration_squared_sum = self.acceleration_squared_sum + acceleration_squared; + if !gyro_squared.is_finite() + || !acceleration_squared.is_finite() + || !gyro_sum.is_finite() + || !acceleration_sum.is_finite() + || !gyro_squared_sum.is_finite() + || !acceleration_squared_sum.is_finite() + { + self.reset(); + return false; + } + self.gyro_sum = gyro_sum; + self.acceleration_sum = acceleration_sum; + self.gyro_squared_sum = gyro_squared_sum; + self.acceleration_squared_sum = acceleration_squared_sum; + self.source = Some(imu.context.source); + self.calibration = Some(imu.context.calibration); + self.last_time = Some(imu.context.measurement_time); + self.last_sequence = Some(imu.context.sequence); + self.count += 1; + true + } + + /// Returns whether the fixed sample window is complete. + pub const fn is_complete(&self) -> bool { + self.count == SAMPLES + } + + fn reset(&mut self) { + *self = Self::new(); + } + + /// Produces a level initialized MEKF profile at the final accepted IMU + /// timestamp. The covariance starts diagonal and is deliberately + /// conservative for unobserved position. + pub fn finish( + self, + time: Timestamp, + config: EstimatorConfig, + integrity: IntegrityConfig, + terrain_down_metres: f32, + ) -> Option> { + if (SAMPLES == 0) + || (self.count != SAMPLES) + || (self.last_time != Some(time)) + || !config.is_valid() + || !terrain_down_metres.is_finite() + || !self.gyro_sum.is_finite() + || !self.acceleration_sum.is_finite() + || !self.gyro_squared_sum.is_finite() + || !self.acceleration_squared_sum.is_finite() + { + return None; + } + let inverse = 1.0 / self.count as f32; + let gyro_bias = self.gyro_sum.scale(inverse); + let average_acceleration = self.acceleration_sum.scale(inverse); + let acceleration_residual = + average_acceleration.sub(Vector3::new(0.0, 0.0, -config.gravity_mps2)); + let gyro_variance = self.gyro_squared_sum * inverse - gyro_bias.norm_squared(); + let acceleration_variance = + self.acceleration_squared_sum * inverse - average_acceleration.norm_squared(); + // The admission gate above proves finite sums and a nonzero sample + // count, so both averages are finite. Their residual and variances can + // still overflow and remain explicit fail-closed checks. + if !acceleration_residual.is_finite() + || !gyro_variance.is_finite() + || !acceleration_variance.is_finite() + || (gyro_bias.norm_squared() + > MAXIMUM_INITIAL_GYRO_BIAS_RAD_S * MAXIMUM_INITIAL_GYRO_BIAS_RAD_S) + || (acceleration_residual.norm_squared() + > MAXIMUM_INITIAL_ACCELERATION_RESIDUAL_MPS2 + * MAXIMUM_INITIAL_ACCELERATION_RESIDUAL_MPS2) + || (gyro_variance > MAXIMUM_INITIAL_GYRO_VARIANCE_RAD2_S2) + || (acceleration_variance > MAXIMUM_INITIAL_ACCELERATION_VARIANCE_M2PS4) + { + return None; + } + let acceleration_bias = acceleration_residual; + let stream = ImuStream { + source: self.source?, + calibration: self.calibration?, + last_sequence: self.last_sequence?, + }; + Eskf::new( + time, + NominalState { + position: PositionNed::metres(0.0, 0.0, 0.0), + velocity: VelocityNed::metres_per_second(0.0, 0.0, 0.0), + attitude: UnitQuaternion::identity(), + gyro_bias: AngularRateFrd::radians_per_second( + gyro_bias.x(), + gyro_bias.y(), + gyro_bias.z(), + ), + acceleration_bias: AccelerationFrd::metres_per_second_squared( + acceleration_bias.x(), + acceleration_bias.y(), + acceleration_bias.z(), + ), + terrain_down_metres, + }, + config, + integrity, + stream, + ) + .ok() + } +} + +impl Default for StationaryInitializer { + fn default() -> Self { + Self::new() + } +} + +#[derive(Clone, Copy)] +struct Covariance { + values: [f32; MATRIX], +} + +impl Covariance { + fn diagonal() -> Self { + let mut values = [0.0; MATRIX]; + let mut index = 0; + while index < MAX_DIM { + values[index * MAX_DIM + index] = match index { + 0..=2 => 100.0, + 3..=5 => 10.0, + 6..=8 => 0.25, + 9..=11 => 0.01, + 12..=14 => 0.25, + _ => 100.0, + }; + index += 1; + } + Self { values } + } + fn get(&self, row: usize, column: usize) -> f32 { + self.values[row * MAX_DIM + column] + } + fn set(&mut self, row: usize, column: usize, value: f32) { + self.values[row * MAX_DIM + column] = value; + } + fn symmetric(&mut self, n: usize) { + let mut row = 0; + while row < n { + let mut col = row + 1; + while col < n { + let value = 0.5 * (self.get(row, col) + self.get(col, row)); + self.set(row, col, value); + self.set(col, row, value); + col += 1; + } + row += 1; + } + } + fn finite(&self, n: usize) -> bool { + let mut row = 0; + while row < n { + let mut col = 0; + while col < n { + if !self.get(row, col).is_finite() { + return false; + } + col += 1; + } + row += 1; + } + true + } + + /// Strict fixed-size Cholesky validation. A finite, symmetric matrix is + /// not sufficient for a Kalman covariance: it must also be positive + /// definite. No diagonal values are repaired on failure. + fn positive_definite(&self, n: usize) -> bool { + if !self.finite(n) { + return false; + } + let mut lower = [0.0; MATRIX]; + for row in 0..n { + for col in 0..=row { + let mut value = self.get(row, col); + for k in 0..col { + value -= lower[row * MAX_DIM + k] * lower[col * MAX_DIM + k]; + } + if !value.is_finite() { + return false; + } + if row == col { + if value <= 0.0 { + return false; + } + let diagonal = sqrt(value); + // `value` is finite and strictly positive, and the + // reviewed bounded square root preserves that domain. + debug_assert!(diagonal.is_finite()); + debug_assert!(diagonal > 0.0); + lower[row * MAX_DIM + col] = diagonal; + } else { + let diagonal = lower[col * MAX_DIM + col]; + // Every lower diagonal was accepted in the corresponding + // earlier diagonal iteration. + debug_assert!(diagonal.is_finite()); + debug_assert!(diagonal > 0.0); + let entry = value / diagonal; + if !entry.is_finite() { + return false; + } + lower[row * MAX_DIM + col] = entry; + } + } + } + true + } +} + +/// Fixed-size MEKF profile. `N` is intentionally a const generic solely to +/// select one of the two reviewed covariance dimensions (15 inertial or 16 +/// local-terrain); no runtime profile switch exists. +#[derive(Clone, Copy)] +pub struct Eskf { + time: Timestamp, + state: NominalState, + covariance: Covariance, + config: EstimatorConfig, + integrity: SourceIntegrity, + imu_stream: ImuStream, + last_range_source: Option, + last_flow_source: Option, + last_local_state_source: Option, + failed: bool, +} + +/// Inertial profile: position, velocity, attitude, gyro bias, accelerometer bias. +pub type InertialEskf = Eskf; +/// Local-terrain profile: inertial state plus terrain down position. +pub type LocalTerrainEskf = Eskf; + +impl Eskf { + fn new( + time: Timestamp, + state: NominalState, + config: EstimatorConfig, + integrity: IntegrityConfig, + imu_stream: ImuStream, + ) -> Result { + if (N != INERTIAL_DIM && N != TERRAIN_DIM) + || !config.is_valid() + || !integrity.is_valid() + || !state_is_valid::(state) + { + return Err(()); + } + let covariance = Covariance::diagonal(); + // This fixed constructor is positive diagonal for both permitted + // dimensions. Candidate covariance validation remains mandatory on + // every propagation and fusion path. + debug_assert!(covariance.positive_definite(N)); + Ok(Self { + time, + state, + covariance, + config, + integrity: SourceIntegrity::new(integrity), + imu_stream, + last_range_source: None, + last_flow_source: None, + last_local_state_source: None, + failed: false, + }) + } + + /// Current filter time in the controller monotonic domain. + pub const fn time(&self) -> Timestamp { + self.time + } + /// Current nominal state. + pub const fn state(&self) -> NominalState { + self.state + } + /// Returns whether covariance remains finite, symmetric, and strictly + /// positive definite according to a fixed-size Cholesky factorization. + pub fn covariance_is_valid(&self) -> bool { + if !self.covariance.finite(N) { + return false; + } + let mut row = 0; + while row < N { + let mut col = row + 1; + while col < N { + if (self.covariance.get(row, col) - self.covariance.get(col, row)).abs() > 1.0e-4 { + return false; + } + col += 1; + } + row += 1; + } + self.covariance.positive_definite(N) + } + + /// Returns whether a numerical candidate has invalidated this filter and + /// made safe reinitialization mandatory. + pub const fn requires_reinitialization(&self) -> bool { + self.failed + } + + fn values_are_valid(&self) -> bool { + !self.failed && state_is_valid::(self.state) && self.covariance_is_valid() + } + + /// Propagates exactly one strictly newer IMU observation. + pub fn propagate(&mut self, imu: ImuObservation) -> Result<(), PropagationError> { + if self.failed { + return Err(PropagationError::FilterInvalid); + } + if (imu.context.validity != Validity::Valid) + || (imu.context.frame != FrameId::BODY_FRD) + || !imu.acceleration.vector().is_finite() + || !imu.angular_rate.vector().is_finite() + { + return Err(PropagationError::InvalidImu); + } + if imu.context.source != self.imu_stream.source { + return Err(PropagationError::ImuSourceMismatch); + } + if imu.context.calibration != self.imu_stream.calibration { + return Err(PropagationError::ImuCalibrationMismatch); + } + let measurement = imu.context.measurement_time.as_microseconds(); + let current = self.time.as_microseconds(); + if measurement <= current { + return Err(PropagationError::NonMonotonicTimestamp); + } + let dt_us = measurement - current; + if dt_us > self.config.maximum_imu_interval_microseconds { + return Err(PropagationError::ImuGapTooLarge); + } + if !sequence_strictly_after(imu.context.sequence, self.imu_stream.last_sequence) { + return Err(PropagationError::ReplayedImu); + } + let dt = dt_us as f32 / 1_000_000.0; + let rotation_increment = imu + .angular_rate + .vector() + .sub(self.state.gyro_bias.vector()) + .scale(dt); + if !rotation_increment.is_finite() { + return Err(PropagationError::NonFinite); + } + if rotation_increment.norm_squared() > MAXIMUM_ROTATION_INCREMENT_RADIANS_SQUARED { + return Err(PropagationError::RotationIncrementTooLarge); + } + // Compute into a fixed-size copy so the public error contract is true + // even if a finite input overflows a later covariance calculation. + let mut next = *self; + if next.propagate_delta(imu, dt).is_err() { + self.failed = true; + return Err(PropagationError::FilterInvalid); + } + next.time = imu.context.measurement_time; + next.imu_stream.last_sequence = imu.context.sequence; + if !next.values_are_valid() { + self.failed = true; + return Err(PropagationError::FilterInvalid); + } + *self = next; + Ok(()) + } + + fn propagate_delta(&mut self, imu: ImuObservation, dt: f32) -> Result<(), MathError> { + let gyro = imu.angular_rate.vector().sub(self.state.gyro_bias.vector()); + let force_body = imu + .acceleration + .vector() + .sub(self.state.acceleration_bias.vector()); + let delta = UnitQuaternion::from_small_rotation_vector(gyro.scale(dt))?; + let attitude = self.state.attitude.compose(delta)?; + let force_navigation = attitude.body_to_ned(force_body); + let acceleration = force_navigation.add(Vector3::new(0.0, 0.0, self.config.gravity_mps2)); + let position = self + .state + .position + .vector() + .add(self.state.velocity.vector().scale(dt)) + .add(acceleration.scale(0.5 * dt * dt)); + let velocity = self.state.velocity.vector().add(acceleration.scale(dt)); + if !acceleration.is_finite() || !position.is_finite() || !velocity.is_finite() { + return Err(MathError::NonFinite); + } + self.state.attitude = attitude; + self.state.position = PositionNed::metres(position.x(), position.y(), position.z()); + self.state.velocity = + VelocityNed::metres_per_second(velocity.x(), velocity.y(), velocity.z()); + self.propagate_covariance(force_navigation, dt, imu.noise); + Ok(()) + } + + fn propagate_covariance(&mut self, force_navigation: Vector3, dt: f32, noise: ImuNoise) { + let mut transition = [0.0; MATRIX]; + let mut i = 0; + while i < N { + transition[i * MAX_DIM + i] = 1.0; + i += 1; + } + for axis in 0..3 { + transition[(POSITION + axis) * MAX_DIM + VELOCITY + axis] = dt; + } + let skew = [ + [0.0, -force_navigation.z(), force_navigation.y()], + [force_navigation.z(), 0.0, -force_navigation.x()], + [-force_navigation.y(), force_navigation.x(), 0.0], + ]; + let rotation = rotation_matrix(self.state.attitude); + for row in 0..3 { + for col in 0..3 { + transition[(VELOCITY + row) * MAX_DIM + ATTITUDE + col] = -skew[row][col] * dt; + transition[(VELOCITY + row) * MAX_DIM + ACCEL_BIAS + col] = + -rotation[row][col] * dt; + transition[(ATTITUDE + row) * MAX_DIM + GYRO_BIAS + col] = -rotation[row][col] * dt; + } + } + let mut temporary = [0.0; MATRIX]; + for row in 0..N { + for col in 0..N { + let mut sum = 0.0; + for k in 0..N { + sum += transition[row * MAX_DIM + k] * self.covariance.get(k, col); + } + temporary[row * MAX_DIM + col] = sum; + } + } + let mut propagated = Covariance { + values: [0.0; MATRIX], + }; + for row in 0..N { + for col in 0..N { + let mut sum = 0.0; + for k in 0..N { + sum += temporary[row * MAX_DIM + k] * transition[col * MAX_DIM + k]; + } + propagated.set(row, col, sum); + } + } + // Continuous process spectral densities integrate once over `dt`. + // Per-sample sensor variances enter through their integrated angle or + // velocity increments and therefore scale with `dt²`. + let gyro_noise = self.config.gyro_noise_variance * dt + noise.gyro_rate.get() * dt * dt; + let accel_noise = + self.config.acceleration_noise_variance * dt + noise.acceleration.get() * dt * dt; + for axis in 0..3 { + propagated.set( + VELOCITY + axis, + VELOCITY + axis, + propagated.get(VELOCITY + axis, VELOCITY + axis) + accel_noise, + ); + propagated.set( + ATTITUDE + axis, + ATTITUDE + axis, + propagated.get(ATTITUDE + axis, ATTITUDE + axis) + gyro_noise, + ); + propagated.set( + GYRO_BIAS + axis, + GYRO_BIAS + axis, + propagated.get(GYRO_BIAS + axis, GYRO_BIAS + axis) + + self.config.gyro_bias_walk_variance * dt, + ); + propagated.set( + ACCEL_BIAS + axis, + ACCEL_BIAS + axis, + propagated.get(ACCEL_BIAS + axis, ACCEL_BIAS + axis) + + self.config.acceleration_bias_walk_variance * dt, + ); + } + if N == TERRAIN_DIM { + propagated.set( + TERRAIN, + TERRAIN, + propagated.get(TERRAIN, TERRAIN) + self.config.terrain_walk_variance * dt, + ); + } + propagated.symmetric(N); + self.covariance = propagated; + } + + /// Returns the co-located optical-flow prediction and its two-row ESKF + /// measurement Jacobian. Offset sensors are refused by the caller: their + /// rigid-body rate term is correlated with propagation IMU noise. + fn co_located_flow_model( + &self, + extrinsics: SensorExtrinsics, + ) -> ([f32; 2], [[f32; MAX_DIM]; 2]) { + let body_velocity = self + .state + .attitude + .ned_to_body(self.state.velocity.vector()); + let sensor_velocity: Vector3 = + extrinsics.sensor_to_body.inverse().rotate(body_velocity); + let predicted = [sensor_velocity.x(), sensor_velocity.y()]; + let mut h = [[0.0; MAX_DIM]; 2]; + for axis in 0..3 { + let basis = match axis { + 0 => Vector3::::new(1.0, 0.0, 0.0), + 1 => Vector3::::new(0.0, 1.0, 0.0), + _ => Vector3::::new(0.0, 0.0, 1.0), + }; + let body = self.state.attitude.ned_to_body(basis); + let sensor: Vector3 = extrinsics.sensor_to_body.inverse().rotate(body); + h[0][VELOCITY + axis] = sensor.x(); + h[1][VELOCITY + axis] = sensor.y(); + + // The filter's attitude error is a left-multiplicative NED + // tangent perturbation. d(Rᵀv)/dδθ = Rᵀ[v]×. + let attitude_body = self + .state + .attitude + .ned_to_body(self.state.velocity.vector().cross(basis)); + let attitude_sensor: Vector3 = + extrinsics.sensor_to_body.inverse().rotate(attitude_body); + h[0][ATTITUDE + axis] = attitude_sensor.x(); + h[1][ATTITUDE + axis] = attitude_sensor.y(); + } + (predicted, h) + } + + /// Fuses a range observation at the filter's current history time. + /// + /// Delayed observations are rejected. A future estimator must only admit + /// delayed aiding with an independently validated interval-splitting and + /// covariance-replay model; retaining a partial rewind layer here would + /// manufacture confidence without that proof. + pub fn fuse_range( + &mut self, + observation: RangeObservation, + extrinsics: SensorExtrinsics, + ) -> Result { + if self.failed { + return Err(FusionError::FilterInvalid); + } + if observation.context.measurement_time != self.time { + return Err(FusionError::TimestampMismatch); + } + if N != TERRAIN_DIM { + return Err(FusionError::TerrainProfileRequired); + } + if observation.context.source != extrinsics.source { + return Err(FusionError::SourceMismatch); + } + if observation.context.validity != Validity::Valid + || observation.context.quality < self.config.minimum_range_quality + { + self.integrity.reject(observation.context.source); + return Err(FusionError::InvalidObservation); + } + if observation.context.frame != extrinsics.sensor_frame { + self.integrity.reject(observation.context.source); + return Err(FusionError::FrameMismatch); + } + if observation.context.calibration != extrinsics.calibration { + self.integrity.reject(observation.context.source); + return Err(FusionError::CalibrationMismatch); + } + if !self.integrity.reserve(extrinsics.source) { + return Err(FusionError::SourceCapacityExhausted); + } + if !self + .integrity + .accepts_sequence(extrinsics.source, observation.context.sequence) + { + self.integrity.reject(extrinsics.source); + return Err(FusionError::ReplayedObservation); + } + if !observation.metres.is_finite() + || observation.metres < self.config.minimum_range_metres + || observation.metres > self.config.maximum_range_metres + { + self.integrity.reject(observation.context.source); + return Err(FusionError::Implausible); + } + let ray_body = extrinsics.sensor_to_body_vector(Vector3::::new(0.0, 0.0, 1.0)); + let ray = self.state.attitude.body_to_ned(ray_body); + let lever = self + .state + .attitude + .body_to_ned(extrinsics.position_body.vector()); + // The ray is a fixed finite unit vector transformed only by normalized + // quaternions. The public lever-arm value remains independently + // validated because it can carry a malformed configuration value. + if !lever.is_finite() || (ray.z() < self.config.minimum_range_down_cosine) { + self.integrity.reject(observation.context.source); + return Err(FusionError::InvalidRangeGeometry); + } + let numerator = + self.state.terrain_down_metres - self.state.position.vector().z() - lever.z(); + if numerator <= 0.0 { + self.integrity.reject(observation.context.source); + return Err(FusionError::InvalidRangeGeometry); + } + let predicted = numerator / ray.z(); + let mut h = [0.0; MAX_DIM]; + h[POSITION + 2] = -1.0 / ray.z(); + h[TERRAIN] = 1.0 / ray.z(); + for axis in 0..3 { + let lever_z = tangent_cross_z(axis, lever); + let ray_z = tangent_cross_z(axis, ray); + h[ATTITUDE + axis] = (-lever_z * ray.z() - numerator * ray_z) / (ray.z() * ray.z()); + } + let innovation = observation.metres - predicted; + if !predicted.is_finite() + || !innovation.is_finite() + || !h.iter().all(|value| value.is_finite()) + { + self.integrity.reject(observation.context.source); + return Err(FusionError::SingularInnovation); + } + let Some(gain) = self.scalar_gain(h, observation.variance) else { + self.integrity.reject(observation.context.source); + return Err(FusionError::SingularInnovation); + }; + let nis = innovation * innovation / gain.innovation_variance; + if !nis.is_finite() || nis > self.config.range_gate_chi_square { + self.integrity.reject(observation.context.source); + return Err(FusionError::InnovationRejected); + } + self.last_range_source = Some(extrinsics.source); + if !self + .integrity + .accept(extrinsics.source, observation.context.receipt_time) + { + return Ok(FusionResult::SourceIsolated); + } + let mut candidate = *self; + if candidate + .apply_scalar(gain, innovation, observation.variance.get()) + .is_err() + || !candidate.values_are_valid() + { + self.failed = true; + return Err(FusionError::FilterInvalid); + } + *self = candidate; + Ok(FusionResult::Fused) + } + + /// Fuses a ground-referenced sensor-frame optical-flow velocity at current time. + /// + /// The sensor must be co-located with the body origin. An offset sensor + /// has a rigid-body `ω × r` term correlated with the IMU sample used for + /// propagation. It is refused until the estimator implements that joint + /// measurement update instead of treating correlated noise as independent. + pub fn fuse_optical_flow( + &mut self, + observation: OpticalFlowObservation, + extrinsics: SensorExtrinsics, + ) -> Result { + if self.failed { + return Err(FusionError::FilterInvalid); + } + if observation.context.measurement_time != self.time { + return Err(FusionError::TimestampMismatch); + } + if observation.context.source != extrinsics.source { + return Err(FusionError::SourceMismatch); + } + if !extrinsics.position_body.vector().is_finite() + || extrinsics.position_body.vector() != Vector3::new(0.0, 0.0, 0.0) + { + return Err(FusionError::UnsupportedOpticalFlowLeverArm); + } + if (observation.context.validity != Validity::Valid) + || (observation.context.quality < self.config.minimum_flow_quality) + { + self.integrity.reject(observation.context.source); + return Err(FusionError::InvalidObservation); + } + if observation.context.frame != extrinsics.sensor_frame { + self.integrity.reject(observation.context.source); + return Err(FusionError::FrameMismatch); + } + if observation.context.calibration != extrinsics.calibration { + self.integrity.reject(observation.context.source); + return Err(FusionError::CalibrationMismatch); + } + if !self.integrity.reserve(extrinsics.source) { + return Err(FusionError::SourceCapacityExhausted); + } + if !self + .integrity + .accepts_sequence(extrinsics.source, observation.context.sequence) + { + self.integrity.reject(extrinsics.source); + return Err(FusionError::ReplayedObservation); + } + if !observation.velocity.is_finite() + || !observation.ground_distance_metres.is_finite() + || (observation.ground_distance_metres <= 0.0) + || (observation.velocity.x().abs() > self.config.maximum_flow_speed_metres_per_second) + || (observation.velocity.y().abs() > self.config.maximum_flow_speed_metres_per_second) + { + self.integrity.reject(observation.context.source); + return Err(FusionError::Implausible); + } + let (predicted, h) = self.co_located_flow_model(extrinsics); + if !predicted.iter().all(|value| value.is_finite()) { + self.integrity.reject(observation.context.source); + return Err(FusionError::Implausible); + } + if !h[0] + .iter() + .take(N) + .chain(h[1].iter().take(N)) + .all(|value| value.is_finite()) + { + self.integrity.reject(observation.context.source); + return Err(FusionError::SingularInnovation); + } + let innovation = [ + observation.velocity.x() - predicted[0], + observation.velocity.y() - predicted[1], + ]; + let Some(gain) = self.two_gain(h, observation.covariance) else { + self.integrity.reject(observation.context.source); + return Err(FusionError::SingularInnovation); + }; + let nis = innovation[0] + * (gain.inverse_s[0] * innovation[0] + gain.inverse_s[1] * innovation[1]) + + innovation[1] + * (gain.inverse_s[2] * innovation[0] + gain.inverse_s[3] * innovation[1]); + if !nis.is_finite() || nis > self.config.flow_gate_chi_square { + self.integrity.reject(observation.context.source); + return Err(FusionError::InnovationRejected); + } + self.last_flow_source = Some(extrinsics.source); + if !self + .integrity + .accept(extrinsics.source, observation.context.receipt_time) + { + return Ok(FusionResult::SourceIsolated); + } + let mut candidate = *self; + if candidate + .apply_two(gain, innovation, observation.covariance) + .is_err() + || !candidate.values_are_valid() + { + self.failed = true; + return Err(FusionError::FilterInvalid); + } + *self = candidate; + Ok(FusionResult::Fused) + } + + /// Fuses one complete local-NED position and velocity observation at the + /// filter's current history time. + /// + /// The configured source and calibration are explicit inputs because a + /// local-state producer is a vehicle integration choice (for example GPS, + /// VIO, or a simulator), not a hidden estimator mode. Position and + /// velocity axes are updated sequentially in a private candidate filter; + /// any failed gate leaves the live nominal state and covariance unchanged. + pub fn fuse_local_position_velocity( + &mut self, + observation: LocalPositionVelocityObservation, + expected_source: SourceId, + expected_calibration: CalibrationRevision, + ) -> Result { + if self.failed { + return Err(FusionError::FilterInvalid); + } + if observation.context.measurement_time != self.time { + return Err(FusionError::TimestampMismatch); + } + if observation.context.source != expected_source { + return Err(FusionError::SourceMismatch); + } + if (observation.context.validity != Validity::Valid) || (observation.context.quality == 0) { + self.integrity.reject(observation.context.source); + return Err(FusionError::InvalidObservation); + } + if observation.context.frame != FrameId::LOCAL_NED { + self.integrity.reject(observation.context.source); + return Err(FusionError::FrameMismatch); + } + if observation.context.calibration != expected_calibration { + self.integrity.reject(observation.context.source); + return Err(FusionError::CalibrationMismatch); + } + if !self.integrity.reserve(expected_source) { + return Err(FusionError::SourceCapacityExhausted); + } + if !self + .integrity + .accepts_sequence(expected_source, observation.context.sequence) + { + self.integrity.reject(expected_source); + return Err(FusionError::ReplayedObservation); + } + let position = observation.position.vector(); + let velocity = observation.velocity.vector(); + if !position.is_finite() + || !velocity.is_finite() + || (position.x().abs() > self.config.maximum_local_position_metres) + || (position.y().abs() > self.config.maximum_local_position_metres) + || (position.z().abs() > self.config.maximum_local_position_metres) + || (velocity.x().abs() > self.config.maximum_local_speed_metres_per_second) + || (velocity.y().abs() > self.config.maximum_local_speed_metres_per_second) + || (velocity.z().abs() > self.config.maximum_local_speed_metres_per_second) + { + self.integrity.reject(observation.context.source); + return Err(FusionError::Implausible); + } + let measurements = [ + (POSITION, position.x(), observation.position_variance), + (POSITION + 1, position.y(), observation.position_variance), + (POSITION + 2, position.z(), observation.position_variance), + (VELOCITY, velocity.x(), observation.velocity_variance), + (VELOCITY + 1, velocity.y(), observation.velocity_variance), + (VELOCITY + 2, velocity.z(), observation.velocity_variance), + ]; + let mut candidate = *self; + for (state_index, measurement, variance) in measurements { + let predicted = if state_index < VELOCITY { + let value = candidate.state.position.vector(); + match state_index - POSITION { + 0 => value.x(), + 1 => value.y(), + _ => value.z(), + } + } else { + let value = candidate.state.velocity.vector(); + match state_index - VELOCITY { + 0 => value.x(), + 1 => value.y(), + _ => value.z(), + } + }; + let mut h = [0.0; MAX_DIM]; + h[state_index] = 1.0; + let Some(gain) = candidate.scalar_gain(h, variance) else { + self.integrity.reject(observation.context.source); + return Err(FusionError::SingularInnovation); + }; + let innovation = measurement - predicted; + let nis = innovation * innovation / gain.innovation_variance; + if !nis.is_finite() || nis > self.config.local_state_gate_chi_square { + self.integrity.reject(observation.context.source); + return Err(FusionError::InnovationRejected); + } + if candidate + .apply_scalar(gain, innovation, variance.get()) + .is_err() + { + self.failed = true; + return Err(FusionError::FilterInvalid); + } + } + if !candidate.values_are_valid() { + self.failed = true; + return Err(FusionError::FilterInvalid); + } + self.last_local_state_source = Some(expected_source); + if !self + .integrity + .accept(expected_source, observation.context.receipt_time) + { + return Ok(FusionResult::SourceIsolated); + } + candidate.integrity = self.integrity; + candidate.last_local_state_source = self.last_local_state_source; + *self = candidate; + Ok(FusionResult::Fused) + } + + /// Returns only capabilities supported by fresh accepted aiding sources. + pub fn health(&self, now: Timestamp) -> EstimatorHealth { + let range = self.integrity.status(self.last_range_source, now); + let optical_flow = self.integrity.status(self.last_flow_source, now); + let local_position_velocity = self.integrity.status(self.last_local_state_source, now); + let finite = self.values_are_valid(); + EstimatorHealth { + finite, + initialized: !self.failed, + range, + optical_flow, + local_position_velocity, + capabilities: EstimatorCapabilities { + attitude: finite, + terrain_relative_height: N == TERRAIN_DIM && range == SourceStatus::Healthy, + terrain_relative_velocity: optical_flow == SourceStatus::Healthy, + local_position: local_position_velocity == SourceStatus::Healthy, + local_velocity: local_position_velocity == SourceStatus::Healthy, + }, + } + } + + fn scalar_gain(&self, h: [f32; MAX_DIM], variance: ScalarVariance) -> Option { + let mut ph = [0.0; MAX_DIM]; + for (row, ph_value) in ph.iter_mut().enumerate().take(N) { + for (col, h_value) in h.iter().enumerate().take(N) { + *ph_value += self.covariance.get(row, col) * h_value; + } + } + let mut innovation_variance = variance.get(); + for (h_value, ph_value) in h.iter().zip(ph).take(N) { + innovation_variance += h_value * ph_value; + } + if !innovation_variance.is_finite() || innovation_variance <= 1.0e-8 { + return None; + } + let mut gain = [0.0; MAX_DIM]; + for (gain_value, ph_value) in gain.iter_mut().zip(ph).take(N) { + *gain_value = ph_value / innovation_variance; + } + Some(ScalarGain { + h, + gain, + innovation_variance, + }) + } + + fn apply_scalar( + &mut self, + update: ScalarGain, + innovation: f32, + variance: f32, + ) -> Result<(), MathError> { + let mut error = [0.0; MAX_DIM]; + for (error_value, gain_value) in error.iter_mut().zip(update.gain).take(N) { + *error_value = gain_value * innovation; + } + if !innovation.is_finite() + || !variance.is_finite() + || (variance <= 0.0) + || !error.iter().take(N).all(|value| value.is_finite()) + { + return Err(MathError::NonFinite); + } + let attitude_error = self.inject(error)?; + let mut a = [0.0; MATRIX]; + for row in 0..N { + for col in 0..N { + a[row * MAX_DIM + col] = + if row == col { 1.0 } else { 0.0 } - update.gain[row] * update.h[col]; + } + } + self.joseph(a, &update.gain, variance); + self.reset_attitude_error(attitude_error); + Ok(()) + } + + fn two_gain(&self, h: [[f32; MAX_DIM]; 2], covariance: crate::Covariance2) -> Option { + let mut ph = [[0.0; 2]; MAX_DIM]; + for (row, ph_value) in ph.iter_mut().enumerate().take(N) { + for col in 0..N { + ph_value[0] += self.covariance.get(row, col) * h[0][col]; + ph_value[1] += self.covariance.get(row, col) * h[1][col]; + } + } + let mut s00 = covariance.xx.get(); + let mut s01 = covariance.xy; + let mut s11 = covariance.yy.get(); + for (row, ph_value) in ph.iter().enumerate().take(N) { + s00 += h[0][row] * ph_value[0]; + s01 += h[0][row] * ph_value[1]; + s11 += h[1][row] * ph_value[1]; + } + let determinant = s00 * s11 - s01 * s01; + // This is the fixed-size equivalent of accepting only a successful + // Cholesky factorization of the innovation covariance. A positive + // determinant alone would also accept a negative-definite 2x2 matrix. + if !s00.is_finite() + || !s01.is_finite() + || !s11.is_finite() + || (s00 <= 1.0e-8) + || (s11 <= 1.0e-8) + || !determinant.is_finite() + || (determinant <= 1.0e-8) + { + return None; + } + let inverse_s = [ + s11 / determinant, + -s01 / determinant, + -s01 / determinant, + s00 / determinant, + ]; + let mut gain = [[0.0; 2]; MAX_DIM]; + for row in 0..N { + gain[row][0] = ph[row][0] * inverse_s[0] + ph[row][1] * inverse_s[2]; + gain[row][1] = ph[row][0] * inverse_s[1] + ph[row][1] * inverse_s[3]; + } + Some(TwoGain { h, gain, inverse_s }) + } + + fn apply_two( + &mut self, + update: TwoGain, + innovation: [f32; 2], + covariance: crate::Covariance2, + ) -> Result<(), MathError> { + let mut error = [0.0; MAX_DIM]; + for (error_value, gain_value) in error.iter_mut().zip(update.gain).take(N) { + *error_value = gain_value[0] * innovation[0] + gain_value[1] * innovation[1]; + } + if !innovation.iter().all(|value| value.is_finite()) + || !error.iter().take(N).all(|value| value.is_finite()) + { + return Err(MathError::NonFinite); + } + let attitude_error = self.inject(error)?; + let mut a = [0.0; MATRIX]; + for row in 0..N { + for col in 0..N { + a[row * MAX_DIM + col] = (if row == col { 1.0 } else { 0.0 }) + - update.gain[row][0] * update.h[0][col] + - update.gain[row][1] * update.h[1][col]; + } + } + let mut first = [0.0; MAX_DIM]; + let mut second = [0.0; MAX_DIM]; + for row in 0..N { + first[row] = + update.gain[row][0] * covariance.xx.get() + update.gain[row][1] * covariance.xy; + second[row] = + update.gain[row][0] * covariance.xy + update.gain[row][1] * covariance.yy.get(); + } + self.joseph_two(a, first, second, &update.gain); + self.reset_attitude_error(attitude_error); + Ok(()) + } + + fn joseph(&mut self, a: [f32; MATRIX], gain: &[f32; MAX_DIM], variance: f32) { + let old = self.covariance; + let mut temporary = [0.0; MATRIX]; + for row in 0..N { + for col in 0..N { + let mut sum = 0.0; + for k in 0..N { + sum += a[row * MAX_DIM + k] * old.get(k, col); + } + temporary[row * MAX_DIM + col] = sum; + } + } + let mut next = Covariance { + values: [0.0; MATRIX], + }; + for row in 0..N { + for col in 0..N { + let mut sum = 0.0; + for k in 0..N { + sum += temporary[row * MAX_DIM + k] * a[col * MAX_DIM + k]; + } + next.set(row, col, sum + gain[row] * variance * gain[col]); + } + } + next.symmetric(N); + self.covariance = next; + } + + fn joseph_two( + &mut self, + a: [f32; MATRIX], + first: [f32; MAX_DIM], + second: [f32; MAX_DIM], + gain: &[[f32; 2]; MAX_DIM], + ) { + let old = self.covariance; + let mut temporary = [0.0; MATRIX]; + for row in 0..N { + for col in 0..N { + let mut sum = 0.0; + for k in 0..N { + sum += a[row * MAX_DIM + k] * old.get(k, col); + } + temporary[row * MAX_DIM + col] = sum; + } + } + let mut next = Covariance { + values: [0.0; MATRIX], + }; + for row in 0..N { + for col in 0..N { + let mut sum = 0.0; + for k in 0..N { + sum += temporary[row * MAX_DIM + k] * a[col * MAX_DIM + k]; + } + let krkt = first[row] * gain[col][0] + second[row] * gain[col][1]; + next.set(row, col, sum + krkt); + } + } + next.symmetric(N); + self.covariance = next; + } + + fn inject(&mut self, error: [f32; MAX_DIM]) -> Result, MathError> { + if !error.iter().take(N).all(|value| value.is_finite()) { + return Err(MathError::NonFinite); + } + let position = self + .state + .position + .vector() + .add(Vector3::new(error[0], error[1], error[2])); + let velocity = self + .state + .velocity + .vector() + .add(Vector3::new(error[3], error[4], error[5])); + let attitude_error = Vector3::::new(error[6], error[7], error[8]); + let attitude = UnitQuaternion::from_small_rotation_vector(attitude_error)? + .compose(self.state.attitude)?; + let gyro = self + .state + .gyro_bias + .vector() + .add(Vector3::new(error[9], error[10], error[11])); + let accel = self + .state + .acceleration_bias + .vector() + .add(Vector3::new(error[12], error[13], error[14])); + if !position.is_finite() + || !velocity.is_finite() + || !gyro.is_finite() + || !accel.is_finite() + || ((N == TERRAIN_DIM) + && !(self.state.terrain_down_metres + error[TERRAIN]).is_finite()) + { + return Err(MathError::NonFinite); + } + self.state.position = PositionNed::metres(position.x(), position.y(), position.z()); + self.state.velocity = + VelocityNed::metres_per_second(velocity.x(), velocity.y(), velocity.z()); + self.state.attitude = attitude; + self.state.gyro_bias = AngularRateFrd::radians_per_second(gyro.x(), gyro.y(), gyro.z()); + self.state.acceleration_bias = + AccelerationFrd::metres_per_second_squared(accel.x(), accel.y(), accel.z()); + if N == TERRAIN_DIM { + self.state.terrain_down_metres += error[TERRAIN]; + } + Ok(attitude_error) + } + + /// Applies the first-order MEKF reset after a left-multiplicative attitude + /// error is injected in NED coordinates. Without this transform the + /// covariance would remain expressed around the old tangent origin. + fn reset_attitude_error(&mut self, attitude_error: Vector3) { + let mut reset = [0.0; MATRIX]; + for axis in 0..N { + reset[axis * MAX_DIM + axis] = 1.0; + } + let skew = [ + [0.0, -attitude_error.z(), attitude_error.y()], + [attitude_error.z(), 0.0, -attitude_error.x()], + [-attitude_error.y(), attitude_error.x(), 0.0], + ]; + for row in 0..3 { + for col in 0..3 { + reset[(ATTITUDE + row) * MAX_DIM + ATTITUDE + col] += 0.5 * skew[row][col]; + } + } + let old = self.covariance; + let mut temporary = [0.0; MATRIX]; + for row in 0..N { + for col in 0..N { + for k in 0..N { + temporary[row * MAX_DIM + col] += reset[row * MAX_DIM + k] * old.get(k, col); + } + } + } + let mut next = Covariance { + values: [0.0; MATRIX], + }; + for row in 0..N { + for col in 0..N { + for k in 0..N { + next.set( + row, + col, + next.get(row, col) + + temporary[row * MAX_DIM + k] * reset[col * MAX_DIM + k], + ); + } + } + } + next.symmetric(N); + self.covariance = next; + } +} + +#[derive(Clone, Copy)] +struct ScalarGain { + h: [f32; MAX_DIM], + gain: [f32; MAX_DIM], + innovation_variance: f32, +} +#[derive(Clone, Copy)] +struct TwoGain { + h: [[f32; MAX_DIM]; 2], + gain: [[f32; 2]; MAX_DIM], + inverse_s: [f32; 4], +} + +fn rotation_matrix(attitude: UnitQuaternion) -> [[f32; 3]; 3] { + let x = attitude.body_to_ned(Vector3::::new(1.0, 0.0, 0.0)); + let y = attitude.body_to_ned(Vector3::::new(0.0, 1.0, 0.0)); + let z = attitude.body_to_ned(Vector3::::new(0.0, 0.0, 1.0)); + [ + [x.x(), y.x(), z.x()], + [x.y(), y.y(), z.y()], + [x.z(), y.z(), z.z()], + ] +} + +fn tangent_cross_z(axis: usize, vector: Vector3) -> f32 { + match axis { + 0 => vector.y(), + 1 => -vector.x(), + _ => 0.0, + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::observation::ImuNoise; + use crate::{ + CalibrationRevision, Covariance2, FrameId, ObservationContext, ObservationSequence, + SourceId, + }; + use kaptan_sensor::imu::ImuSnapshot; + + fn context(time: u64, frame: FrameId, source: u8, quality: u8) -> ObservationContext { + ObservationContext::new( + Timestamp::from_microseconds(time), + Timestamp::from_microseconds(time + 1), + SourceId::new(source), + frame, + CalibrationRevision::new(1), + quality, + Validity::Valid, + ObservationSequence::new(time as u32), + ) + } + fn noise() -> ImuNoise { + ImuNoise { + gyro_rate: ScalarVariance::new(1.0e-4).unwrap(), + acceleration: ScalarVariance::new(1.0e-3).unwrap(), + } + } + + fn imu_stream() -> ImuStream { + ImuStream { + source: SourceId::new(1), + calibration: CalibrationRevision::new(1), + last_sequence: ObservationSequence::new(10), + } + } + fn imu(time: u64, acceleration_mg: [i16; 3], rate_mrad: [i16; 3]) -> ImuObservation { + ImuObservation::from_snapshot( + ImuSnapshot::new( + Timestamp::from_microseconds(time), + acceleration_mg, + rate_mrad, + 0, + ), + context(time, FrameId::BODY_FRD, 1, 255), + noise(), + ) + } + fn initialized() -> Eskf { + let mut init = StationaryInitializer::<8>::new(); + for time in 3..=10 { + assert!(init.push(imu(time, [0, 0, -1_000], [0; 3]))); + } + init.finish( + Timestamp::from_microseconds(10), + EstimatorConfig::default(), + IntegrityConfig::default(), + 2.0, + ) + .unwrap() + } + + fn range() -> RangeObservation { + RangeObservation { + context: context(10, FrameId::new(7), 2, 255), + metres: 2.0, + variance: ScalarVariance::new(0.1).unwrap(), + } + } + + fn range_extrinsics() -> SensorExtrinsics { + SensorExtrinsics { + source: SourceId::new(2), + sensor_frame: FrameId::new(7), + calibration: CalibrationRevision::new(1), + sensor_to_body: UnitQuaternion::identity(), + position_body: crate::PositionFrd::metres(0.0, 0.0, 0.0), + } + } + + fn flow() -> OpticalFlowObservation { + OpticalFlowObservation { + context: context(10, FrameId::new(8), 3, 255), + velocity: kaptan_math::Vector2::new(0.0, 0.0), + ground_distance_metres: 1.0, + covariance: Covariance2::new( + ScalarVariance::new(0.1).unwrap(), + 0.0, + ScalarVariance::new(0.1).unwrap(), + ) + .unwrap(), + } + } + + fn flow_extrinsics() -> SensorExtrinsics { + SensorExtrinsics { + source: SourceId::new(3), + sensor_frame: FrameId::new(8), + calibration: CalibrationRevision::new(1), + sensor_to_body: UnitQuaternion::identity(), + position_body: crate::PositionFrd::metres(0.0, 0.0, 0.0), + } + } + + fn local() -> LocalPositionVelocityObservation { + LocalPositionVelocityObservation { + context: context(10, FrameId::LOCAL_NED, 5, 255), + position: PositionNed::metres(0.0, 0.0, 0.0), + velocity: VelocityNed::metres_per_second(0.0, 0.0, 0.0), + position_variance: ScalarVariance::new(0.1).unwrap(), + velocity_variance: ScalarVariance::new(0.1).unwrap(), + } + } + + #[test] + fn stationary_initialization() { + let mut init = StationaryInitializer::<4>::new(); + for time in 0..4 { + init.push(imu(time, [0, 0, -990], [10, -20, 30])); + } + let filter: InertialEskf = init + .finish( + Timestamp::from_microseconds(3), + EstimatorConfig::default(), + IntegrityConfig::default(), + 0.0, + ) + .unwrap(); + assert!((filter.state().gyro_bias().vector().x() - 0.01).abs() < 1.0e-4); + assert!(filter.state().attitude().is_normalized()); + } + + #[test] + fn stationary_window_validation() { + let mut mixed = StationaryInitializer::<2>::new(); + assert!(mixed.push(imu(1, [0, 0, -1_000], [0; 3]))); + let mut wrong_source = imu(2, [0, 0, -1_000], [0; 3]); + wrong_source.context.source = SourceId::new(2); + assert!(!mixed.push(wrong_source)); + assert!(!mixed.is_complete()); + assert!(mixed.push(imu(3, [0, 0, -1_000], [0; 3]))); + assert!(mixed.push(imu(4, [0, 0, -1_000], [0; 3]))); + assert!( + mixed + .finish::<15>( + Timestamp::from_microseconds(4), + EstimatorConfig::default(), + IntegrityConfig::default(), + 0.0 + ) + .is_some() + ); + + let mut reordered = StationaryInitializer::<2>::new(); + assert!(reordered.push(imu(4, [0, 0, -1_000], [0; 3]))); + assert!(!reordered.push(imu(3, [0, 0, -1_000], [0; 3]))); + assert!(!reordered.is_complete()); + + let mut moving = StationaryInitializer::<2>::new(); + assert!(moving.push(imu(1, [0, 0, -1_000], [200, 0, 0]))); + assert!(moving.push(imu(2, [0, 0, -1_000], [200, 0, 0]))); + assert!( + moving + .finish::<15>( + Timestamp::from_microseconds(2), + EstimatorConfig::default(), + IntegrityConfig::default(), + 0.0 + ) + .is_none() + ); + } + + #[test] + fn rotation_normalizes() { + let mut filter: InertialEskf = initialized(); + for time in 11..1_011 { + filter + .propagate(imu(time * 1_000, [0, 0, -1_000], [0, 0, 1_000])) + .unwrap(); + } + assert!(filter.state().attitude().is_normalized()); + assert!(filter.covariance_is_valid()); + } + + #[test] + fn covariance_cholesky() { + let mut filter: InertialEskf = initialized(); + let mut seed = 0x4d59_5df4_u32; + for sample in 1..=2_000_u64 { + seed = seed.wrapping_mul(1_664_525).wrapping_add(1_013_904_223); + let x = (seed >> 16) as i16 % 200; + seed = seed.wrapping_mul(1_664_525).wrapping_add(1_013_904_223); + let y = (seed >> 16) as i16 % 200; + seed = seed.wrapping_mul(1_664_525).wrapping_add(1_013_904_223); + let yaw = (seed >> 16) as i16 % 100; + filter + .propagate(imu(10 + sample * 1_000, [x, y, -1_000], [0, 0, yaw])) + .unwrap(); + assert!(filter.state().attitude().is_normalized()); + assert!(filter.covariance_is_valid()); + } + } + + #[test] + fn propagation_uses_ned() { + let mut filter: InertialEskf = initialized(); + filter + .propagate(imu(10_010, [1_000, 0, -1_000], [0; 3])) + .unwrap(); + assert!(filter.state().velocity().vector().x() > 0.009); + assert!(filter.state().velocity().vector().z().abs() < 0.001); + } + + #[test] + fn propagation_matches_reference() { + let mut filter: InertialEskf = initialized(); + let dt = 0.01_f64; + for step in 1..=100_u64 { + filter + .propagate(imu(10 + step * 10_000, [1_000, 0, -1_000], [0; 3])) + .unwrap(); + } + let expected_velocity = GRAVITY_MPS2 as f64 * 100.0 * dt; + let expected_position = 0.5 * GRAVITY_MPS2 as f64 * (100.0 * dt).powi(2); + let state = filter.state(); + assert!((state.velocity().vector().x() as f64 - expected_velocity).abs() < 2.0e-4); + assert!((state.position().vector().x() as f64 - expected_position).abs() < 2.0e-4); + assert!(filter.covariance_is_valid()); + } + + #[test] + fn local_aiding_bounds_state() { + let mut filter: InertialEskf = initialized(); + let observation = LocalPositionVelocityObservation { + context: ObservationContext::new( + Timestamp::from_microseconds(10), + Timestamp::from_microseconds(11), + SourceId::new(5), + FrameId::LOCAL_NED, + CalibrationRevision::new(2), + 255, + Validity::Valid, + ObservationSequence::new(7), + ), + position: PositionNed::metres(2.0, -1.0, 0.5), + velocity: VelocityNed::metres_per_second(0.4, -0.2, 0.1), + position_variance: ScalarVariance::new(0.01).unwrap(), + velocity_variance: ScalarVariance::new(0.01).unwrap(), + }; + assert_eq!( + filter.fuse_local_position_velocity( + observation, + SourceId::new(5), + CalibrationRevision::new(2), + ), + Ok(FusionResult::Fused) + ); + let state = filter.state(); + assert!((state.position().vector().x() - 2.0).abs() < 0.01); + assert!((state.position().vector().y() + 1.0).abs() < 0.01); + assert!((state.velocity().vector().x() - 0.4).abs() < 0.01); + assert!( + filter + .health(Timestamp::from_microseconds(12)) + .capabilities + .local_position + ); + } + + #[test] + fn rejected_aiding_atomic() { + let mut filter: InertialEskf = initialized(); + let before_state = filter.state(); + let before_covariance = filter.covariance; + let observation = LocalPositionVelocityObservation { + context: ObservationContext::new( + Timestamp::from_microseconds(10), + Timestamp::from_microseconds(11), + SourceId::new(5), + FrameId::LOCAL_NED, + CalibrationRevision::new(2), + 255, + Validity::Valid, + ObservationSequence::new(8), + ), + position: PositionNed::metres(0.0, 9_999.0, 0.0), + velocity: VelocityNed::metres_per_second(0.0, 0.0, 0.0), + position_variance: ScalarVariance::new(0.01).unwrap(), + velocity_variance: ScalarVariance::new(0.01).unwrap(), + }; + assert_eq!( + filter.fuse_local_position_velocity( + observation, + SourceId::new(5), + CalibrationRevision::new(2), + ), + Err(FusionError::InnovationRejected) + ); + assert_eq!(filter.state(), before_state); + assert_eq!(filter.covariance.values, before_covariance.values); + let health = filter.health(Timestamp::from_microseconds(11)); + assert_eq!(health.local_position_velocity, SourceStatus::Unknown); + assert!(!health.capabilities.local_position); + assert!(!health.capabilities.local_velocity); + } + + #[test] + fn local_reject_does_not_refresh_health() { + let mut filter: InertialEskf = initialized(); + assert_eq!( + filter.fuse_local_position_velocity( + local(), + SourceId::new(5), + CalibrationRevision::new(1), + ), + Ok(FusionResult::Fused) + ); + + let mut rejected = local(); + rejected.context.sequence = ObservationSequence::new(11); + rejected.context.receipt_time = Timestamp::from_microseconds(250_010); + rejected.position = PositionNed::metres(9_999.0, 0.0, 0.0); + assert_eq!( + filter.fuse_local_position_velocity( + rejected, + SourceId::new(5), + CalibrationRevision::new(1), + ), + Err(FusionError::InnovationRejected) + ); + let health = filter.health(Timestamp::from_microseconds(250_012)); + assert_eq!(health.local_position_velocity, SourceStatus::Stale); + assert!(!health.capabilities.local_position); + assert!(!health.capabilities.local_velocity); + } + + #[test] + fn rejects_non_monotonic_imu_time() { + let mut filter: InertialEskf = initialized(); + assert_eq!( + filter.propagate(imu(10, [0, 0, -1_000], [0; 3])), + Err(PropagationError::NonMonotonicTimestamp) + ); + } + + #[test] + fn propagation_rejects_bad_imu() { + let mut filter: InertialEskf = initialized(); + let before = filter.state(); + let mut invalid = imu(20_000, [0, 0, -1_000], [0; 3]); + invalid.context.validity = Validity::Invalid; + assert_eq!(filter.propagate(invalid), Err(PropagationError::InvalidImu)); + assert_eq!( + filter.propagate(imu(40_001, [0, 0, -1_000], [0; 3])), + Err(PropagationError::ImuGapTooLarge) + ); + assert_eq!(filter.state(), before); + assert!(!filter.requires_reinitialization()); + } + + #[test] + fn propagation_binds_initialized_imu_stream_without_mutation_on_refusal() { + let mut source_filter: InertialEskf = initialized(); + let source_before = source_filter; + let mut foreign_source = imu(11, [0, 0, -1_000], [0; 3]); + foreign_source.context.source = SourceId::new(9); + assert_eq!( + source_filter.propagate(foreign_source), + Err(PropagationError::ImuSourceMismatch) + ); + assert_eq!(source_filter.time(), source_before.time()); + assert_eq!(source_filter.state(), source_before.state()); + assert_eq!( + source_filter.covariance.values, + source_before.covariance.values + ); + assert!(!source_filter.requires_reinitialization()); + assert_eq!( + source_filter.propagate(imu(11, [0, 0, -1_000], [0; 3])), + Ok(()) + ); + + let mut calibration_filter: InertialEskf = initialized(); + let calibration_before = calibration_filter; + let mut changed_calibration = imu(11, [0, 0, -1_000], [0; 3]); + changed_calibration.context.calibration = CalibrationRevision::new(2); + assert_eq!( + calibration_filter.propagate(changed_calibration), + Err(PropagationError::ImuCalibrationMismatch) + ); + assert_eq!(calibration_filter.time(), calibration_before.time()); + assert_eq!(calibration_filter.state(), calibration_before.state()); + assert_eq!( + calibration_filter.covariance.values, + calibration_before.covariance.values + ); + assert!(!calibration_filter.requires_reinitialization()); + assert_eq!( + calibration_filter.propagate(imu(11, [0, 0, -1_000], [0; 3])), + Ok(()) + ); + + let mut sequence_filter: InertialEskf = initialized(); + assert_eq!( + sequence_filter.propagate(imu(11, [0, 0, -1_000], [0; 3])), + Ok(()) + ); + let sequence_before = sequence_filter; + let mut replay = imu(12, [0, 0, -1_000], [0; 3]); + replay.context.sequence = ObservationSequence::new(11); + assert_eq!( + sequence_filter.propagate(replay), + Err(PropagationError::ReplayedImu) + ); + assert_eq!(sequence_filter.time(), sequence_before.time()); + assert_eq!(sequence_filter.state(), sequence_before.state()); + assert_eq!( + sequence_filter.covariance.values, + sequence_before.covariance.values + ); + assert!(!sequence_filter.requires_reinitialization()); + assert_eq!( + sequence_filter.propagate(imu(12, [0, 0, -1_000], [0; 3])), + Ok(()) + ); + } + + #[test] + fn covariance_fault_latch() { + let mut filter: InertialEskf = initialized(); + let before = filter.state(); + for row in 0..INERTIAL_DIM { + for col in 0..INERTIAL_DIM { + filter.covariance.set(row, col, f32::MAX); + } + } + assert_eq!( + filter.propagate(imu(20_000, [0, 0, -1_000], [0; 3])), + Err(PropagationError::FilterInvalid) + ); + assert_eq!(filter.state(), before); + assert!(filter.requires_reinitialization()); + let health = filter.health(Timestamp::from_microseconds(20_000)); + assert!(!health.finite); + assert!(!health.initialized); + assert_eq!( + filter.propagate(imu(20_001, [0, 0, -1_000], [0; 3])), + Err(PropagationError::FilterInvalid) + ); + } + + #[test] + fn rotation_limit_preserves_state() { + let mut filter: InertialEskf = initialized(); + filter.config.maximum_imu_interval_microseconds = 100_000; + let before = filter.state(); + + assert_eq!( + filter.propagate(imu(100_010, [0, 0, -1_000], [32_767, 0, 0])), + Err(PropagationError::RotationIncrementTooLarge) + ); + assert_eq!(filter.state(), before); + } + + #[test] + fn range_update_is_finite() { + let mut filter: LocalTerrainEskf = initialized(); + let extrinsics = SensorExtrinsics { + source: SourceId::new(2), + sensor_frame: FrameId::new(7), + calibration: CalibrationRevision::new(1), + sensor_to_body: UnitQuaternion::identity(), + position_body: crate::PositionFrd::metres(0.0, 0.0, 0.2), + }; + let observation = RangeObservation { + context: context(10, FrameId::new(7), 2, 255), + metres: 1.8, + variance: ScalarVariance::new(0.01).unwrap(), + }; + assert_eq!( + filter.fuse_range(observation, extrinsics), + Ok(FusionResult::Fused) + ); + assert!(filter.covariance_is_valid()); + assert!( + filter + .health(Timestamp::from_microseconds(11)) + .capabilities + .terrain_relative_height + ); + } + + #[test] + fn range_geometry_and_staleness() { + let mut filter: LocalTerrainEskf = initialized(); + let extrinsics = SensorExtrinsics { + source: SourceId::new(2), + sensor_frame: FrameId::new(7), + calibration: CalibrationRevision::new(1), + sensor_to_body: UnitQuaternion::identity(), + position_body: crate::PositionFrd::metres(0.0, 0.0, 0.0), + }; + filter.state.attitude = + UnitQuaternion::from_components(0.707_106_77, 0.0, 0.707_106_77, 0.0).unwrap(); + let observation = RangeObservation { + context: context(10, FrameId::new(7), 2, 255), + metres: 2.0, + variance: ScalarVariance::new(0.1).unwrap(), + }; + assert_eq!( + filter.fuse_range(observation, extrinsics), + Err(FusionError::InvalidRangeGeometry) + ); + + filter.state.attitude = UnitQuaternion::identity(); + let mut recovered = observation; + recovered.context.sequence = ObservationSequence::new(11); + assert_eq!( + filter.fuse_range(recovered, extrinsics), + Ok(FusionResult::Fused) + ); + let health = filter.health(Timestamp::from_microseconds(300_002)); + assert_eq!(health.range, SourceStatus::Stale); + assert!(!health.capabilities.terrain_relative_height); + } + + #[test] + fn flow_frame_and_quality() { + let mut filter: InertialEskf = initialized(); + let extrinsics = SensorExtrinsics { + source: SourceId::new(3), + sensor_frame: FrameId::new(8), + calibration: CalibrationRevision::new(1), + sensor_to_body: UnitQuaternion::from_components(0.0, 0.0, 0.0, 1.0).unwrap(), + position_body: crate::PositionFrd::metres(0.0, 0.0, 0.0), + }; + filter.state.velocity = VelocityNed::metres_per_second(1.0, 0.0, 0.0); + let covariance = Covariance2::new( + ScalarVariance::new(0.01).unwrap(), + 0.0, + ScalarVariance::new(0.01).unwrap(), + ) + .unwrap(); + let rejected = OpticalFlowObservation { + context: context(10, FrameId::new(8), 3, 1), + velocity: kaptan_math::Vector2::new(0.0, 0.0), + ground_distance_metres: 1.0, + covariance, + }; + assert_eq!( + filter.fuse_optical_flow(rejected, extrinsics), + Err(FusionError::InvalidObservation) + ); + let mut accepted = OpticalFlowObservation { + context: context(10, FrameId::new(8), 3, 255), + velocity: kaptan_math::Vector2::new(-1.0, 0.0), + ground_distance_metres: 1.0, + covariance, + }; + accepted.context.sequence = ObservationSequence::new(11); + assert_eq!( + filter.fuse_optical_flow(accepted, extrinsics), + Ok(FusionResult::Fused) + ); + } + + #[test] + fn two_axis_gain_validation() { + let mut filter: InertialEskf = initialized(); + filter.covariance.set(VELOCITY, VELOCITY, -0.02); + filter.covariance.set(VELOCITY + 1, VELOCITY + 1, -0.02); + let mut h = [[0.0; MAX_DIM]; 2]; + h[0][VELOCITY] = 1.0; + h[1][VELOCITY + 1] = 1.0; + let covariance = Covariance2::new( + ScalarVariance::new(0.01).unwrap(), + 0.0, + ScalarVariance::new(0.01).unwrap(), + ) + .unwrap(); + + assert!(filter.two_gain(h, covariance).is_none()); + } + + #[test] + fn range_source_recovery() { + let mut filter: LocalTerrainEskf = initialized(); + let extrinsics = SensorExtrinsics { + source: SourceId::new(4), + sensor_frame: FrameId::new(7), + calibration: CalibrationRevision::new(1), + sensor_to_body: UnitQuaternion::identity(), + position_body: crate::PositionFrd::metres(0.0, 0.0, 0.0), + }; + let bad = |sequence| RangeObservation { + context: ObservationContext::new( + Timestamp::from_microseconds(10), + Timestamp::from_microseconds(10 + u64::from(sequence)), + SourceId::new(4), + FrameId::new(7), + CalibrationRevision::new(1), + 255, + Validity::Valid, + ObservationSequence::new(sequence), + ), + metres: 40.0, + variance: ScalarVariance::new(0.001).unwrap(), + }; + for sequence in 0..3 { + assert_eq!( + filter.fuse_range(bad(sequence), extrinsics), + Err(FusionError::InnovationRejected) + ); + } + assert_eq!( + filter.health(Timestamp::from_microseconds(20)).range, + SourceStatus::Unknown + ); + let good = |sequence| RangeObservation { + context: ObservationContext::new( + Timestamp::from_microseconds(10), + Timestamp::from_microseconds(30 + u64::from(sequence)), + SourceId::new(4), + FrameId::new(7), + CalibrationRevision::new(1), + 255, + Validity::Valid, + ObservationSequence::new(sequence), + ), + metres: 2.0, + variance: ScalarVariance::new(0.1).unwrap(), + }; + assert_eq!( + filter.fuse_range(good(3), extrinsics), + Ok(FusionResult::SourceIsolated) + ); + assert_eq!( + filter.health(Timestamp::from_microseconds(34)).range, + SourceStatus::Isolated + ); + assert_eq!( + filter.fuse_range(good(4), extrinsics), + Ok(FusionResult::Fused) + ); + } + + #[test] + fn configuration_validation() { + let config = EstimatorConfig::default(); + for invalid in [ + EstimatorConfig { + gravity_mps2: 0.0, + ..config + }, + EstimatorConfig { + gravity_mps2: f32::NAN, + ..config + }, + EstimatorConfig { + gyro_noise_variance: -1.0, + ..config + }, + EstimatorConfig { + gyro_noise_variance: f32::NAN, + ..config + }, + EstimatorConfig { + acceleration_noise_variance: f32::NAN, + ..config + }, + EstimatorConfig { + acceleration_noise_variance: -1.0, + ..config + }, + EstimatorConfig { + gyro_bias_walk_variance: -1.0, + ..config + }, + EstimatorConfig { + gyro_bias_walk_variance: f32::NAN, + ..config + }, + EstimatorConfig { + acceleration_bias_walk_variance: -1.0, + ..config + }, + EstimatorConfig { + acceleration_bias_walk_variance: f32::NAN, + ..config + }, + EstimatorConfig { + terrain_walk_variance: -1.0, + ..config + }, + EstimatorConfig { + terrain_walk_variance: f32::NAN, + ..config + }, + EstimatorConfig { + maximum_imu_interval_microseconds: 0, + ..config + }, + EstimatorConfig { + minimum_range_down_cosine: 1.1, + ..config + }, + EstimatorConfig { + minimum_range_down_cosine: f32::NAN, + ..config + }, + EstimatorConfig { + minimum_range_down_cosine: 0.0, + ..config + }, + EstimatorConfig { + minimum_range_metres: 0.0, + ..config + }, + EstimatorConfig { + minimum_range_metres: f32::NAN, + ..config + }, + EstimatorConfig { + maximum_range_metres: 0.01, + ..config + }, + EstimatorConfig { + maximum_range_metres: f32::NAN, + ..config + }, + EstimatorConfig { + maximum_flow_speed_metres_per_second: 0.0, + ..config + }, + EstimatorConfig { + maximum_flow_speed_metres_per_second: f32::NAN, + ..config + }, + EstimatorConfig { + range_gate_chi_square: 0.0, + ..config + }, + EstimatorConfig { + range_gate_chi_square: f32::NAN, + ..config + }, + EstimatorConfig { + flow_gate_chi_square: 0.0, + ..config + }, + EstimatorConfig { + flow_gate_chi_square: f32::NAN, + ..config + }, + EstimatorConfig { + maximum_local_position_metres: 0.0, + ..config + }, + EstimatorConfig { + maximum_local_position_metres: f32::NAN, + ..config + }, + EstimatorConfig { + maximum_local_speed_metres_per_second: 0.0, + ..config + }, + EstimatorConfig { + maximum_local_speed_metres_per_second: f32::NAN, + ..config + }, + EstimatorConfig { + local_state_gate_chi_square: 0.0, + ..config + }, + EstimatorConfig { + local_state_gate_chi_square: f32::NAN, + ..config + }, + ] { + assert!(!invalid.is_valid()); + } + for invalid in [ + IntegrityConfig { + maximum_age_microseconds: 0, + ..IntegrityConfig::default() + }, + IntegrityConfig { + rejection_limit: 0, + ..IntegrityConfig::default() + }, + IntegrityConfig { + readmission_accepts: 0, + ..IntegrityConfig::default() + }, + ] { + assert!(!invalid.is_valid()); + } + } + + #[test] + fn initialization_validation() { + let config = EstimatorConfig::default(); + let mut initializer = StationaryInitializer::<1>::default(); + let mut invalid_imu = imu(1, [0, 0, -1_000], [0; 3]); + invalid_imu.context.validity = Validity::Invalid; + assert!(!initializer.push(invalid_imu)); + invalid_imu.context.validity = Validity::Valid; + invalid_imu.context.frame = FrameId::new(7); + assert!(!initializer.push(invalid_imu)); + invalid_imu.context.frame = FrameId::BODY_FRD; + invalid_imu.acceleration = AccelerationFrd::metres_per_second_squared(f32::NAN, 0.0, 0.0); + assert!(!initializer.push(invalid_imu)); + invalid_imu.acceleration = AccelerationFrd::metres_per_second_squared(0.0, 0.0, 0.0); + invalid_imu.angular_rate = AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); + assert!(!initializer.push(invalid_imu)); + assert!(initializer.push(imu(2, [0, 0, -1_000], [0; 3]))); + assert!(initializer.is_complete()); + assert!(!initializer.push(imu(3, [0, 0, -1_000], [0; 3]))); + assert!( + initializer + .finish::<15>( + Timestamp::from_microseconds(2), + config, + IntegrityConfig::default(), + f32::NAN + ) + .is_none() + ); + assert!( + StationaryInitializer::<0>::new() + .finish::<15>( + Timestamp::from_microseconds(4), + config, + IntegrityConfig::default(), + 0.0 + ) + .is_none() + ); + assert!( + StationaryInitializer::<1>::new() + .finish::<15>( + Timestamp::from_microseconds(10), + config, + IntegrityConfig::default(), + 0.0 + ) + .is_none() + ); + let mut invalid_finish = StationaryInitializer::<1>::new(); + assert!(invalid_finish.push(imu(4, [0, 0, -1_000], [0; 3]))); + assert!( + invalid_finish + .finish::<15>( + Timestamp::from_microseconds(4), + EstimatorConfig { + gravity_mps2: 0.0, + ..config + }, + IntegrityConfig::default(), + 0.0 + ) + .is_none() + ); + let mut invalid_sum = StationaryInitializer::<1>::new(); + invalid_sum.count = 1; + invalid_sum.last_time = Some(Timestamp::from_microseconds(10)); + invalid_sum.gyro_sum = Vector3::new(f32::NAN, 0.0, 0.0); + assert!( + invalid_sum + .finish::<15>( + Timestamp::from_microseconds(10), + config, + IntegrityConfig::default(), + 0.0 + ) + .is_none() + ); + let mut invalid_acceleration_sum = StationaryInitializer::<1>::new(); + invalid_acceleration_sum.count = 1; + invalid_acceleration_sum.last_time = Some(Timestamp::from_microseconds(10)); + invalid_acceleration_sum.acceleration_sum = Vector3::new(f32::NAN, 0.0, 0.0); + assert!( + invalid_acceleration_sum + .finish::<15>( + Timestamp::from_microseconds(10), + config, + IntegrityConfig::default(), + 0.0 + ) + .is_none() + ); + + let mut timestamp_mismatch = StationaryInitializer::<1>::new(); + assert!(timestamp_mismatch.push(imu(5, [0, 0, -1_000], [0; 3]))); + assert!( + timestamp_mismatch + .finish::<15>( + Timestamp::from_microseconds(6), + config, + IntegrityConfig::default(), + 0.0 + ) + .is_none() + ); + } + + #[test] + fn state_validation() { + let config = EstimatorConfig::default(); + let filter: LocalTerrainEskf = initialized(); + let state = filter.state(); + assert!(state.acceleration_bias().vector().norm() < 2.0e-6); + assert_eq!(state.terrain_down_metres::<16>(), Some(2.0)); + assert_eq!( + initialized::<15>().state().terrain_down_metres::<15>(), + None + ); + assert_eq!( + filter.health(Timestamp::from_microseconds(10)).range, + SourceStatus::Unknown + ); + assert!( + filter + .health(Timestamp::from_microseconds(10)) + .capabilities + .attitude + ); + let mut invalid_state = state; + invalid_state.position = PositionNed::metres(f32::NAN, 0.0, 0.0); + assert!(!state_is_valid::<16>(invalid_state)); + invalid_state = state; + invalid_state.velocity = VelocityNed::metres_per_second(f32::NAN, 0.0, 0.0); + assert!(!state_is_valid::<16>(invalid_state)); + invalid_state = state; + invalid_state.gyro_bias = AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); + assert!(!state_is_valid::<16>(invalid_state)); + invalid_state = state; + invalid_state.acceleration_bias = + AccelerationFrd::metres_per_second_squared(f32::NAN, 0.0, 0.0); + assert!(!state_is_valid::<16>(invalid_state)); + invalid_state = state; + invalid_state.terrain_down_metres = f32::NAN; + assert!(!state_is_valid::<16>(invalid_state)); + assert!( + Eskf::<15>::new( + Timestamp::from_microseconds(10), + state, + EstimatorConfig { + gravity_mps2: 0.0, + ..config + }, + IntegrityConfig::default(), + imu_stream(), + ) + .is_err() + ); + assert!( + Eskf::<15>::new( + Timestamp::from_microseconds(10), + state, + config, + IntegrityConfig { + rejection_limit: 0, + ..IntegrityConfig::default() + }, + imu_stream(), + ) + .is_err() + ); + } + + #[test] + fn covariance_validation() { + let config = EstimatorConfig::default(); + let state = initialized::<16>().state(); + let mut covariance = Covariance::diagonal(); + covariance.set(0, 1, f32::NAN); + assert!(!covariance.finite(2)); + assert!(!covariance.positive_definite(2)); + covariance = Covariance { + values: [0.0; MATRIX], + }; + assert!(!covariance.positive_definite(2)); + covariance.set(0, 0, 1.0); + covariance.set(1, 1, 1.0); + covariance.set(1, 0, 2.0); + covariance.set(0, 1, 2.0); + assert!(!covariance.positive_definite(2)); + covariance = Covariance { + values: [0.0; MATRIX], + }; + covariance.set(0, 0, f32::MIN_POSITIVE); + covariance.set(1, 0, f32::MAX); + covariance.set(1, 1, f32::MAX); + assert!(!covariance.positive_definite(2)); + + assert!( + Eskf::<14>::new( + Timestamp::from_microseconds(10), + state, + config, + IntegrityConfig::default(), + imu_stream(), + ) + .is_err() + ); + } + + #[test] + fn source_integrity_state_machine() { + let config = IntegrityConfig { + maximum_age_microseconds: 5, + rejection_limit: 2, + readmission_accepts: 2, + }; + let mut integrity = SourceIntegrity::new(config); + assert_eq!( + integrity.status(None, Timestamp::from_microseconds(0)), + SourceStatus::Unknown + ); + assert_eq!( + integrity.status(Some(SourceId::new(1)), Timestamp::from_microseconds(0)), + SourceStatus::Unknown + ); + for source in 1..=4 { + assert!(integrity.reserve(SourceId::new(source))); + } + assert!(integrity.reserve(SourceId::new(1))); + assert!(!integrity.reserve(SourceId::new(5))); + assert!(integrity.accepts_sequence(SourceId::new(1), ObservationSequence::new(10))); + assert!(!integrity.accepts_sequence(SourceId::new(1), ObservationSequence::new(10))); + assert!(!integrity.accepts_sequence(SourceId::new(1), ObservationSequence::new(9))); + assert!(integrity.accepts_sequence(SourceId::new(1), ObservationSequence::new(11))); + assert!(integrity.accept(SourceId::new(1), Timestamp::from_microseconds(9))); + integrity.reject(SourceId::new(1)); + assert_eq!( + integrity.status(Some(SourceId::new(1)), Timestamp::from_microseconds(10)), + SourceStatus::Healthy + ); + assert_eq!( + integrity.status(Some(SourceId::new(1)), Timestamp::from_microseconds(16)), + SourceStatus::Stale + ); + integrity.reject(SourceId::new(1)); + assert_eq!( + integrity.status(Some(SourceId::new(1)), Timestamp::from_microseconds(17)), + SourceStatus::Isolated + ); + assert!(!integrity.accept(SourceId::new(1), Timestamp::from_microseconds(18))); + assert!(integrity.accept(SourceId::new(1), Timestamp::from_microseconds(19))); + assert_eq!( + integrity.status(Some(SourceId::new(1)), Timestamp::from_microseconds(19)), + SourceStatus::Healthy + ); + } + + #[test] + fn range_metadata_validation() { + let range_extrinsics = range_extrinsics(); + let mut mismatch = range(); + mismatch.context.measurement_time = Timestamp::from_microseconds(11); + assert_eq!( + initialized::<16>().fuse_range(mismatch, range_extrinsics), + Err(FusionError::TimestampMismatch) + ); + assert_eq!( + initialized::<15>().fuse_range(range(), range_extrinsics), + Err(FusionError::TerrainProfileRequired) + ); + let mut invalid = range(); + invalid.context.validity = Validity::Invalid; + assert_eq!( + initialized::<16>().fuse_range(invalid, range_extrinsics), + Err(FusionError::InvalidObservation) + ); + let mut wrong_frame = range(); + wrong_frame.context.frame = FrameId::new(8); + assert_eq!( + initialized::<16>().fuse_range(wrong_frame, range_extrinsics), + Err(FusionError::FrameMismatch) + ); + let mut wrong_source = range(); + wrong_source.context.source = SourceId::new(3); + assert_eq!( + initialized::<16>().fuse_range(wrong_source, range_extrinsics), + Err(FusionError::SourceMismatch) + ); + let mut wrong_calibration = range(); + wrong_calibration.context.calibration = CalibrationRevision::new(2); + assert_eq!( + initialized::<16>().fuse_range(wrong_calibration, range_extrinsics), + Err(FusionError::CalibrationMismatch) + ); + let mut implausible = range(); + implausible.metres = 0.0; + assert_eq!( + initialized::<16>().fuse_range(implausible, range_extrinsics), + Err(FusionError::Implausible) + ); + let mut range_quality = range(); + range_quality.context.quality = 0; + assert_eq!( + initialized::<16>().fuse_range(range_quality, range_extrinsics), + Err(FusionError::InvalidObservation) + ); + let mut range_nan = range(); + range_nan.metres = f32::NAN; + assert_eq!( + initialized::<16>().fuse_range(range_nan, range_extrinsics), + Err(FusionError::Implausible) + ); + let mut range_high = range(); + range_high.metres = 41.0; + assert_eq!( + initialized::<16>().fuse_range(range_high, range_extrinsics), + Err(FusionError::Implausible) + ); + let mut replay = initialized::<16>(); + assert_eq!( + replay.fuse_range(range(), range_extrinsics), + Ok(FusionResult::Fused) + ); + assert_eq!( + replay.fuse_range(range(), range_extrinsics), + Err(FusionError::ReplayedObservation) + ); + } + + #[test] + fn rejected_aiding_is_unhealthy() { + let mut filter: LocalTerrainEskf = initialized(); + let mut wrong_source = range(); + wrong_source.context.source = SourceId::new(4); + assert_eq!( + filter.fuse_range(wrong_source, range_extrinsics()), + Err(FusionError::SourceMismatch) + ); + assert!(filter.integrity.entries.iter().all(|entry| !entry.used)); + + let mut rejected = range(); + rejected.metres = 40.0; + rejected.context.sequence = ObservationSequence::new(10); + assert_eq!( + filter.fuse_range(rejected, range_extrinsics()), + Err(FusionError::InnovationRejected) + ); + assert_eq!( + filter.health(Timestamp::from_microseconds(10)).range, + SourceStatus::Unknown + ); + + let mut accepted = range(); + accepted.context.sequence = ObservationSequence::new(11); + accepted.context.receipt_time = Timestamp::from_microseconds(11); + assert_eq!( + filter.fuse_range(accepted, range_extrinsics()), + Ok(FusionResult::Fused) + ); + assert_eq!( + filter.health(Timestamp::from_microseconds(11)).range, + SourceStatus::Healthy + ); + } + + #[test] + fn bad_metadata_cannot_fill_sources() { + let mut filter: LocalTerrainEskf = initialized(); + for source in 1..=4 { + let mut malformed = range(); + malformed.context.source = SourceId::new(source); + malformed.context.frame = FrameId::new(8); + let mut extrinsics = range_extrinsics(); + extrinsics.source = SourceId::new(source); + assert_eq!( + filter.fuse_range(malformed, extrinsics), + Err(FusionError::FrameMismatch) + ); + } + assert!(filter.integrity.entries.iter().all(|entry| !entry.used)); + + let mut valid = range(); + valid.context.source = SourceId::new(5); + let mut extrinsics = range_extrinsics(); + extrinsics.source = SourceId::new(5); + assert_eq!( + filter.fuse_range(valid, extrinsics), + Ok(FusionResult::Fused) + ); + } + + #[test] + fn reserved_source_isolates_on_bad_metadata() { + let mut filter: LocalTerrainEskf = initialized(); + assert_eq!( + filter.fuse_range(range(), range_extrinsics()), + Ok(FusionResult::Fused) + ); + for _ in 0..IntegrityConfig::default().rejection_limit { + let mut malformed = range(); + malformed.context.frame = FrameId::new(8); + assert_eq!( + filter.fuse_range(malformed, range_extrinsics()), + Err(FusionError::FrameMismatch) + ); + } + assert_eq!( + filter.health(Timestamp::from_microseconds(11)).range, + SourceStatus::Isolated + ); + } + + #[test] + fn flow_metadata_validation() { + let flow_extrinsics = flow_extrinsics(); + let mut flow_invalid = flow(); + flow_invalid.context.quality = 0; + assert_eq!( + initialized::<15>().fuse_optical_flow(flow_invalid, flow_extrinsics), + Err(FusionError::InvalidObservation) + ); + let mut flow_validity = flow(); + flow_validity.context.validity = Validity::Invalid; + assert_eq!( + initialized::<15>().fuse_optical_flow(flow_validity, flow_extrinsics), + Err(FusionError::InvalidObservation) + ); + let mut flow_frame = flow(); + flow_frame.context.frame = FrameId::new(9); + assert_eq!( + initialized::<15>().fuse_optical_flow(flow_frame, flow_extrinsics), + Err(FusionError::FrameMismatch) + ); + let mut flow_source = flow(); + flow_source.context.source = SourceId::new(4); + assert_eq!( + initialized::<15>().fuse_optical_flow(flow_source, flow_extrinsics), + Err(FusionError::SourceMismatch) + ); + let mut flow_calibration = flow(); + flow_calibration.context.calibration = CalibrationRevision::new(2); + assert_eq!( + initialized::<15>().fuse_optical_flow(flow_calibration, flow_extrinsics), + Err(FusionError::CalibrationMismatch) + ); + let mut flow_implausible = flow(); + flow_implausible.ground_distance_metres = 0.0; + assert_eq!( + initialized::<15>().fuse_optical_flow(flow_implausible, flow_extrinsics), + Err(FusionError::Implausible) + ); + let mut flow_nan_velocity = flow(); + flow_nan_velocity.velocity = kaptan_math::Vector2::new(f32::NAN, 0.0); + assert_eq!( + initialized::<15>().fuse_optical_flow(flow_nan_velocity, flow_extrinsics), + Err(FusionError::Implausible) + ); + let mut flow_nan_distance = flow(); + flow_nan_distance.ground_distance_metres = f32::NAN; + assert_eq!( + initialized::<15>().fuse_optical_flow(flow_nan_distance, flow_extrinsics), + Err(FusionError::Implausible) + ); + let mut flow_fast_x = flow(); + flow_fast_x.velocity = kaptan_math::Vector2::new(26.0, 0.0); + assert_eq!( + initialized::<15>().fuse_optical_flow(flow_fast_x, flow_extrinsics), + Err(FusionError::Implausible) + ); + let mut flow_fast_y = flow(); + flow_fast_y.velocity = kaptan_math::Vector2::new(0.0, 26.0); + assert_eq!( + initialized::<15>().fuse_optical_flow(flow_fast_y, flow_extrinsics), + Err(FusionError::Implausible) + ); + } + + #[test] + fn optical_flow_refuses_offset_sensor_without_mutating_integrity() { + let mut filter: InertialEskf = initialized(); + let mut lever_extrinsics = flow_extrinsics(); + lever_extrinsics.position_body = crate::PositionFrd::metres(1.0, 0.0, 0.0); + assert_eq!( + filter.fuse_optical_flow(flow(), lever_extrinsics), + Err(FusionError::UnsupportedOpticalFlowLeverArm) + ); + assert!(filter.integrity.entries.iter().all(|entry| !entry.used)); + } + + #[test] + fn optical_flow_models_attitude_error_for_a_colocated_sensor() { + let mut attitude_filter: InertialEskf = initialized(); + attitude_filter.state.velocity = VelocityNed::metres_per_second(2.0, 0.0, 0.0); + let mut attitude_flow = flow(); + attitude_flow.velocity = kaptan_math::Vector2::new(2.0, 0.0); + let before_attitude = attitude_filter.covariance.get(ATTITUDE + 2, ATTITUDE + 2); + assert_eq!( + attitude_filter.fuse_optical_flow(attitude_flow, flow_extrinsics()), + Ok(FusionResult::Fused) + ); + assert!(attitude_filter.covariance.get(ATTITUDE + 2, ATTITUDE + 2) < before_attitude); + + let mut finite_difference: InertialEskf = initialized(); + finite_difference.state.velocity = VelocityNed::metres_per_second(1.2, -0.7, 0.4); + finite_difference.state.attitude = + UnitQuaternion::from_components(0.962_425_2, 0.087_155_7, -0.130_526_2, 0.217_376_3) + .unwrap(); + let mut rotated_sensor = flow_extrinsics(); + rotated_sensor.sensor_to_body = + UnitQuaternion::from_components(0.965_925_8, 0.0, 0.0, 0.258_819).unwrap(); + let (_, jacobian) = finite_difference.co_located_flow_model(rotated_sensor); + let base_attitude = finite_difference.state.attitude; + const EPSILON: f32 = 1.0e-3; + for axis in 0..3 { + let basis = match axis { + 0 => Vector3::::new(1.0, 0.0, 0.0), + 1 => Vector3::::new(0.0, 1.0, 0.0), + _ => Vector3::::new(0.0, 0.0, 1.0), + }; + let mut plus = finite_difference; + plus.state.attitude = UnitQuaternion::from_small_rotation_vector(basis.scale(EPSILON)) + .unwrap() + .compose(base_attitude) + .unwrap(); + let mut minus = finite_difference; + minus.state.attitude = + UnitQuaternion::from_small_rotation_vector(basis.scale(-EPSILON)) + .unwrap() + .compose(base_attitude) + .unwrap(); + let (positive, _) = plus.co_located_flow_model(rotated_sensor); + let (negative, _) = minus.co_located_flow_model(rotated_sensor); + for row in 0..2 { + let numerical = (positive[row] - negative[row]) / (2.0 * EPSILON); + assert!((numerical - jacobian[row][ATTITUDE + axis]).abs() < 2.0e-3); + } + } + } + + #[test] + fn left_mekf_reset_uses_positive_half_skew() { + let mut filter: InertialEskf = initialized(); + filter.covariance.set(ATTITUDE + 1, POSITION, 2.0); + filter.covariance.set(POSITION, ATTITUDE + 1, 2.0); + filter.reset_attitude_error(Vector3::new(0.0, 0.0, 0.2)); + assert!((filter.covariance.get(ATTITUDE, POSITION) + 0.2).abs() < 1.0e-6); + assert!((filter.covariance.get(POSITION, ATTITUDE) + 0.2).abs() < 1.0e-6); + } + + #[test] + fn local_metadata_validation() { + let mut local_invalid = local(); + local_invalid.context.quality = 0; + assert_eq!( + initialized::<15>().fuse_local_position_velocity( + local_invalid, + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::InvalidObservation) + ); + let mut local_validity = local(); + local_validity.context.validity = Validity::Invalid; + assert_eq!( + initialized::<15>().fuse_local_position_velocity( + local_validity, + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::InvalidObservation) + ); + let mut local_frame = local(); + local_frame.context.frame = FrameId::BODY_FRD; + assert_eq!( + initialized::<15>().fuse_local_position_velocity( + local_frame, + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::FrameMismatch) + ); + assert_eq!( + initialized::<15>().fuse_local_position_velocity( + local(), + SourceId::new(6), + CalibrationRevision::new(1) + ), + Err(FusionError::SourceMismatch) + ); + assert_eq!( + initialized::<15>().fuse_local_position_velocity( + local(), + SourceId::new(5), + CalibrationRevision::new(2) + ), + Err(FusionError::CalibrationMismatch) + ); + let mut local_implausible = local(); + local_implausible.position = PositionNed::metres(20_000.0, 0.0, 0.0); + assert_eq!( + initialized::<15>().fuse_local_position_velocity( + local_implausible, + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::Implausible) + ); + let mut local_nan_position = local(); + local_nan_position.position = PositionNed::metres(f32::NAN, 0.0, 0.0); + assert_eq!( + initialized::<15>().fuse_local_position_velocity( + local_nan_position, + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::Implausible) + ); + let mut local_nan_velocity = local(); + local_nan_velocity.velocity = VelocityNed::metres_per_second(f32::NAN, 0.0, 0.0); + assert_eq!( + initialized::<15>().fuse_local_position_velocity( + local_nan_velocity, + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::Implausible) + ); + let mut local_position_y = local(); + local_position_y.position = PositionNed::metres(0.0, 20_000.0, 0.0); + assert_eq!( + initialized::<15>().fuse_local_position_velocity( + local_position_y, + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::Implausible) + ); + let mut local_position_z = local(); + local_position_z.position = PositionNed::metres(0.0, 0.0, 20_000.0); + assert_eq!( + initialized::<15>().fuse_local_position_velocity( + local_position_z, + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::Implausible) + ); + let mut local_velocity_x = local(); + local_velocity_x.velocity = VelocityNed::metres_per_second(200.0, 0.0, 0.0); + assert_eq!( + initialized::<15>().fuse_local_position_velocity( + local_velocity_x, + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::Implausible) + ); + let mut local_velocity_y = local(); + local_velocity_y.velocity = VelocityNed::metres_per_second(0.0, 200.0, 0.0); + assert_eq!( + initialized::<15>().fuse_local_position_velocity( + local_velocity_y, + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::Implausible) + ); + let mut local_velocity_z = local(); + local_velocity_z.velocity = VelocityNed::metres_per_second(0.0, 0.0, 200.0); + assert_eq!( + initialized::<15>().fuse_local_position_velocity( + local_velocity_z, + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::Implausible) + ); + } + + #[test] + fn propagation_faults_fail_closed() { + let mut no_receipt = SourceIntegrity::new(IntegrityConfig::default()); + assert!(no_receipt.reserve(SourceId::new(1))); + assert_eq!( + no_receipt.status(Some(SourceId::new(1)), Timestamp::from_microseconds(10)), + SourceStatus::Unknown + ); + let mut wrong_frame_imu = imu(11, [0, 0, -1_000], [0; 3]); + wrong_frame_imu.context.frame = FrameId::new(7); + assert_eq!( + initialized::<15>().propagate(wrong_frame_imu), + Err(PropagationError::InvalidImu) + ); + let mut nonfinite_acceleration = imu(11, [0, 0, -1_000], [0; 3]); + nonfinite_acceleration.acceleration = + AccelerationFrd::metres_per_second_squared(f32::NAN, 0.0, 0.0); + assert_eq!( + initialized::<15>().propagate(nonfinite_acceleration), + Err(PropagationError::InvalidImu) + ); + let mut nonfinite_rate = imu(11, [0, 0, -1_000], [0; 3]); + nonfinite_rate.angular_rate = AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); + assert_eq!( + initialized::<15>().propagate(nonfinite_rate), + Err(PropagationError::InvalidImu) + ); + } + + #[test] + fn range_faults_fail_closed() { + let range_extrinsics = range_extrinsics(); + let mut range_capacity = initialized::<16>(); + for source in 1..=4 { + assert!(range_capacity.integrity.reserve(SourceId::new(source))); + } + let mut over_capacity = range(); + over_capacity.context.source = SourceId::new(5); + let mut capacity_extrinsics = range_extrinsics; + capacity_extrinsics.source = SourceId::new(5); + assert_eq!( + range_capacity.fuse_range(over_capacity, capacity_extrinsics), + Err(FusionError::SourceCapacityExhausted) + ); + + let mut below_terrain = initialized::<16>(); + below_terrain.state.terrain_down_metres = 0.0; + assert_eq!( + below_terrain.fuse_range(range(), range_extrinsics), + Err(FusionError::InvalidRangeGeometry) + ); + let mut singular_range = initialized::<16>(); + singular_range + .covariance + .set(POSITION + 2, POSITION + 2, -0.1); + singular_range.covariance.set(TERRAIN, TERRAIN, 0.0); + assert_eq!( + singular_range.fuse_range(range(), range_extrinsics), + Err(FusionError::SingularInnovation) + ); + let mut infinite_range_nis = initialized::<16>(); + infinite_range_nis.config.maximum_range_metres = f32::MAX; + let mut enormous_range = range(); + enormous_range.metres = f32::MAX; + assert_eq!( + infinite_range_nis.fuse_range(enormous_range, range_extrinsics), + Err(FusionError::InnovationRejected) + ); + let mut nonfinite_prediction = initialized::<16>(); + nonfinite_prediction.state.terrain_down_metres = f32::MAX; + nonfinite_prediction.state.position = PositionNed::metres(0.0, 0.0, -f32::MAX); + assert_eq!( + nonfinite_prediction.fuse_range(range(), range_extrinsics), + Err(FusionError::SingularInnovation) + ); + let mut nonfinite_innovation = initialized::<16>(); + nonfinite_innovation.config.maximum_range_metres = f32::MAX; + nonfinite_innovation.state.terrain_down_metres = f32::MAX; + let mut matching_maximum = range(); + matching_maximum.metres = f32::MAX; + assert_eq!( + nonfinite_innovation.fuse_range(matching_maximum, range_extrinsics), + Ok(FusionResult::Fused) + ); + let mut invalid_range_candidate = initialized::<16>(); + invalid_range_candidate.state.gyro_bias = + AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); + assert_eq!( + invalid_range_candidate.fuse_range(range(), range_extrinsics), + Err(FusionError::FilterInvalid) + ); + let mut failed_range = initialized::<16>(); + failed_range.failed = true; + assert_eq!( + failed_range.fuse_range(range(), range_extrinsics), + Err(FusionError::FilterInvalid) + ); + } + + #[test] + fn flow_faults_fail_closed() { + let flow_extrinsics = flow_extrinsics(); + let mut flow_timestamp = flow(); + flow_timestamp.context.measurement_time = Timestamp::from_microseconds(11); + assert_eq!( + initialized::<15>().fuse_optical_flow(flow_timestamp, flow_extrinsics), + Err(FusionError::TimestampMismatch) + ); + let mut flow_capacity = initialized::<15>(); + for source in 1..=4 { + assert!(flow_capacity.integrity.reserve(SourceId::new(source))); + } + let mut flow_over_capacity = flow(); + flow_over_capacity.context.source = SourceId::new(5); + let mut capacity_extrinsics = flow_extrinsics; + capacity_extrinsics.source = SourceId::new(5); + assert_eq!( + flow_capacity.fuse_optical_flow(flow_over_capacity, capacity_extrinsics), + Err(FusionError::SourceCapacityExhausted) + ); + let mut flow_replay = initialized::<15>(); + assert_eq!( + flow_replay.fuse_optical_flow(flow(), flow_extrinsics), + Ok(FusionResult::Fused) + ); + assert_eq!( + flow_replay.fuse_optical_flow(flow(), flow_extrinsics), + Err(FusionError::ReplayedObservation) + ); + let mut flow_singular = initialized::<15>(); + flow_singular.covariance.set(VELOCITY, VELOCITY, -0.1); + flow_singular + .covariance + .set(VELOCITY + 1, VELOCITY + 1, -0.1); + assert_eq!( + flow_singular.fuse_optical_flow(flow(), flow_extrinsics), + Err(FusionError::SingularInnovation) + ); + let mut flow_gate = flow(); + flow_gate.velocity = kaptan_math::Vector2::new(24.0, 0.0); + assert_eq!( + initialized::<15>().fuse_optical_flow(flow_gate, flow_extrinsics), + Err(FusionError::InnovationRejected) + ); + let mut infinite_flow_nis = initialized::<15>(); + infinite_flow_nis + .config + .maximum_flow_speed_metres_per_second = f32::MAX; + let mut enormous_flow = flow(); + enormous_flow.velocity = kaptan_math::Vector2::new(f32::MAX, 0.0); + assert_eq!( + infinite_flow_nis.fuse_optical_flow(enormous_flow, flow_extrinsics), + Err(FusionError::InnovationRejected) + ); + let mut flow_isolated = initialized::<15>(); + assert!(flow_isolated.integrity.reserve(SourceId::new(3))); + flow_isolated + .integrity + .entry_mut(SourceId::new(3)) + .expect("test reserved source") + .isolated = true; + assert_eq!( + flow_isolated.fuse_optical_flow(flow(), flow_extrinsics), + Ok(FusionResult::SourceIsolated) + ); + let mut failed_flow = initialized::<15>(); + failed_flow.failed = true; + assert_eq!( + failed_flow.fuse_optical_flow(flow(), flow_extrinsics), + Err(FusionError::FilterInvalid) + ); + let mut invalid_flow_candidate = initialized::<15>(); + invalid_flow_candidate.state.gyro_bias = + AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); + assert_eq!( + invalid_flow_candidate.fuse_optical_flow(flow(), flow_extrinsics), + Err(FusionError::FilterInvalid) + ); + } + + #[test] + fn candidate_validation_and_nonfinite_extrinsics_fail_closed() { + let mut invalid_nominal = initialized::(); + invalid_nominal.state.gyro_bias = AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); + assert!(!invalid_nominal.values_are_valid()); + + // The candidate update must be committed atomically. These entries + // are unobserved by the range/flow Jacobians, so gain construction and + // the update itself succeed while final covariance validation rejects + // the private candidate. + let mut range_candidate = initialized::(); + range_candidate.covariance.set(0, 0, -1.0); + let range_state = range_candidate.state; + let range_covariance = range_candidate.covariance; + assert_eq!( + range_candidate.fuse_range(range(), range_extrinsics()), + Err(FusionError::FilterInvalid) + ); + assert_eq!(range_candidate.state, range_state); + assert_eq!(range_candidate.covariance.values, range_covariance.values); + assert!(range_candidate.requires_reinitialization()); + + let mut flow_candidate = initialized::(); + flow_candidate.covariance.set(0, 0, -1.0); + let flow_state = flow_candidate.state; + let flow_covariance = flow_candidate.covariance; + assert_eq!( + flow_candidate.fuse_optical_flow(flow(), flow_extrinsics()), + Err(FusionError::FilterInvalid) + ); + assert_eq!(flow_candidate.state, flow_state); + assert_eq!(flow_candidate.covariance.values, flow_covariance.values); + assert!(flow_candidate.requires_reinitialization()); + + let mut invalid_extrinsics = flow_extrinsics(); + invalid_extrinsics.position_body = crate::PositionFrd::metres(f32::NAN, 0.0, 0.0); + assert_eq!( + initialized::().fuse_optical_flow(flow(), invalid_extrinsics), + Err(FusionError::UnsupportedOpticalFlowLeverArm) + ); + } + + #[test] + fn stationary_initializer_internal_numeric_faults_fail_closed() { + let complete = || { + let mut initializer = StationaryInitializer::<1>::new(); + initializer.count = 1; + initializer.source = Some(SourceId::new(1)); + initializer.calibration = Some(CalibrationRevision::new(1)); + initializer.last_time = Some(Timestamp::from_microseconds(10)); + initializer.last_sequence = Some(ObservationSequence::new(1)); + initializer + }; + let config = EstimatorConfig::default(); + let integrity = IntegrityConfig::default(); + + for mutator in [ + |initializer: &mut StationaryInitializer<2>| { + initializer.gyro_sum = Vector3::new(f32::NAN, 0.0, 0.0) + }, + |initializer: &mut StationaryInitializer<2>| { + initializer.acceleration_sum = Vector3::new(f32::NAN, 0.0, 0.0) + }, + |initializer: &mut StationaryInitializer<2>| initializer.gyro_squared_sum = f32::NAN, + |initializer: &mut StationaryInitializer<2>| { + initializer.acceleration_squared_sum = f32::NAN + }, + ] { + let mut initializer = StationaryInitializer::<2>::new(); + mutator(&mut initializer); + assert!(!initializer.push(imu(10, [0, 0, -1_000], [0; 3]))); + assert!(!initializer.is_complete()); + } + + let mut nonfinite_gyro_square = complete(); + nonfinite_gyro_square.gyro_squared_sum = f32::NAN; + assert!( + nonfinite_gyro_square + .finish::(Timestamp::from_microseconds(10), config, integrity, 0.0) + .is_none() + ); + + let mut nonfinite_acceleration_square = complete(); + nonfinite_acceleration_square.acceleration_squared_sum = f32::NAN; + assert!( + nonfinite_acceleration_square + .finish::(Timestamp::from_microseconds(10), config, integrity, 0.0) + .is_none() + ); + + let mut nonfinite_residual = complete(); + nonfinite_residual.acceleration_sum = Vector3::new(0.0, 0.0, f32::MAX); + assert!( + nonfinite_residual + .finish::( + Timestamp::from_microseconds(10), + EstimatorConfig { + gravity_mps2: f32::MAX, + ..config + }, + integrity, + 0.0, + ) + .is_none() + ); + + let mut nonfinite_gyro_variance = complete(); + nonfinite_gyro_variance.gyro_sum = Vector3::new(f32::MAX, 0.0, 0.0); + assert!( + nonfinite_gyro_variance + .finish::(Timestamp::from_microseconds(10), config, integrity, 0.0) + .is_none() + ); + + let mut nonfinite_acceleration_variance = complete(); + nonfinite_acceleration_variance.acceleration_sum = Vector3::new(f32::MAX, 0.0, 0.0); + assert!( + nonfinite_acceleration_variance + .finish::(Timestamp::from_microseconds(10), config, integrity, 0.0) + .is_none() + ); + + let mut residual_limit = complete(); + residual_limit.acceleration_sum = Vector3::new(1.0, 0.0, -GRAVITY_MPS2); + assert!( + residual_limit + .finish::(Timestamp::from_microseconds(10), config, integrity, 0.0) + .is_none() + ); + + let mut gyro_variance_limit = complete(); + gyro_variance_limit.acceleration_sum = Vector3::new(0.0, 0.0, -GRAVITY_MPS2); + gyro_variance_limit.acceleration_squared_sum = GRAVITY_MPS2 * GRAVITY_MPS2; + gyro_variance_limit.gyro_squared_sum = 0.01; + assert!( + gyro_variance_limit + .finish::(Timestamp::from_microseconds(10), config, integrity, 0.0) + .is_none() + ); + + let mut acceleration_variance_limit = complete(); + acceleration_variance_limit.acceleration_sum = Vector3::new(0.0, 0.0, -GRAVITY_MPS2); + acceleration_variance_limit.acceleration_squared_sum = GRAVITY_MPS2 * GRAVITY_MPS2 + 1.0; + assert!( + acceleration_variance_limit + .finish::(Timestamp::from_microseconds(10), config, integrity, 0.0) + .is_none() + ); + } + + #[test] + fn range_numerical_boundary_faults_fail_closed() { + let mut invalid_lever = range_extrinsics(); + invalid_lever.position_body = crate::PositionFrd::metres(f32::NAN, 0.0, 0.0); + assert_eq!( + initialized::().fuse_range(range(), invalid_lever), + Err(FusionError::InvalidRangeGeometry) + ); + + let mut nonfinite_innovation = initialized::(); + nonfinite_innovation.config.minimum_range_metres = -f32::MAX; + nonfinite_innovation.config.maximum_range_metres = f32::MAX; + nonfinite_innovation.state.terrain_down_metres = f32::MAX; + let mut negative_maximum_range = range(); + negative_maximum_range.metres = -f32::MAX; + assert_eq!( + nonfinite_innovation.fuse_range(negative_maximum_range, range_extrinsics()), + Err(FusionError::SingularInnovation) + ); + + let mut nonfinite_jacobian = initialized::(); + let near_horizontal = + UnitQuaternion::::from_components(0.707_142_1, 0.0, 0.707_071_4, 0.0) + .unwrap(); + nonfinite_jacobian.state.attitude = nonfinite_jacobian + .state + .attitude + .compose(near_horizontal) + .unwrap(); + nonfinite_jacobian.state.terrain_down_metres = 1.0e34; + nonfinite_jacobian.config.minimum_range_down_cosine = 1.0e-5; + nonfinite_jacobian.config.maximum_range_metres = f32::MAX; + assert_eq!( + nonfinite_jacobian.fuse_range(range(), range_extrinsics()), + Err(FusionError::SingularInnovation) + ); + } + + #[test] + fn finite_flow_innovation_covariance_overflow_is_rejected() { + let mut filter = initialized::(); + filter.config.gyro_noise_variance = 1.0e31; + let mut high_acceleration = imu(11, [0, 0, -1_000], [0; 3]); + high_acceleration.acceleration = + AccelerationFrd::metres_per_second_squared(1.0e13, 0.0, -GRAVITY_MPS2); + assert!(filter.propagate(high_acceleration).is_ok()); + assert!(filter.covariance_is_valid()); + let (_, h) = filter.co_located_flow_model(flow_extrinsics()); + assert!(filter.two_gain(h, flow().covariance).is_none()); + + let before_state = filter.state; + let before_covariance = filter.covariance.values; + let mut observation = flow(); + observation.context = context(11, FrameId::new(8), 3, 255); + assert_eq!( + filter.fuse_optical_flow(observation, flow_extrinsics()), + Err(FusionError::SingularInnovation) + ); + assert_eq!(filter.state, before_state); + assert_eq!(filter.covariance.values, before_covariance); + } + + #[test] + fn local_faults_fail_closed() { + let mut local_timestamp = local(); + local_timestamp.context.measurement_time = Timestamp::from_microseconds(11); + assert_eq!( + initialized::<15>().fuse_local_position_velocity( + local_timestamp, + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::TimestampMismatch) + ); + let mut local_capacity = initialized::<15>(); + for source in 1..=4 { + assert!(local_capacity.integrity.reserve(SourceId::new(source))); + } + assert_eq!( + local_capacity.fuse_local_position_velocity( + local(), + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::SourceCapacityExhausted) + ); + let mut local_replay = initialized::<15>(); + assert_eq!( + local_replay.fuse_local_position_velocity( + local(), + SourceId::new(5), + CalibrationRevision::new(1) + ), + Ok(FusionResult::Fused) + ); + assert_eq!( + local_replay.fuse_local_position_velocity( + local(), + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::ReplayedObservation) + ); + let mut local_singular = initialized::<15>(); + local_singular.covariance.set(POSITION, POSITION, -0.1); + assert_eq!( + local_singular.fuse_local_position_velocity( + local(), + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::SingularInnovation) + ); + let mut infinite_local_nis = initialized::<15>(); + infinite_local_nis.config.maximum_local_position_metres = f32::MAX; + let mut enormous_local = local(); + enormous_local.position = PositionNed::metres(f32::MAX, 0.0, 0.0); + assert_eq!( + infinite_local_nis.fuse_local_position_velocity( + enormous_local, + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::InnovationRejected) + ); + let mut local_isolated = initialized::<15>(); + assert!(local_isolated.integrity.reserve(SourceId::new(5))); + local_isolated + .integrity + .entry_mut(SourceId::new(5)) + .expect("test reserved source") + .isolated = true; + assert_eq!( + local_isolated.fuse_local_position_velocity( + local(), + SourceId::new(5), + CalibrationRevision::new(1) + ), + Ok(FusionResult::SourceIsolated) + ); + let mut failed_local = initialized::<15>(); + failed_local.failed = true; + assert_eq!( + failed_local.fuse_local_position_velocity( + local(), + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::FilterInvalid) + ); + let mut invalid_local_candidate = initialized::<15>(); + invalid_local_candidate.state.gyro_bias = + AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); + assert_eq!( + invalid_local_candidate.fuse_local_position_velocity( + local(), + SourceId::new(5), + CalibrationRevision::new(1) + ), + Err(FusionError::FilterInvalid) + ); + } + + #[test] + fn numeric_helpers_fail_closed() { + let mut asymmetric = initialized::<15>(); + asymmetric.covariance.set(0, 1, 0.5); + assert!(!asymmetric.covariance_is_valid()); + let mut overflow_propagation = initialized::<15>(); + overflow_propagation + .config + .maximum_imu_interval_microseconds = u64::MAX; + overflow_propagation.state.velocity = VelocityNed::metres_per_second(f32::MAX, 0.0, 0.0); + let mut overflow_imu = imu(u64::MAX - 1, [0, 0, -1_000], [0; 3]); + overflow_imu.context.sequence = ObservationSequence::new(11); + assert_eq!( + overflow_propagation.propagate(overflow_imu), + Err(PropagationError::FilterInvalid) + ); + let mut nonfinite_rotation = initialized::<15>(); + nonfinite_rotation.config.maximum_imu_interval_microseconds = u64::MAX; + let mut extreme_rate = imu(u64::MAX - 1, [0, 0, -1_000], [0; 3]); + extreme_rate.context.sequence = ObservationSequence::new(11); + extreme_rate.angular_rate = AngularRateFrd::radians_per_second(f32::MAX, 0.0, 0.0); + assert_eq!( + nonfinite_rotation.propagate(extreme_rate), + Err(PropagationError::NonFinite) + ); + let mut nonfinite_acceleration_delta = initialized::<15>(); + nonfinite_acceleration_delta.config.gravity_mps2 = f32::MAX; + let mut large_acceleration = imu(11, [0, 0, -1_000], [0; 3]); + large_acceleration.acceleration = + AccelerationFrd::metres_per_second_squared(f32::MAX, 0.0, f32::MAX); + assert!( + nonfinite_acceleration_delta + .propagate_delta(large_acceleration, 0.001) + .is_err() + ); + let mut nonfinite_velocity_delta = initialized::<15>(); + nonfinite_velocity_delta.state.position = PositionNed::metres(-f32::MAX, 0.0, 0.0); + let mut velocity_overflow_imu = imu(11, [0, 0, -1_000], [0; 3]); + velocity_overflow_imu.acceleration = + AccelerationFrd::metres_per_second_squared(f32::MAX, 0.0, -GRAVITY_MPS2); + assert!( + nonfinite_velocity_delta + .propagate_delta(velocity_overflow_imu, 1.1) + .is_err() + ); + + let mut gain_filter = initialized::<15>(); + let scalar = ScalarGain { + h: [0.0; MAX_DIM], + gain: [0.0; MAX_DIM], + innovation_variance: 1.0, + }; + assert!( + gain_filter + .scalar_gain([0.0; MAX_DIM], ScalarVariance::new(0.1).unwrap()) + .is_some() + ); + gain_filter.covariance.set(0, 0, -0.1); + let mut scalar_h = [0.0; MAX_DIM]; + scalar_h[0] = 1.0; + assert!( + gain_filter + .scalar_gain(scalar_h, ScalarVariance::new(0.1).unwrap()) + .is_none() + ); + let mut nonfinite_gain_filter = initialized::<15>(); + nonfinite_gain_filter.covariance.set(0, 0, f32::NAN); + assert!( + nonfinite_gain_filter + .scalar_gain(scalar_h, ScalarVariance::new(0.1).unwrap()) + .is_none() + ); + assert!(gain_filter.apply_scalar(scalar, f32::NAN, 0.1).is_err()); + assert!(gain_filter.apply_scalar(scalar, 0.0, f32::NAN).is_err()); + assert!(gain_filter.apply_scalar(scalar, 0.0, 0.0).is_err()); + let overflowing_scalar = ScalarGain { + h: [0.0; MAX_DIM], + gain: [f32::MAX; MAX_DIM], + innovation_variance: 1.0, + }; + assert!( + gain_filter + .apply_scalar(overflowing_scalar, f32::MAX, 0.1) + .is_err() + ); + let two = TwoGain { + h: [[0.0; MAX_DIM]; 2], + gain: [[0.0; 2]; MAX_DIM], + inverse_s: [1.0, 0.0, 0.0, 1.0], + }; + assert!( + gain_filter + .apply_two(two, [f32::NAN, 0.0], flow().covariance) + .is_err() + ); + let overflowing_two = TwoGain { + h: [[0.0; MAX_DIM]; 2], + gain: [[f32::MAX; 2]; MAX_DIM], + inverse_s: [1.0, 0.0, 0.0, 1.0], + }; + assert!( + gain_filter + .apply_two(overflowing_two, [f32::MAX, f32::MAX], flow().covariance) + .is_err() + ); + let mut bad_two_gain = initialized::<15>(); + let mut two_h = [[0.0; MAX_DIM]; 2]; + two_h[0][VELOCITY] = 1.0; + two_h[1][VELOCITY + 1] = 1.0; + assert!( + bad_two_gain + .two_gain( + two_h, + Covariance2 { + xx: ScalarVariance::new(0.1).unwrap(), + xy: f32::NAN, + yy: ScalarVariance::new(0.1).unwrap() + } + ) + .is_none() + ); + bad_two_gain.covariance.set(VELOCITY, VELOCITY, f32::MAX); + assert!(bad_two_gain.two_gain(two_h, flow().covariance).is_none()); + + let mut nonfinite_s00 = initialized::<15>(); + nonfinite_s00.covariance.set(VELOCITY, VELOCITY, f32::NAN); + assert!(nonfinite_s00.two_gain(two_h, flow().covariance).is_none()); + + let mut nonfinite_s11 = initialized::<15>(); + nonfinite_s11 + .covariance + .set(VELOCITY + 1, VELOCITY + 1, f32::NAN); + assert!(nonfinite_s11.two_gain(two_h, flow().covariance).is_none()); + + let mut nonpositive_s11 = initialized::<15>(); + nonpositive_s11 + .covariance + .set(VELOCITY + 1, VELOCITY + 1, -flow().covariance.yy.get()); + assert!(nonpositive_s11.two_gain(two_h, flow().covariance).is_none()); + + let mut singular_innovation_covariance = initialized::<15>(); + singular_innovation_covariance + .covariance + .set(VELOCITY, VELOCITY, 0.0); + singular_innovation_covariance + .covariance + .set(VELOCITY + 1, VELOCITY + 1, 0.0); + assert!( + singular_innovation_covariance + .two_gain( + two_h, + Covariance2 { + xx: ScalarVariance::new(0.1).unwrap(), + xy: 0.1, + yy: ScalarVariance::new(0.1).unwrap(), + }, + ) + .is_none() + ); + let mut error = [0.0; MAX_DIM]; + error[0] = f32::NAN; + assert!(gain_filter.inject(error).is_err()); + let mut position_overflow = initialized::<15>(); + position_overflow.state.position = PositionNed::metres(f32::MAX, 0.0, 0.0); + let mut error = [0.0; MAX_DIM]; + error[0] = f32::MAX; + assert!(position_overflow.inject(error).is_err()); + let mut velocity_overflow = initialized::<15>(); + velocity_overflow.state.velocity = VelocityNed::metres_per_second(f32::MAX, 0.0, 0.0); + let mut error = [0.0; MAX_DIM]; + error[3] = f32::MAX; + assert!(velocity_overflow.inject(error).is_err()); + let mut gyro_overflow = initialized::<15>(); + gyro_overflow.state.gyro_bias = AngularRateFrd::radians_per_second(f32::MAX, 0.0, 0.0); + let mut error = [0.0; MAX_DIM]; + error[9] = f32::MAX; + assert!(gyro_overflow.inject(error).is_err()); + let mut acceleration_overflow = initialized::<15>(); + acceleration_overflow.state.acceleration_bias = + AccelerationFrd::metres_per_second_squared(f32::MAX, 0.0, 0.0); + let mut error = [0.0; MAX_DIM]; + error[12] = f32::MAX; + assert!(acceleration_overflow.inject(error).is_err()); + let mut terrain_overflow = initialized::<16>(); + terrain_overflow.state.terrain_down_metres = f32::MAX; + let mut error = [0.0; MAX_DIM]; + error[TERRAIN] = f32::MAX; + assert!(terrain_overflow.inject(error).is_err()); + } + + #[test] + fn defensive_estimator_paths_are_explicitly_fail_closed() { + let source = SourceId::new(9); + let mut integrity = SourceIntegrity::new(IntegrityConfig::default()); + assert!(!integrity.accepts_sequence(source, ObservationSequence::new(1))); + assert!(!integrity.accept(source, Timestamp::from_microseconds(1))); + + let mut initializer = StationaryInitializer::<1>::new(); + let mut overflowing_sample = imu(1, [0, 0, -1_000], [0; 3]); + overflowing_sample.acceleration = + AccelerationFrd::metres_per_second_squared(f32::MAX, 0.0, 0.0); + assert!(!initializer.push(overflowing_sample)); + assert!(!initializer.is_complete()); + + // A finite covariance can still overflow while constructing its + // Cholesky factor. It must be rejected rather than repaired. + let mut covariance = Covariance::diagonal(); + covariance.set(0, 0, 1.0); + covariance.set(1, 0, f32::MAX); + covariance.set(0, 1, f32::MAX); + covariance.set(1, 1, 1.0); + assert!(!covariance.positive_definite(2)); + + let mut terrain = initialized::(); + terrain.covariance.set(TERRAIN, TERRAIN, 1.0); + let before = terrain.covariance.get(TERRAIN, TERRAIN); + terrain + .propagate(imu(10_010, [0, 0, -1_000], [0; 3])) + .unwrap(); + assert!(terrain.covariance.get(TERRAIN, TERRAIN) > before); + + // A candidate whose unobserved terrain covariance is invalid must not + // be committed by an otherwise valid six-axis local-state update. + let mut invalid_candidate = initialized::(); + invalid_candidate.covariance.set(TERRAIN, TERRAIN, -1.0); + assert_eq!( + invalid_candidate.fuse_local_position_velocity( + local(), + SourceId::new(5), + CalibrationRevision::new(1), + ), + Err(FusionError::FilterInvalid) + ); + assert!(invalid_candidate.requires_reinitialization()); + } + + #[test] + fn optical_flow_numerical_model_guards_reject_before_fusion() { + let mut nonfinite_prediction = initialized::(); + let yaw_delta = + UnitQuaternion::::from_small_rotation_vector(Vector3::new(0.0, 0.0, 0.785)) + .unwrap(); + nonfinite_prediction.state.attitude = nonfinite_prediction + .state + .attitude + .compose(yaw_delta) + .unwrap(); + nonfinite_prediction.state.velocity = + VelocityNed::metres_per_second(f32::MAX, f32::MAX, 0.0); + nonfinite_prediction + .config + .maximum_flow_speed_metres_per_second = f32::MAX; + assert_eq!( + nonfinite_prediction.fuse_optical_flow(flow(), flow_extrinsics()), + Err(FusionError::Implausible) + ); + + // The measurement only observes the sensor x/y axes. This finite + // velocity projects into the unobserved sensor z axis without an + // error, while its attitude Jacobian overflows a measured component. + let mut nonfinite_jacobian = initialized::(); + nonfinite_jacobian.state.velocity = + VelocityNed::metres_per_second(f32::MAX, 0.0, -f32::MAX); + nonfinite_jacobian + .config + .maximum_flow_speed_metres_per_second = f32::MAX; + let mut rotated_extrinsics = flow_extrinsics(); + rotated_extrinsics.sensor_to_body = + UnitQuaternion::from_components(1.0, 0.0, -0.5, 0.0).unwrap(); + assert_eq!( + nonfinite_jacobian.fuse_optical_flow(flow(), rotated_extrinsics), + Err(FusionError::SingularInnovation) + ); + } + + #[test] + fn initializer_and_constructor_rejection_matrix_is_atomic() { + let state = initialized::().state; + assert_eq!(state.terrain_down_metres::(), None); + assert_eq!(state.terrain_down_metres::(), Some(2.0)); + assert!(state_is_valid::(state)); + assert!(state_is_valid::(state)); + + let mut nonfinite_position = state; + nonfinite_position.position = PositionNed::metres(f32::NAN, 0.0, 0.0); + assert!(!state_is_valid::(nonfinite_position)); + let mut nonfinite_velocity = state; + nonfinite_velocity.velocity = VelocityNed::metres_per_second(f32::NAN, 0.0, 0.0); + assert!(!state_is_valid::(nonfinite_velocity)); + let mut nonfinite_gyro = state; + nonfinite_gyro.gyro_bias = AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); + assert!(!state_is_valid::(nonfinite_gyro)); + let mut nonfinite_acceleration = state; + nonfinite_acceleration.acceleration_bias = + AccelerationFrd::metres_per_second_squared(f32::NAN, 0.0, 0.0); + assert!(!state_is_valid::(nonfinite_acceleration)); + let mut nonfinite_terrain = state; + nonfinite_terrain.terrain_down_metres = f32::NAN; + assert!(!state_is_valid::(nonfinite_terrain)); + + let mut full = StationaryInitializer::<1>::new(); + assert!(full.push(imu(10, [0, 0, -1_000], [0; 3]))); + assert!(!full.push(imu(11, [0, 0, -1_000], [0; 3]))); + + for malformed in [ + { + let mut value = imu(10, [0, 0, -1_000], [0; 3]); + value.context.validity = Validity::Invalid; + value + }, + { + let mut value = imu(10, [0, 0, -1_000], [0; 3]); + value.context.frame = FrameId::new(9); + value + }, + { + let mut value = imu(10, [0, 0, -1_000], [0; 3]); + value.acceleration = AccelerationFrd::metres_per_second_squared(f32::NAN, 0.0, 0.0); + value + }, + { + let mut value = imu(10, [0, 0, -1_000], [0; 3]); + value.angular_rate = AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); + value + }, + ] { + let mut accumulator = StationaryInitializer::<1>::new(); + assert!(!accumulator.push(malformed)); + assert!(!accumulator.is_complete()); + } + + for mismatched in [ + { + let mut value = imu(11, [0, 0, -1_000], [0; 3]); + value.context.source = SourceId::new(9); + value + }, + { + let mut value = imu(11, [0, 0, -1_000], [0; 3]); + value.context.calibration = CalibrationRevision::new(9); + value + }, + imu(10, [0, 0, -1_000], [0; 3]), + { + let mut value = imu(11, [0, 0, -1_000], [0; 3]); + value.context.sequence = ObservationSequence::new(10); + value + }, + ] { + let mut accumulator = StationaryInitializer::<2>::new(); + assert!(accumulator.push(imu(10, [0, 0, -1_000], [0; 3]))); + assert!(!accumulator.push(mismatched)); + assert!(!accumulator.is_complete()); + } + + let mut numeric = StationaryInitializer::<2>::new(); + assert!(numeric.push(imu(10, [0, 0, -1_000], [0; 3]))); + numeric.gyro_squared_sum = f32::MAX; + let mut huge_rate = imu(11, [0, 0, -1_000], [0; 3]); + huge_rate.angular_rate = AngularRateFrd::radians_per_second(f32::MAX, 0.0, 0.0); + assert!(!numeric.push(huge_rate)); + assert!(!numeric.is_complete()); + + let complete = || { + let mut accumulator = StationaryInitializer::<1>::new(); + assert!(accumulator.push(imu(10, [0, 0, -1_000], [0; 3]))); + accumulator + }; + assert!( + StationaryInitializer::<0>::new() + .finish::( + Timestamp::from_microseconds(10), + EstimatorConfig::default(), + IntegrityConfig::default(), + 2.0, + ) + .is_none() + ); + assert!( + complete() + .finish::( + Timestamp::from_microseconds(11), + EstimatorConfig::default(), + IntegrityConfig::default(), + 2.0, + ) + .is_none() + ); + assert!( + complete() + .finish::( + Timestamp::from_microseconds(10), + EstimatorConfig { + gravity_mps2: 0.0, + ..EstimatorConfig::default() + }, + IntegrityConfig::default(), + 2.0, + ) + .is_none() + ); + assert!( + complete() + .finish::( + Timestamp::from_microseconds(10), + EstimatorConfig::default(), + IntegrityConfig::default(), + f32::NAN, + ) + .is_none() + ); + + let stream = imu_stream(); + assert!( + Eskf::<14>::new( + Timestamp::from_microseconds(10), + state, + EstimatorConfig::default(), + IntegrityConfig::default(), + stream, + ) + .is_err() + ); + assert!( + Eskf::::new( + Timestamp::from_microseconds(10), + nonfinite_position, + EstimatorConfig::default(), + IntegrityConfig::default(), + stream, + ) + .is_err() + ); + assert!( + Eskf::::new( + Timestamp::from_microseconds(10), + state, + EstimatorConfig::default(), + IntegrityConfig { + rejection_limit: 0, + ..IntegrityConfig::default() + }, + stream, + ) + .is_err() + ); + } +} diff --git a/libs/kaptan-navigation/src/lib.rs b/libs/kaptan-navigation/src/lib.rs new file mode 100644 index 0000000..98e8e82 --- /dev/null +++ b/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; diff --git a/libs/kaptan-navigation/src/observation.rs b/libs/kaptan-navigation/src/observation.rs new file mode 100644 index 0000000..69a3c77 --- /dev/null +++ b/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 { + 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 { + 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); +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 { + self.0 + } +} + +/// NED velocity in metres per second. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct VelocityNed(Vector3); +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 { + 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); +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 { + self.0 + } +} + +/// Body FRD specific force in metres per second squared. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct AccelerationFrd(Vector3); +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 { + self.0 + } +} + +/// Body FRD angular rate in radians per second. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct AngularRateFrd(Vector3); +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 { + 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, + /// 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 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) -> Vector3 { + 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) + ); + } +}