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

//! Fixed-size multiplicative error-state Kalman filter.

use kaptan_err_propagate::KaptanError;
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, KaptanError)]
pub enum PropagationError {
    /// An IMU observation was invalid or was not calibrated into FRD.
    #[kaptan_error(id = 8001)]
    InvalidImu,
    /// IMU source differs from the stream bound during stationary initialization.
    #[kaptan_error(id = 8002)]
    ImuSourceMismatch,
    /// IMU calibration differs from the stream bound during stationary initialization.
    #[kaptan_error(id = 8003)]
    ImuCalibrationMismatch,
    /// IMU sequence repeated or moved backwards within the bound stream.
    #[kaptan_error(id = 8004)]
    ReplayedImu,
    /// IMU measurement time did not advance strictly.
    #[kaptan_error(id = 8005)]
    NonMonotonicTimestamp,
    /// The interval exceeded the bounded propagation contract.
    #[kaptan_error(id = 8006)]
    ImuGapTooLarge,
    /// The IMU rate and interval exceed the bounded quaternion increment.
    #[kaptan_error(id = 8007)]
    RotationIncrementTooLarge,
    /// Input or resulting state was non-finite.
    #[kaptan_error(id = 8008)]
    NonFinite,
    /// The candidate state or covariance failed validation; reinitialize before reuse.
    #[kaptan_error(id = 8009)]
    FilterInvalid,
}

/// Aiding fusion refusal with no nominal-state update.
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
pub enum FusionError {
    /// Observation is not at this filter's current history time.
    #[kaptan_error(id = 8101)]
    TimestampMismatch,
    /// Observation was device-invalid.
    #[kaptan_error(id = 8102)]
    InvalidObservation,
    /// Observation source does not identify the configured physical device.
    #[kaptan_error(id = 8103)]
    SourceMismatch,
    /// Observation frame does not match the configured extrinsics.
    #[kaptan_error(id = 8104)]
    FrameMismatch,
    /// Observation calibration revision does not match the configured transform.
    #[kaptan_error(id = 8105)]
    CalibrationMismatch,
    /// The fixed source-integrity table has no entry for this source.
    #[kaptan_error(id = 8106)]
    SourceCapacityExhausted,
    /// A source repeated or moved backwards from its accepted sequence stream.
    #[kaptan_error(id = 8107)]
    ReplayedObservation,
    /// This estimator profile has no terrain state.
    #[kaptan_error(id = 8108)]
    TerrainProfileRequired,
    /// Optical flow from an offset sensor needs a correlated IMU-noise update
    /// that this profile does not implement.
    #[kaptan_error(id = 8109)]
    UnsupportedOpticalFlowLeverArm,
    /// Measurement fails physical range or velocity plausibility bounds.
    #[kaptan_error(id = 8110)]
    Implausible,
    /// Range ray is too tilted or does not intersect terrain below the sensor.
    #[kaptan_error(id = 8111)]
    InvalidRangeGeometry,
    /// Innovation covariance was singular/non-finite.
    #[kaptan_error(id = 8112)]
    SingularInnovation,
    /// Innovation exceeds the configured chi-square gate.
    #[kaptan_error(id = 8113)]
    InnovationRejected,
    /// The candidate state or covariance failed validation; reinitialize before reuse.
    #[kaptan_error(id = 8114)]
    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<Timestamp>,
    last_sequence: Option<ObservationSequence>,
    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<SourceId>, 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<Frd, Ned>,
    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<Frd, Ned> {
        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<const N: usize>(self) -> Option<f32> {
        if N == TERRAIN_DIM {
            Some(self.terrain_down_metres)
        } else {
            None
        }
    }
}

fn state_is_valid<const N: usize>(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<const SAMPLES: usize> {
    count: usize,
    gyro_sum: Vector3<Frd>,
    acceleration_sum: Vector3<Frd>,
    gyro_squared_sum: f32,
    acceleration_squared_sum: f32,
    source: Option<SourceId>,
    calibration: Option<CalibrationRevision>,
    last_time: Option<Timestamp>,
    last_sequence: Option<ObservationSequence>,
}

#[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<const SAMPLES: usize> StationaryInitializer<SAMPLES> {
    /// 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<const N: usize>(
        self,
        time: Timestamp,
        config: EstimatorConfig,
        integrity: IntegrityConfig,
        terrain_down_metres: f32,
    ) -> Option<Eskf<N>> {
        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<const SAMPLES: usize> Default for StationaryInitializer<SAMPLES> {
    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<const N: usize> {
    time: Timestamp,
    state: NominalState,
    covariance: Covariance,
    config: EstimatorConfig,
    integrity: SourceIntegrity,
    imu_stream: ImuStream,
    last_range_source: Option<SourceId>,
    last_flow_source: Option<SourceId>,
    last_local_state_source: Option<SourceId>,
    failed: bool,
}

/// Inertial profile: position, velocity, attitude, gyro bias, accelerometer bias.
pub type InertialEskf = Eskf<INERTIAL_DIM>;
/// Local-terrain profile: inertial state plus terrain down position.
pub type LocalTerrainEskf = Eskf<TERRAIN_DIM>;

impl<const N: usize> Eskf<N> {
    fn new(
        time: Timestamp,
        state: NominalState,
        config: EstimatorConfig,
        integrity: IntegrityConfig,
        imu_stream: ImuStream,
    ) -> Result<Self, ()> {
        if (N != INERTIAL_DIM && N != TERRAIN_DIM)
            || !config.is_valid()
            || !integrity.is_valid()
            || !state_is_valid::<N>(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::<N>(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<Ned>, 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<Sensor> =
            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::<Ned>::new(1.0, 0.0, 0.0),
                1 => Vector3::<Ned>::new(0.0, 1.0, 0.0),
                _ => Vector3::<Ned>::new(0.0, 0.0, 1.0),
            };
            let body = self.state.attitude.ned_to_body(basis);
            let sensor: Vector3<Sensor> = 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<Sensor> =
                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<FusionResult, FusionError> {
        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::<Sensor>::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<FusionResult, FusionError> {
        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<FusionResult, FusionError> {
        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<ScalarGain> {
        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<TwoGain> {
        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<Vector3<Ned>, 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::<Ned>::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<Ned>) {
        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<Frd, Ned>) -> [[f32; 3]; 3] {
    let x = attitude.body_to_ned(Vector3::<Frd>::new(1.0, 0.0, 0.0));
    let y = attitude.body_to_ned(Vector3::<Frd>::new(0.0, 1.0, 0.0));
    let z = attitude.body_to_ned(Vector3::<Frd>::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<Ned>) -> 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<const N: usize>() -> Eskf<N> {
        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::<Ned>::new(1.0, 0.0, 0.0),
                1 => Vector3::<Ned>::new(0.0, 1.0, 0.0),
                _ => Vector3::<Ned>::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::<INERTIAL_DIM>();
        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::<TERRAIN_DIM>();
        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::<INERTIAL_DIM>();
        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::<INERTIAL_DIM>().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::<INERTIAL_DIM>(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::<INERTIAL_DIM>(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::<INERTIAL_DIM>(
                    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::<INERTIAL_DIM>(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::<INERTIAL_DIM>(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::<INERTIAL_DIM>(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::<INERTIAL_DIM>(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::<INERTIAL_DIM>(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::<TERRAIN_DIM>().fuse_range(range(), invalid_lever),
            Err(FusionError::InvalidRangeGeometry)
        );

        let mut nonfinite_innovation = initialized::<TERRAIN_DIM>();
        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::<TERRAIN_DIM>();
        let near_horizontal =
            UnitQuaternion::<Frd, Frd>::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::<INERTIAL_DIM>();
        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_DIM>();
        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::<TERRAIN_DIM>();
        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::<INERTIAL_DIM>();
        let yaw_delta =
            UnitQuaternion::<Frd, Frd>::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::<INERTIAL_DIM>();
        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::<INERTIAL_DIM>().state;
        assert_eq!(state.terrain_down_metres::<INERTIAL_DIM>(), None);
        assert_eq!(state.terrain_down_metres::<TERRAIN_DIM>(), Some(2.0));
        assert!(state_is_valid::<INERTIAL_DIM>(state));
        assert!(state_is_valid::<TERRAIN_DIM>(state));

        let mut nonfinite_position = state;
        nonfinite_position.position = PositionNed::metres(f32::NAN, 0.0, 0.0);
        assert!(!state_is_valid::<INERTIAL_DIM>(nonfinite_position));
        let mut nonfinite_velocity = state;
        nonfinite_velocity.velocity = VelocityNed::metres_per_second(f32::NAN, 0.0, 0.0);
        assert!(!state_is_valid::<INERTIAL_DIM>(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::<INERTIAL_DIM>(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::<INERTIAL_DIM>(nonfinite_acceleration));
        let mut nonfinite_terrain = state;
        nonfinite_terrain.terrain_down_metres = f32::NAN;
        assert!(!state_is_valid::<TERRAIN_DIM>(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::<INERTIAL_DIM>(
                    Timestamp::from_microseconds(10),
                    EstimatorConfig::default(),
                    IntegrityConfig::default(),
                    2.0,
                )
                .is_none()
        );
        assert!(
            complete()
                .finish::<INERTIAL_DIM>(
                    Timestamp::from_microseconds(11),
                    EstimatorConfig::default(),
                    IntegrityConfig::default(),
                    2.0,
                )
                .is_none()
        );
        assert!(
            complete()
                .finish::<INERTIAL_DIM>(
                    Timestamp::from_microseconds(10),
                    EstimatorConfig {
                        gravity_mps2: 0.0,
                        ..EstimatorConfig::default()
                    },
                    IntegrityConfig::default(),
                    2.0,
                )
                .is_none()
        );
        assert!(
            complete()
                .finish::<INERTIAL_DIM>(
                    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::<INERTIAL_DIM>::new(
                Timestamp::from_microseconds(10),
                nonfinite_position,
                EstimatorConfig::default(),
                IntegrityConfig::default(),
                stream,
            )
            .is_err()
        );
        assert!(
            Eskf::<INERTIAL_DIM>::new(
                Timestamp::from_microseconds(10),
                state,
                EstimatorConfig::default(),
                IntegrityConfig {
                    rejection_limit: 0,
                    ..IntegrityConfig::default()
                },
                stream,
            )
            .is_err()
        );
    }
}