sidereon-core 0.16.1

//! Loosely coupled GNSS PVT updates for the INS error-state filter.
//!
//! Measurement epochs are required to match the propagated INS epoch exactly.
//! A later time-synchronization layer can lift that requirement without
//! changing the measurement model here.

use crate::astro::constants::earth::OMEGA_E_DOT_RAD_S;
use crate::astro::math::mat3::{inline_rxr, inline_tr, mul_vec3, Mat3};
use crate::astro::math::vec3::{add3, cross3, scale3, sub3};
use crate::inertial::state::skew;
use crate::inertial::{
    mechanize_ecef, validate_finite, validate_vec3, ImuCalibration, ImuErrorModel, ImuSample,
    ImuSpec, MechanizationConfig,
};

use super::ekf::{ekf_correct_closed_loop, EkfCorrection, EkfCorrectionReport, EkfUpdateOptions};
use super::error_state::{
    linearize_error_state_ecef, predict_error_state_covariance, ErrorStateImuKinematics,
};
use super::state::{
    invalid_input, validate_covariance_matrix, FusionError, InsFilterState, ERROR_ATTITUDE_INDEX,
    ERROR_GYRO_BIAS_INDEX, ERROR_POSITION_INDEX, ERROR_VELOCITY_INDEX,
};
use super::tight::{TightCouplingConfig, TightFusionState};
use super::timesync::TimeSyncHistory;

const LOOSE_MIN_SATELLITES: usize = 4;
const POSITION_ROWS: usize = 3;
const POSITION_VELOCITY_ROWS: usize = 6;

/// GNSS PVT measurement used by the loose-coupled INS update.
///
/// The covariance matrix is ordered as `[position_x, position_y, position_z]`
/// for a position-only fix and as `[position_x, position_y, position_z,
/// velocity_x, velocity_y, velocity_z]` when velocity is present.
#[derive(Debug, Clone, PartialEq)]
pub struct GnssFixMeasurement {
    /// Measurement epoch in seconds since J2000 on the caller's GNSS time scale.
    pub t_j2000_s: f64,
    /// GNSS antenna position in ECEF meters.
    pub position_ecef_m: [f64; 3],
    /// Optional GNSS antenna velocity in ECEF meters per second.
    pub velocity_ecef_mps: Option<[f64; 3]>,
    /// Measurement covariance in the order documented on this type.
    pub covariance: Vec<Vec<f64>>,
    /// Number of satellites used by the upstream GNSS fix.
    pub satellites_used: usize,
    /// Whether the upstream GNSS solver reported a successful fix.
    pub solution_valid: bool,
}

impl GnssFixMeasurement {
    /// Build a position-only GNSS fix measurement.
    pub fn position(
        t_j2000_s: f64,
        position_ecef_m: [f64; 3],
        position_covariance_m2: [[f64; 3]; 3],
        satellites_used: usize,
    ) -> Result<Self, FusionError> {
        let measurement = Self {
            t_j2000_s,
            position_ecef_m,
            velocity_ecef_mps: None,
            covariance: mat3_to_rows(position_covariance_m2),
            satellites_used,
            solution_valid: true,
        };
        measurement.validate()?;
        Ok(measurement)
    }

    /// Build a position and velocity GNSS fix measurement.
    pub fn position_velocity(
        t_j2000_s: f64,
        position_ecef_m: [f64; 3],
        velocity_ecef_mps: [f64; 3],
        covariance: Vec<Vec<f64>>,
        satellites_used: usize,
    ) -> Result<Self, FusionError> {
        let measurement = Self {
            t_j2000_s,
            position_ecef_m,
            velocity_ecef_mps: Some(velocity_ecef_mps),
            covariance,
            satellites_used,
            solution_valid: true,
        };
        measurement.validate()?;
        Ok(measurement)
    }

    /// Validate finite values, solver status, satellite count, and covariance.
    pub fn validate(&self) -> Result<(), FusionError> {
        validate_finite(self.t_j2000_s, "t_j2000_s").map_err(FusionError::from)?;
        validate_vec3(self.position_ecef_m, "position_ecef_m").map_err(FusionError::from)?;
        if let Some(velocity) = self.velocity_ecef_mps {
            validate_vec3(velocity, "velocity_ecef_mps").map_err(FusionError::from)?;
        }
        if !self.solution_valid {
            return Err(invalid_input(
                "solution_valid",
                "GNSS fix must be successful",
            ));
        }
        if self.satellites_used < LOOSE_MIN_SATELLITES {
            return Err(invalid_input(
                "satellites_used",
                "at least 4 satellites required",
            ));
        }
        validate_covariance_matrix(&self.covariance, self.row_count(), "gnss_covariance")
    }

    /// Return the number of measurement rows implied by this fix.
    pub fn row_count(&self) -> usize {
        if self.velocity_ecef_mps.is_some() {
            POSITION_VELOCITY_ROWS
        } else {
            POSITION_ROWS
        }
    }
}

/// Configuration for loose-coupled GNSS updates.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct LooseCouplingConfig {
    /// Body-frame vector from IMU origin to GNSS antenna phase center, in meters.
    pub lever_arm_body_m: [f64; 3],
    /// Generic EKF correction options applied to each loose update.
    pub update_options: EkfUpdateOptions,
}

impl Default for LooseCouplingConfig {
    fn default() -> Self {
        Self {
            lever_arm_body_m: [0.0; 3],
            update_options: EkfUpdateOptions::default(),
        }
    }
}

impl LooseCouplingConfig {
    /// Validate finite lever-arm entries and nested update options.
    pub fn validate(&self) -> Result<(), FusionError> {
        validate_vec3(self.lever_arm_body_m, "lever_arm_body_m").map_err(FusionError::from)?;
        if let Some(gate) = self.update_options.innovation_gate {
            gate.validate()?;
        }
        Ok(())
    }
}

/// Configuration for an [`InertialFilter`].
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct InertialFilterConfig {
    /// IMU stochastic model used for covariance prediction.
    pub imu_spec: ImuSpec,
    /// Deterministic IMU calibration applied before mechanization.
    pub imu_model: ImuErrorModel,
    /// Strapdown mechanization options.
    pub mechanization: MechanizationConfig,
    /// Loose GNSS update options.
    pub loose: LooseCouplingConfig,
    /// Tight raw GNSS update options.
    pub tight: TightCouplingConfig,
}

impl InertialFilterConfig {
    /// Build a filter configuration with default calibration and loose settings.
    pub fn new(imu_spec: ImuSpec) -> Result<Self, FusionError> {
        let config = Self {
            imu_spec,
            imu_model: ImuErrorModel::default(),
            mechanization: MechanizationConfig::default(),
            loose: LooseCouplingConfig::default(),
            tight: TightCouplingConfig::default(),
        };
        config.validate()?;
        Ok(config)
    }

    /// Validate IMU, mechanization, and loose-coupling settings.
    pub fn validate(&self) -> Result<(), FusionError> {
        self.imu_spec.validate().map_err(FusionError::from)?;
        self.imu_model.bias.validate().map_err(FusionError::from)?;
        self.imu_model
            .calibration
            .validate()
            .map_err(FusionError::from)?;
        self.loose.validate()?;
        self.tight.validate()
    }
}

/// Result of one fusion update.
#[derive(Debug, Clone, PartialEq)]
pub struct FusionUpdate {
    /// Whether the update modified the nominal state and covariance.
    pub applied: bool,
    /// Normalized innovation squared for the update rows.
    pub nis: f64,
    /// Number of measurement rows entering the update.
    pub rows: usize,
    /// Number of rows accepted by any configured innovation screen.
    pub accepted_rows: usize,
    /// Number of rows rejected by any configured innovation screen.
    pub rejected_rows: usize,
    /// Full EKF correction report from the shared correction primitive.
    pub ekf: EkfCorrectionReport,
}

impl FusionUpdate {
    fn from_report(rows: usize, report: EkfCorrectionReport) -> Self {
        Self {
            applied: report.applied,
            nis: report.normalized_innovation_squared,
            rows,
            accepted_rows: report.accepted_rows,
            rejected_rows: report.rejected_rows,
            ekf: report,
        }
    }
}

/// Closed-loop INS filter with loose GNSS PVT updates.
#[derive(Debug, Clone, PartialEq)]
pub struct InertialFilter {
    pub(super) state: InsFilterState,
    pub(super) config: InertialFilterConfig,
    pub(super) last_body_rate_wrt_ecef_rps: [f64; 3],
    pub(super) time_sync: TimeSyncHistory,
    pub(super) tight: TightFusionState,
}

impl InertialFilter {
    /// Build a filter with default calibration and loose-coupling settings.
    pub fn new(state: InsFilterState, imu_spec: ImuSpec) -> Result<Self, FusionError> {
        let config = InertialFilterConfig::new(imu_spec)?;
        Self::with_config(state, config)
    }

    /// Build a filter with explicit configuration.
    pub fn with_config(
        state: InsFilterState,
        config: InertialFilterConfig,
    ) -> Result<Self, FusionError> {
        state.validate()?;
        config.validate()?;
        let tight = TightFusionState::from_filter_state(&state, config.tight)?;
        let time_sync = TimeSyncHistory::from_initial(&state, &tight);
        Ok(Self {
            state,
            config,
            last_body_rate_wrt_ecef_rps: [0.0; 3],
            time_sync,
            tight,
        })
    }

    /// Borrow the current INS filter state.
    pub const fn state(&self) -> &InsFilterState {
        &self.state
    }

    /// Mutably borrow the current INS filter state.
    pub fn state_mut(&mut self) -> &mut InsFilterState {
        &mut self.state
    }

    /// Borrow the immutable filter configuration.
    pub const fn config(&self) -> &InertialFilterConfig {
        &self.config
    }

    /// Return the most recent body angular rate relative to ECEF, resolved in body axes.
    pub const fn last_body_rate_wrt_ecef_rps(&self) -> [f64; 3] {
        self.last_body_rate_wrt_ecef_rps
    }

    /// Propagate the nominal INS state and error covariance with one IMU sample.
    pub fn propagate(&mut self, sample: ImuSample) -> Result<&InsFilterState, FusionError> {
        let previous_t_j2000_s = self.state.nominal.t_j2000_s;
        self.time_sync
            .validate_next_imu(previous_t_j2000_s, sample)?;
        self.propagate_core(sample)?;
        self.time_sync.push_imu(previous_t_j2000_s, sample);
        Ok(&self.state)
    }

    pub(super) fn propagate_core(&mut self, sample: ImuSample) -> Result<(), FusionError> {
        self.state.validate()?;
        self.config.validate()?;
        self.tight.align_with_filter_state(&self.state)?;

        let previous = self.state.nominal;
        let imu_model = self.effective_imu_model()?;
        let increment = imu_model
            .correct_sample(&sample, previous.t_j2000_s)
            .map_err(FusionError::from)?;
        let kinematics = ErrorStateImuKinematics::new(
            scale3(increment.delta_velocity_mps, 1.0 / increment.dt_s),
            scale3(increment.delta_theta_rad, 1.0 / increment.dt_s),
        )?;
        let linearization = linearize_error_state_ecef(
            &previous,
            kinematics,
            &self.config.imu_spec,
            increment.dt_s,
            self.state.layout(),
        )?;
        let next_nominal = mechanize_ecef(&previous, &increment, self.config.mechanization)
            .map_err(FusionError::from)?;
        let body_rate_wrt_ecef_rps = body_rate_relative_to_ecef(
            &next_nominal.attitude_body_to_ecef,
            kinematics.angular_rate_body_rps,
        );

        predict_error_state_covariance(
            &mut self.state.covariance,
            &linearization.phi,
            &linearization.q_d,
        )?;
        self.tight.predict_covariance(
            &linearization.phi,
            &linearization.q_d,
            increment.dt_s,
            self.config.tight,
        )?;
        self.tight.copy_base_covariance_to_state(&mut self.state)?;
        self.state.nominal = next_nominal;
        self.state.reset_error_state();
        self.last_body_rate_wrt_ecef_rps = body_rate_wrt_ecef_rps;
        self.state.validate()?;
        Ok(())
    }

    /// Apply a loose GNSS PVT update at the current propagated epoch.
    ///
    /// GNSS epochs must be strictly increasing across the filter's stateful
    /// update surface, matching the standalone time-sync order validator;
    /// duplicate or regressed epochs are rejected rather than fused twice.
    pub fn update_loose(
        &mut self,
        measurement: &GnssFixMeasurement,
    ) -> Result<FusionUpdate, FusionError> {
        if let Some(last) = self.time_sync.last_measurement_t_j2000_s() {
            if measurement.t_j2000_s <= last {
                return Err(invalid_input(
                    "t_j2000_s",
                    "GNSS measurement epochs must be strictly increasing",
                ));
            }
        }
        let update = self.update_loose_core(measurement)?;
        let snapshot = self.snapshot();
        self.time_sync
            .push_loose_measurement_and_checkpoint(measurement.clone(), snapshot);
        Ok(update)
    }

    pub(super) fn update_loose_core(
        &mut self,
        measurement: &GnssFixMeasurement,
    ) -> Result<FusionUpdate, FusionError> {
        let correction = loose_coupling_correction(
            &self.state,
            measurement,
            self.config.loose.lever_arm_body_m,
            self.last_body_rate_wrt_ecef_rps,
        )?;
        let rows = correction.row_count();
        let report = ekf_correct_closed_loop(
            &mut self.state,
            &correction,
            self.config.loose.update_options,
        )?;
        self.tight
            .replace_base_covariance_and_clear_cross(&self.state.covariance)?;
        Ok(FusionUpdate::from_report(rows, report))
    }

    fn effective_imu_model(&self) -> Result<ImuErrorModel, FusionError> {
        let mut bias = self.config.imu_model.bias;
        for axis in 0..3 {
            bias.accel_mps2[axis] += self.state.nominal.accel_bias_mps2[axis];
            bias.gyro_rps[axis] += self.state.nominal.gyro_bias_rps[axis];
        }
        let calibration = effective_calibration(
            self.config.imu_model.calibration,
            self.state.accel_scale_factor,
            self.state.gyro_scale_factor,
        )?;
        let model = ImuErrorModel { bias, calibration };
        model.bias.validate().map_err(FusionError::from)?;
        model.calibration.validate().map_err(FusionError::from)?;
        Ok(model)
    }
}

/// Build the loose-coupled GNSS EKF correction for an INS state.
///
/// The returned design matrix follows the nominal-error convention used by
/// [`InsFilterState`]: navigation errors are subtracted during closed-loop reset,
/// while bias errors are added to the closed-loop bias estimates.
///
/// `body_rate_wrt_ecef_rps` is the body angular rate relative to ECEF, resolved
/// in body axes. A body fixed in ECEF supplies zero for this rate even though
/// its gyroscopes measure Earth rate.
pub fn loose_coupling_correction(
    state: &InsFilterState,
    measurement: &GnssFixMeasurement,
    lever_arm_body_m: [f64; 3],
    body_rate_wrt_ecef_rps: [f64; 3],
) -> Result<EkfCorrection, FusionError> {
    state.validate()?;
    measurement.validate()?;
    validate_vec3(lever_arm_body_m, "lever_arm_body_m").map_err(FusionError::from)?;
    validate_vec3(body_rate_wrt_ecef_rps, "body_rate_wrt_ecef_rps").map_err(FusionError::from)?;
    if measurement.t_j2000_s != state.nominal.t_j2000_s {
        return Err(invalid_input("t_j2000_s", "must equal nominal state epoch"));
    }

    let dimension = state.dimension();
    let c_b_e = state.nominal.attitude_body_to_ecef;
    let lever_ecef_m = mul_vec3(&c_b_e, lever_arm_body_m);
    let antenna_position_ecef_m = add3(state.nominal.position_ecef_m, lever_ecef_m);
    let lever_velocity_body_mps = cross3(body_rate_wrt_ecef_rps, lever_arm_body_m);
    let lever_velocity_ecef_mps = mul_vec3(&c_b_e, lever_velocity_body_mps);
    let antenna_velocity_ecef_mps = add3(state.nominal.velocity_ecef_mps, lever_velocity_ecef_mps);

    let mut innovation = Vec::with_capacity(measurement.row_count());
    let mut design = Vec::with_capacity(measurement.row_count());
    let position_residual = sub3(measurement.position_ecef_m, antenna_position_ecef_m);
    let lever_position_skew = skew(lever_ecef_m);
    for axis in 0..3 {
        let mut row = vec![0.0; dimension];
        row[ERROR_POSITION_INDEX + axis] = -1.0;
        row[ERROR_ATTITUDE_INDEX..ERROR_ATTITUDE_INDEX + 3]
            .copy_from_slice(&lever_position_skew[axis]);
        innovation.push(position_residual[axis]);
        design.push(row);
    }

    if let Some(velocity_ecef_mps) = measurement.velocity_ecef_mps {
        let velocity_residual = sub3(velocity_ecef_mps, antenna_velocity_ecef_mps);
        let lever_velocity_skew = skew(lever_velocity_ecef_mps);
        let gyro_bias_block = inline_rxr(&c_b_e, &skew(lever_arm_body_m));
        for axis in 0..3 {
            let mut row = vec![0.0; dimension];
            row[ERROR_VELOCITY_INDEX + axis] = -1.0;
            row[ERROR_ATTITUDE_INDEX..ERROR_ATTITUDE_INDEX + 3]
                .copy_from_slice(&lever_velocity_skew[axis]);
            row[ERROR_GYRO_BIAS_INDEX..ERROR_GYRO_BIAS_INDEX + 3]
                .copy_from_slice(&gyro_bias_block[axis]);
            innovation.push(velocity_residual[axis]);
            design.push(row);
        }
    }

    EkfCorrection::new(innovation, design, measurement.covariance.clone())
}

fn body_rate_relative_to_ecef(
    attitude_body_to_ecef: &Mat3,
    inertial_body_rate_rps: [f64; 3],
) -> [f64; 3] {
    let attitude_ecef_to_body = inline_tr(attitude_body_to_ecef);
    let earth_rate_body_rps = mul_vec3(&attitude_ecef_to_body, [0.0, 0.0, OMEGA_E_DOT_RAD_S]);
    sub3(inertial_body_rate_rps, earth_rate_body_rps)
}

fn effective_calibration(
    base: ImuCalibration,
    accel_scale_factor: [f64; 3],
    gyro_scale_factor: [f64; 3],
) -> Result<ImuCalibration, FusionError> {
    let mut calibration = base;
    for axis in 0..3 {
        calibration.accel_scale_misalignment[axis][axis] += accel_scale_factor[axis];
        calibration.gyro_scale_misalignment[axis][axis] += gyro_scale_factor[axis];
    }
    calibration.validate().map_err(FusionError::from)?;
    Ok(calibration)
}

fn mat3_to_rows(matrix: [[f64; 3]; 3]) -> Vec<Vec<f64>> {
    matrix.into_iter().map(Vec::from).collect()
}

#[cfg(test)]
mod tests {
    //! Provenance: loose-coupled GNSS/INS equations follow Groves, Principles
    //! of GNSS, Inertial, and Multisensor Integrated Navigation Systems, 2nd
    //! ed., Chapter 14, with the lever-arm position and velocity model stated
    //! in the build spec. Synthetic noise uses the SplitMix64 sequence pattern
    //! from `astro/propagator/covariance.rs`. NEES/NIS consistency bands use
    //! the Bar-Shalom two-sided chi-square test.

    use super::*;
    use crate::astro::constants::earth::{OMEGA_E_DOT_RAD_S, WGS84_A_M};
    use crate::astro::math::mat3::{inline_tr, Mat3};
    use crate::astro::math::vec3::{dot3, norm3};
    use crate::fusion::state::{
        ERROR_ACCEL_BIAS_INDEX, ERROR_GYRO_BIAS_INDEX, ERROR_STATE_DIMENSION_15,
    };
    use crate::inertial::frames::gravity_ecef_mps2;
    use crate::inertial::state::{mat3_identity, mat3_mul, mat3_mul_vec, reorthonormalize_dcm};
    use crate::inertial::{CorrectedImuIncrement, NavState};
    use nalgebra::{DMatrix, DVector};

    fn assert_close(actual: f64, expected: f64, tolerance: f64) {
        assert!(
            (actual - expected).abs() <= tolerance,
            "actual {actual:.17e}, expected {expected:.17e}, tolerance {tolerance:.17e}"
        );
    }

    fn covariance_from_diag(diagonal: &[f64]) -> Vec<Vec<f64>> {
        let mut covariance = vec![vec![0.0; diagonal.len()]; diagonal.len()];
        for (idx, value) in diagonal.iter().enumerate() {
            covariance[idx][idx] = *value;
        }
        covariance
    }

    fn reference_filter_state(
        nominal: NavState,
        diagonal: &[f64],
    ) -> Result<InsFilterState, FusionError> {
        InsFilterState::from_diagonal(
            nominal,
            super::super::state::ErrorStateLayout::Fifteen,
            diagonal,
        )
    }

    #[test]
    fn loose_correction_builds_lever_arm_rows_and_keeps_input_covariance() {
        let state = reference_filter_state(
            NavState::new(10.0, [10.0, 20.0, 30.0], [1.0, 2.0, 3.0], mat3_identity())
                .expect("state"),
            &[1.0; ERROR_STATE_DIMENSION_15],
        )
        .expect("filter state");
        let lever = [0.5, -1.0, 2.0];
        let omega = [0.1, 0.2, -0.3];
        let lever_position = lever;
        let lever_velocity = cross3(omega, lever);
        let position_residual = [1.0, -2.0, 3.0];
        let velocity_residual = [0.4, -0.5, 0.6];
        let covariance = covariance_from_diag(&[4.0, 5.0, 6.0, 0.7, 0.8, 0.9]);
        let measurement = GnssFixMeasurement::position_velocity(
            10.0,
            add3(
                add3(state.nominal.position_ecef_m, lever_position),
                position_residual,
            ),
            add3(
                add3(state.nominal.velocity_ecef_mps, lever_velocity),
                velocity_residual,
            ),
            covariance.clone(),
            6,
        )
        .expect("measurement");

        let correction =
            loose_coupling_correction(&state, &measurement, lever, omega).expect("correction");

        for axis in 0..3 {
            assert_close(
                correction.innovation[axis],
                position_residual[axis],
                2.0e-16,
            );
            assert_close(
                correction.innovation[3 + axis],
                velocity_residual[axis],
                2.0e-16,
            );
        }
        assert_eq!(correction.measurement_covariance, covariance);
        assert_eq!(
            correction.design[0][ERROR_POSITION_INDEX].to_bits(),
            (-1.0_f64).to_bits()
        );
        assert_eq!(
            correction.design[1][ERROR_POSITION_INDEX + 1].to_bits(),
            (-1.0_f64).to_bits()
        );
        let lever_skew = skew(lever);
        for (row, expected_row) in lever_skew.iter().enumerate() {
            for (col, expected) in expected_row.iter().enumerate() {
                assert_eq!(
                    correction.design[row][ERROR_ATTITUDE_INDEX + col].to_bits(),
                    expected.to_bits()
                );
            }
        }
        let gyro_block = skew(lever);
        for (row, expected_row) in gyro_block.iter().enumerate() {
            for (col, expected) in expected_row.iter().enumerate() {
                assert_eq!(
                    correction.design[3 + row][ERROR_GYRO_BIAS_INDEX + col].to_bits(),
                    expected.to_bits()
                );
            }
        }
    }

    #[test]
    fn propagated_static_ecef_body_reports_zero_lever_velocity() {
        let lever = [1.0, 0.5, -0.25];
        let truth =
            NavState::new(0.0, [WGS84_A_M, 0.0, 0.0], [0.0; 3], mat3_identity()).expect("truth");
        let state =
            reference_filter_state(truth, &[1.0; ERROR_STATE_DIMENSION_15]).expect("filter state");
        let spec = ImuSpec::datasheet(
            0.0,
            0.0,
            0.0,
            0.0,
            crate::inertial::config::RANDOM_WALK_BIAS_TAU_S,
            crate::inertial::config::RANDOM_WALK_BIAS_TAU_S,
            None,
            None,
        );
        let mut config = InertialFilterConfig::new(spec).expect("config");
        config.loose.lever_arm_body_m = lever;
        let mut filter = InertialFilter::with_config(state, config).expect("filter");
        let (truth_next, sample, truth_body_rate_wrt_ecef) =
            inverted_static_sample(&truth, 1.0, 1.0, [0.0; 3], [0.0; 3]);

        for value in truth_body_rate_wrt_ecef {
            assert_close(value, 0.0, 0.0);
        }
        filter.propagate(sample).expect("propagate");
        for value in filter.last_body_rate_wrt_ecef_rps() {
            assert_close(value, 0.0, 0.0);
        }

        let antenna_position = add3(
            truth_next.position_ecef_m,
            mul_vec3(&truth_next.attitude_body_to_ecef, lever),
        );
        let measurement = GnssFixMeasurement::position_velocity(
            truth_next.t_j2000_s,
            antenna_position,
            truth_next.velocity_ecef_mps,
            covariance_from_diag(&[1.0, 1.0, 1.0, 1.0e-6, 1.0e-6, 1.0e-6]),
            8,
        )
        .expect("measurement");
        let correction = loose_coupling_correction(
            filter.state(),
            &measurement,
            lever,
            filter.last_body_rate_wrt_ecef_rps(),
        )
        .expect("correction");
        for axis in 0..3 {
            assert_close(correction.innovation[3 + axis], 0.0, 0.0);
        }
    }

    #[test]
    fn loose_update_rejects_failed_or_short_gnss_fix() {
        let measurement = GnssFixMeasurement {
            t_j2000_s: 0.0,
            position_ecef_m: [WGS84_A_M, 0.0, 0.0],
            velocity_ecef_mps: None,
            covariance: covariance_from_diag(&[1.0, 1.0, 1.0]),
            satellites_used: 3,
            solution_valid: true,
        };
        assert!(matches!(
            measurement.validate(),
            Err(FusionError::InvalidInput {
                field: "satellites_used",
                reason: "at least 4 satellites required"
            })
        ));

        let failed = GnssFixMeasurement {
            satellites_used: 6,
            solution_valid: false,
            ..measurement
        };
        assert!(matches!(
            failed.validate(),
            Err(FusionError::InvalidInput {
                field: "solution_valid",
                reason: "GNSS fix must be successful"
            })
        ));
    }

    #[test]
    fn synthetic_static_truth_recovers_within_three_sigma_and_biases_converge() {
        let dt_s = 1.0;
        let steps = 20usize;
        let lever = [1.0, 0.5, -0.25];
        let accel_bias = [0.0015, -0.0010, 0.0020];
        let gyro_bias = [0.000009765625, -0.000009765625, 0.00001953125];
        let mut truth =
            NavState::new(0.0, [WGS84_A_M, 0.0, 0.0], [0.0; 3], mat3_identity()).expect("truth");
        let nominal = NavState::new(
            0.0,
            [WGS84_A_M + 2.0, -1.0, 0.5],
            [0.3, -0.2, 0.1],
            mat3_identity(),
        )
        .expect("nominal");
        let mut diagonal = vec![0.0; ERROR_STATE_DIMENSION_15];
        for axis in 0..3 {
            diagonal[ERROR_POSITION_INDEX + axis] = 25.0;
            diagonal[ERROR_VELOCITY_INDEX + axis] = 1.0;
            diagonal[ERROR_ATTITUDE_INDEX + axis] = 0.05 * 0.05;
            diagonal[ERROR_ACCEL_BIAS_INDEX + axis] = 0.05 * 0.05;
            diagonal[ERROR_GYRO_BIAS_INDEX + axis] = 0.003 * 0.003;
        }
        let state = reference_filter_state(nominal, &diagonal).expect("filter state");
        let spec = ImuSpec::datasheet(0.02, 0.001, 0.004, 2.0e-4, 300.0, 300.0, None, None);
        let mut config = InertialFilterConfig::new(spec).expect("config");
        config.loose.lever_arm_body_m = lever;
        let mut filter = InertialFilter::with_config(state, config).expect("filter");
        let mut rng = SplitMix64::new(0x4c4f_4f53_455f_0001);
        let position_sigma_m = 0.20;
        let velocity_sigma_mps = 0.030;
        let covariance = covariance_from_diag(&[
            position_sigma_m * position_sigma_m,
            position_sigma_m * position_sigma_m,
            position_sigma_m * position_sigma_m,
            velocity_sigma_mps * velocity_sigma_mps,
            velocity_sigma_mps * velocity_sigma_mps,
            velocity_sigma_mps * velocity_sigma_mps,
        ]);

        for step in 1..=steps {
            let (truth_next, sample, true_body_rate_wrt_ecef) =
                inverted_static_sample(&truth, step as f64 * dt_s, dt_s, accel_bias, gyro_bias);
            truth = truth_next;
            filter.propagate(sample).expect("propagate");
            let antenna_position = add3(
                truth.position_ecef_m,
                mul_vec3(&truth.attitude_body_to_ecef, lever),
            );
            let antenna_velocity = add3(
                truth.velocity_ecef_mps,
                mul_vec3(
                    &truth.attitude_body_to_ecef,
                    cross3(true_body_rate_wrt_ecef, lever),
                ),
            );
            let measurement = GnssFixMeasurement::position_velocity(
                truth.t_j2000_s,
                add_noise3(antenna_position, position_sigma_m, &mut rng),
                add_noise3(antenna_velocity, velocity_sigma_mps, &mut rng),
                covariance.clone(),
                8,
            )
            .expect("measurement");
            let update = filter.update_loose(&measurement).expect("loose update");
            assert!(update.applied);
            assert_eq!(
                update.nis.to_bits(),
                update.ekf.normalized_innovation_squared.to_bits()
            );
        }

        let state = filter.state();
        for (axis, expected_accel_bias) in accel_bias.iter().enumerate() {
            let position_error = state.nominal.position_ecef_m[axis] - truth.position_ecef_m[axis];
            let velocity_error =
                state.nominal.velocity_ecef_mps[axis] - truth.velocity_ecef_mps[axis];
            let position_bound = 3.0
                * state.covariance[ERROR_POSITION_INDEX + axis][ERROR_POSITION_INDEX + axis].sqrt();
            assert!(
                position_error.abs() <= position_bound,
                "position axis {axis} error {position_error:.17e}, bound {position_bound:.17e}"
            );
            assert!(
                velocity_error.abs()
                    <= 3.0
                        * state.covariance[ERROR_VELOCITY_INDEX + axis]
                            [ERROR_VELOCITY_INDEX + axis]
                            .sqrt(),
                "velocity axis {axis} error {velocity_error:.17e}"
            );
            let accel_bias_error = state.nominal.accel_bias_mps2[axis] - *expected_accel_bias;
            let accel_bias_bound = 3.0
                * state.covariance[ERROR_ACCEL_BIAS_INDEX + axis][ERROR_ACCEL_BIAS_INDEX + axis]
                    .sqrt();
            assert!(
                accel_bias_error.abs() <= accel_bias_bound,
                "accelerometer bias axis {axis} error {accel_bias_error:.17e}, bound {accel_bias_bound:.17e}"
            );
        }
    }

    #[test]
    fn lever_velocity_update_converges_observable_gyro_bias_components() {
        let dt_s = 0.1;
        let lever = [1.0, 0.5, -0.25];
        let gyro_bias = [0.0009765625, -0.0009765625, 0.001953125];
        let truth =
            NavState::new(0.0, [WGS84_A_M, 0.0, 0.0], [0.0; 3], mat3_identity()).expect("truth");
        let mut diagonal = vec![0.0; ERROR_STATE_DIMENSION_15];
        for axis in 0..3 {
            diagonal[ERROR_POSITION_INDEX + axis] = 1.0;
            diagonal[ERROR_VELOCITY_INDEX + axis] = 1.0e-10;
            diagonal[ERROR_ATTITUDE_INDEX + axis] = 1.0e-10;
            diagonal[ERROR_ACCEL_BIAS_INDEX + axis] = 1.0e-10;
            diagonal[ERROR_GYRO_BIAS_INDEX + axis] = 1.0e-4;
        }
        let state = reference_filter_state(truth, &diagonal).expect("filter state");
        let spec = ImuSpec::datasheet(
            0.0,
            0.0,
            0.0,
            0.0,
            crate::inertial::config::RANDOM_WALK_BIAS_TAU_S,
            crate::inertial::config::RANDOM_WALK_BIAS_TAU_S,
            None,
            None,
        );
        let mut config = InertialFilterConfig::new(spec).expect("config");
        config.loose.lever_arm_body_m = lever;
        let mut filter = InertialFilter::with_config(state, config).expect("filter");
        let (truth_next, sample, true_body_rate_wrt_ecef) =
            inverted_static_sample(&truth, dt_s, dt_s, [0.0; 3], gyro_bias);
        filter.propagate(sample).expect("propagate");

        let antenna_position = add3(
            truth_next.position_ecef_m,
            mul_vec3(&truth_next.attitude_body_to_ecef, lever),
        );
        let antenna_velocity = add3(
            truth_next.velocity_ecef_mps,
            mul_vec3(
                &truth_next.attitude_body_to_ecef,
                cross3(true_body_rate_wrt_ecef, lever),
            ),
        );
        let measurement = GnssFixMeasurement::position_velocity(
            truth_next.t_j2000_s,
            antenna_position,
            antenna_velocity,
            covariance_from_diag(&[1.0e6, 1.0e6, 1.0e6, 1.0e-8, 1.0e-8, 1.0e-8]),
            8,
        )
        .expect("measurement");
        let update = filter.update_loose(&measurement).expect("loose update");
        assert!(update.applied);

        let state = filter.state();
        for (axis, expected_gyro_bias) in gyro_bias.iter().enumerate() {
            let error = state.nominal.gyro_bias_rps[axis] - *expected_gyro_bias;
            let bound = 3.0
                * state.covariance[ERROR_GYRO_BIAS_INDEX + axis][ERROR_GYRO_BIAS_INDEX + axis]
                    .sqrt();
            assert!(
                error.abs() <= bound,
                "gyroscope bias axis {axis} error {error:.17e}, bound {bound:.17e}"
            );
        }
    }

    #[test]
    fn loose_nees_and_nis_land_inside_bar_shalom_chi_square_bands() {
        let trials = 40usize;
        let alpha = 0.05;
        let p_diag: [f64; 6] = [9.0, 4.0, 16.0, 0.25, 0.36, 0.49];
        let r_diag: [f64; 6] = [1.0, 1.44, 0.64, 0.04, 0.09, 0.16];
        let truth =
            NavState::new(20.0, [WGS84_A_M, 0.0, 0.0], [0.0; 3], mat3_identity()).expect("truth");
        let mut rng = SplitMix64::new(0x4241_5253_4841_4c4f);
        let mut nees_sum = 0.0;
        let mut nis_sum = 0.0;

        for _ in 0..trials {
            let mut initial_error = [0.0; 6];
            let mut measurement_noise = [0.0; 6];
            for idx in 0..6 {
                initial_error[idx] = p_diag[idx].sqrt() * rng.standard_normal();
                measurement_noise[idx] = r_diag[idx].sqrt() * rng.standard_normal();
            }
            let nominal = NavState::new(
                20.0,
                [
                    truth.position_ecef_m[0] + initial_error[0],
                    truth.position_ecef_m[1] + initial_error[1],
                    truth.position_ecef_m[2] + initial_error[2],
                ],
                [
                    truth.velocity_ecef_mps[0] + initial_error[3],
                    truth.velocity_ecef_mps[1] + initial_error[4],
                    truth.velocity_ecef_mps[2] + initial_error[5],
                ],
                mat3_identity(),
            )
            .expect("nominal");
            let mut diagonal = vec![0.0; ERROR_STATE_DIMENSION_15];
            diagonal[..6].copy_from_slice(&p_diag);
            for value in diagonal.iter_mut().take(ERROR_STATE_DIMENSION_15).skip(6) {
                *value = 1.0;
            }
            let state = reference_filter_state(nominal, &diagonal).expect("filter state");
            let spec = ImuSpec::datasheet(
                0.0,
                0.0,
                0.0,
                0.0,
                crate::inertial::config::RANDOM_WALK_BIAS_TAU_S,
                crate::inertial::config::RANDOM_WALK_BIAS_TAU_S,
                None,
                None,
            );
            let mut filter = InertialFilter::new(state, spec).expect("filter");
            let measurement = GnssFixMeasurement::position_velocity(
                20.0,
                [
                    truth.position_ecef_m[0] + measurement_noise[0],
                    truth.position_ecef_m[1] + measurement_noise[1],
                    truth.position_ecef_m[2] + measurement_noise[2],
                ],
                [
                    truth.velocity_ecef_mps[0] + measurement_noise[3],
                    truth.velocity_ecef_mps[1] + measurement_noise[4],
                    truth.velocity_ecef_mps[2] + measurement_noise[5],
                ],
                covariance_from_diag(&r_diag),
                8,
            )
            .expect("measurement");
            let expected_nis = (0..6)
                .map(|idx| {
                    let innovation = measurement_noise[idx] - initial_error[idx];
                    innovation * innovation / (p_diag[idx] + r_diag[idx])
                })
                .sum::<f64>();
            let update = filter.update_loose(&measurement).expect("loose update");
            assert_close(update.nis, expected_nis, 1.0e-9);
            nis_sum += update.nis;

            let updated = filter.state();
            for idx in 0..6 {
                let expected_variance = p_diag[idx] * r_diag[idx] / (p_diag[idx] + r_diag[idx]);
                assert_close(updated.covariance[idx][idx], expected_variance, 5.0e-15);
            }
            let dx = [
                updated.nominal.position_ecef_m[0] - truth.position_ecef_m[0],
                updated.nominal.position_ecef_m[1] - truth.position_ecef_m[1],
                updated.nominal.position_ecef_m[2] - truth.position_ecef_m[2],
                updated.nominal.velocity_ecef_mps[0] - truth.velocity_ecef_mps[0],
                updated.nominal.velocity_ecef_mps[1] - truth.velocity_ecef_mps[1],
                updated.nominal.velocity_ecef_mps[2] - truth.velocity_ecef_mps[2],
            ];
            nees_sum += quadratic_form(&updated.covariance, &dx, 6);
        }

        let nis_average = nis_sum / trials as f64;
        let nees_average = nees_sum / trials as f64;
        let dof = trials * 6;
        let lower = crate::quality::chi2_inv(alpha * 0.5, dof).expect("lower") / trials as f64;
        let upper =
            crate::quality::chi2_inv(1.0 - alpha * 0.5, dof).expect("upper") / trials as f64;
        assert!(
            (lower..=upper).contains(&nis_average),
            "NIS average {nis_average:.17e}, band [{lower:.17e}, {upper:.17e}]"
        );
        assert!(
            (lower..=upper).contains(&nees_average),
            "NEES average {nees_average:.17e}, band [{lower:.17e}, {upper:.17e}]"
        );
    }

    fn inverted_static_sample(
        state: &NavState,
        t_j2000_s: f64,
        dt_s: f64,
        accel_bias_mps2: [f64; 3],
        gyro_bias_rps: [f64; 3],
    ) -> (NavState, ImuSample, [f64; 3]) {
        let true_delta_theta_rad = [0.0, 0.0, OMEGA_E_DOT_RAD_S * dt_s];
        let true_delta_velocity_mps =
            inverse_delta_velocity(state, [0.0; 3], true_delta_theta_rad, dt_s);
        let increment = CorrectedImuIncrement {
            t_j2000_s,
            delta_velocity_mps: true_delta_velocity_mps,
            delta_theta_rad: true_delta_theta_rad,
            dt_s,
        };
        let truth_next =
            mechanize_ecef(state, &increment, MechanizationConfig::default()).expect("truth step");
        let sample = ImuSample::increment(
            t_j2000_s,
            add3(true_delta_velocity_mps, scale3(accel_bias_mps2, dt_s)),
            add3(true_delta_theta_rad, scale3(gyro_bias_rps, dt_s)),
            dt_s,
        );
        let true_body_rate_wrt_ecef = body_rate_relative_to_ecef(
            &truth_next.attitude_body_to_ecef,
            scale3(true_delta_theta_rad, 1.0 / dt_s),
        );
        (truth_next, sample, true_body_rate_wrt_ecef)
    }

    fn inverse_delta_velocity(
        state: &NavState,
        target_velocity_ecef_mps: [f64; 3],
        delta_theta_rad: [f64; 3],
        dt_s: f64,
    ) -> [f64; 3] {
        let c_avg = mid_interval_dcm(&state.attitude_body_to_ecef, delta_theta_rad, dt_s);
        let c_avg_t = inline_tr(&c_avg);
        let gravity = gravity_ecef_mps2(state.position_ecef_m).expect("gravity");
        let required_ecef = sub3(
            sub3(target_velocity_ecef_mps, state.velocity_ecef_mps),
            scale3(gravity, dt_s),
        );
        mat3_mul_vec(&c_avg_t, required_ecef)
    }

    fn mid_interval_dcm(
        attitude_body_to_ecef: &Mat3,
        delta_theta_rad: [f64; 3],
        dt_s: f64,
    ) -> Mat3 {
        let earth_half = earth_rotation_first_order(0.5 * dt_s);
        let body_half =
            crate::inertial::rodrigues_delta_dcm(scale3(delta_theta_rad, 0.5)).expect("body half");
        reorthonormalize_dcm(&mat3_mul(
            &mat3_mul(&earth_half, attitude_body_to_ecef),
            &body_half,
        ))
        .expect("mid dcm")
    }

    fn earth_rotation_first_order(dt_s: f64) -> Mat3 {
        [
            [1.0, OMEGA_E_DOT_RAD_S * dt_s, 0.0],
            [-OMEGA_E_DOT_RAD_S * dt_s, 1.0, 0.0],
            [0.0, 0.0, 1.0],
        ]
    }

    fn add_noise3(value: [f64; 3], sigma: f64, rng: &mut SplitMix64) -> [f64; 3] {
        [
            value[0] + sigma * rng.symmetric_unit(),
            value[1] + sigma * rng.symmetric_unit(),
            value[2] + sigma * rng.symmetric_unit(),
        ]
    }

    fn quadratic_form(covariance: &[Vec<f64>], dx: &[f64], dimension: usize) -> f64 {
        let mut data = Vec::with_capacity(dimension * dimension);
        for row in covariance.iter().take(dimension) {
            data.extend(row.iter().take(dimension));
        }
        let matrix = DMatrix::from_row_slice(dimension, dimension, &data);
        let vector = DVector::from_column_slice(dx);
        let solved = matrix.cholesky().expect("covariance SPD").solve(&vector);
        dot_slice(dx, solved.as_slice())
    }

    fn dot_slice(a: &[f64], b: &[f64]) -> f64 {
        a.iter().zip(b).map(|(x, y)| x * y).sum()
    }

    struct SplitMix64 {
        state: u64,
    }

    impl SplitMix64 {
        fn new(seed: u64) -> Self {
            Self { state: seed }
        }

        fn next_u64(&mut self) -> u64 {
            self.state = self.state.wrapping_add(0x9e37_79b9_7f4a_7c15);
            let mut z = self.state;
            z = (z ^ (z >> 30)).wrapping_mul(0xbf58_476d_1ce4_e5b9);
            z = (z ^ (z >> 27)).wrapping_mul(0x94d0_49bb_1331_11eb);
            z ^ (z >> 31)
        }

        fn unit_f64(&mut self) -> f64 {
            let bits = 0x3ff0_0000_0000_0000 | (self.next_u64() >> 12);
            f64::from_bits(bits) - 1.0
        }

        fn symmetric_unit(&mut self) -> f64 {
            2.0 * self.unit_f64() - 1.0
        }

        fn standard_normal(&mut self) -> f64 {
            let u1 = self.unit_f64().max(f64::MIN_POSITIVE);
            let u2 = self.unit_f64();
            (-2.0 * u1.ln()).sqrt() * (2.0 * core::f64::consts::PI * u2).cos()
        }
    }

    #[test]
    fn splitmix_sequence_matches_covariance_fixture_pattern_bits() {
        let mut rng = SplitMix64::new(0x9876_5432_10fe_dcba);
        assert_eq!(rng.next_u64(), 0xaf45_24ce_f491_bb91);
        assert_eq!(rng.next_u64(), 0x25fc_5376_94a6_001c);
        let mut rng = SplitMix64::new(0x9876_5432_10fe_dcba);
        assert_eq!(rng.unit_f64().to_bits(), 0x3fe5_e8a4_99de_9236);
    }

    #[test]
    fn gyro_bias_test_vector_is_observable_for_non_axis_lever() {
        let lever = [1.0, 0.5, -0.25];
        let gyro_bias = [0.000009765625, -0.000009765625, 0.00001953125];
        assert_eq!(dot3(lever, gyro_bias).to_bits(), 0.0_f64.to_bits());
        assert!(norm3(cross3(gyro_bias, lever)) > 0.0);
    }
}