sidereon_core/fusion/
loose.rs

1//! Loosely coupled GNSS PVT updates for the INS error-state filter.
2//!
3//! Measurement epochs are required to match the propagated INS epoch exactly.
4//! A later time-synchronization layer can lift that requirement without
5//! changing the measurement model here.
6
7use crate::astro::constants::earth::OMEGA_E_DOT_RAD_S;
8use crate::astro::math::mat3::{inline_rxr, inline_tr, mul_vec3, Mat3};
9use crate::astro::math::vec3::{add3, cross3, scale3, sub3};
10use crate::inertial::state::skew;
11use crate::inertial::{
12    mechanize_ecef, validate_finite, validate_vec3, ImuCalibration, ImuErrorModel, ImuSample,
13    ImuSpec, MechanizationConfig,
14};
15
16use super::ekf::{ekf_correct_closed_loop, EkfCorrection, EkfCorrectionReport, EkfUpdateOptions};
17use super::error_state::{
18    linearize_error_state_ecef, predict_error_state_covariance, ErrorStateImuKinematics,
19};
20use super::state::{
21    invalid_input, validate_covariance_matrix, FusionError, InsFilterState, ERROR_ATTITUDE_INDEX,
22    ERROR_GYRO_BIAS_INDEX, ERROR_POSITION_INDEX, ERROR_VELOCITY_INDEX,
23};
24use super::tight::{TightCouplingConfig, TightFusionState};
25use super::timesync::TimeSyncHistory;
26
27const LOOSE_MIN_SATELLITES: usize = 4;
28const POSITION_ROWS: usize = 3;
29const POSITION_VELOCITY_ROWS: usize = 6;
30
31/// GNSS PVT measurement used by the loose-coupled INS update.
32///
33/// The covariance matrix is ordered as `[position_x, position_y, position_z]`
34/// for a position-only fix and as `[position_x, position_y, position_z,
35/// velocity_x, velocity_y, velocity_z]` when velocity is present.
36#[derive(Debug, Clone, PartialEq)]
37pub struct GnssFixMeasurement {
38    /// Measurement epoch in seconds since J2000 on the caller's GNSS time scale.
39    pub t_j2000_s: f64,
40    /// GNSS antenna position in ECEF meters.
41    pub position_ecef_m: [f64; 3],
42    /// Optional GNSS antenna velocity in ECEF meters per second.
43    pub velocity_ecef_mps: Option<[f64; 3]>,
44    /// Measurement covariance in the order documented on this type.
45    pub covariance: Vec<Vec<f64>>,
46    /// Number of satellites used by the upstream GNSS fix.
47    pub satellites_used: usize,
48    /// Whether the upstream GNSS solver reported a successful fix.
49    pub solution_valid: bool,
50}
51
52impl GnssFixMeasurement {
53    /// Build a position-only GNSS fix measurement.
54    pub fn position(
55        t_j2000_s: f64,
56        position_ecef_m: [f64; 3],
57        position_covariance_m2: [[f64; 3]; 3],
58        satellites_used: usize,
59    ) -> Result<Self, FusionError> {
60        let measurement = Self {
61            t_j2000_s,
62            position_ecef_m,
63            velocity_ecef_mps: None,
64            covariance: mat3_to_rows(position_covariance_m2),
65            satellites_used,
66            solution_valid: true,
67        };
68        measurement.validate()?;
69        Ok(measurement)
70    }
71
72    /// Build a position and velocity GNSS fix measurement.
73    pub fn position_velocity(
74        t_j2000_s: f64,
75        position_ecef_m: [f64; 3],
76        velocity_ecef_mps: [f64; 3],
77        covariance: Vec<Vec<f64>>,
78        satellites_used: usize,
79    ) -> Result<Self, FusionError> {
80        let measurement = Self {
81            t_j2000_s,
82            position_ecef_m,
83            velocity_ecef_mps: Some(velocity_ecef_mps),
84            covariance,
85            satellites_used,
86            solution_valid: true,
87        };
88        measurement.validate()?;
89        Ok(measurement)
90    }
91
92    /// Validate finite values, solver status, satellite count, and covariance.
93    pub fn validate(&self) -> Result<(), FusionError> {
94        validate_finite(self.t_j2000_s, "t_j2000_s").map_err(FusionError::from)?;
95        validate_vec3(self.position_ecef_m, "position_ecef_m").map_err(FusionError::from)?;
96        if let Some(velocity) = self.velocity_ecef_mps {
97            validate_vec3(velocity, "velocity_ecef_mps").map_err(FusionError::from)?;
98        }
99        if !self.solution_valid {
100            return Err(invalid_input(
101                "solution_valid",
102                "GNSS fix must be successful",
103            ));
104        }
105        if self.satellites_used < LOOSE_MIN_SATELLITES {
106            return Err(invalid_input(
107                "satellites_used",
108                "at least 4 satellites required",
109            ));
110        }
111        validate_covariance_matrix(&self.covariance, self.row_count(), "gnss_covariance")
112    }
113
114    /// Return the number of measurement rows implied by this fix.
115    pub fn row_count(&self) -> usize {
116        if self.velocity_ecef_mps.is_some() {
117            POSITION_VELOCITY_ROWS
118        } else {
119            POSITION_ROWS
120        }
121    }
122}
123
124/// Configuration for loose-coupled GNSS updates.
125#[derive(Debug, Clone, Copy, PartialEq)]
126pub struct LooseCouplingConfig {
127    /// Body-frame vector from IMU origin to GNSS antenna phase center, in meters.
128    pub lever_arm_body_m: [f64; 3],
129    /// Generic EKF correction options applied to each loose update.
130    pub update_options: EkfUpdateOptions,
131}
132
133impl Default for LooseCouplingConfig {
134    fn default() -> Self {
135        Self {
136            lever_arm_body_m: [0.0; 3],
137            update_options: EkfUpdateOptions::default(),
138        }
139    }
140}
141
142impl LooseCouplingConfig {
143    /// Validate finite lever-arm entries and nested update options.
144    pub fn validate(&self) -> Result<(), FusionError> {
145        validate_vec3(self.lever_arm_body_m, "lever_arm_body_m").map_err(FusionError::from)?;
146        if let Some(gate) = self.update_options.innovation_gate {
147            gate.validate()?;
148        }
149        Ok(())
150    }
151}
152
153/// Configuration for an [`InertialFilter`].
154#[derive(Debug, Clone, Copy, PartialEq)]
155pub struct InertialFilterConfig {
156    /// IMU stochastic model used for covariance prediction.
157    pub imu_spec: ImuSpec,
158    /// Deterministic IMU calibration applied before mechanization.
159    pub imu_model: ImuErrorModel,
160    /// Strapdown mechanization options.
161    pub mechanization: MechanizationConfig,
162    /// Loose GNSS update options.
163    pub loose: LooseCouplingConfig,
164    /// Tight raw GNSS update options.
165    pub tight: TightCouplingConfig,
166}
167
168impl InertialFilterConfig {
169    /// Build a filter configuration with default calibration and loose settings.
170    pub fn new(imu_spec: ImuSpec) -> Result<Self, FusionError> {
171        let config = Self {
172            imu_spec,
173            imu_model: ImuErrorModel::default(),
174            mechanization: MechanizationConfig::default(),
175            loose: LooseCouplingConfig::default(),
176            tight: TightCouplingConfig::default(),
177        };
178        config.validate()?;
179        Ok(config)
180    }
181
182    /// Validate IMU, mechanization, and loose-coupling settings.
183    pub fn validate(&self) -> Result<(), FusionError> {
184        self.imu_spec.validate().map_err(FusionError::from)?;
185        self.imu_model.bias.validate().map_err(FusionError::from)?;
186        self.imu_model
187            .calibration
188            .validate()
189            .map_err(FusionError::from)?;
190        self.loose.validate()?;
191        self.tight.validate()
192    }
193}
194
195/// Result of one fusion update.
196#[derive(Debug, Clone, PartialEq)]
197pub struct FusionUpdate {
198    /// Whether the update modified the nominal state and covariance.
199    pub applied: bool,
200    /// Normalized innovation squared for the update rows.
201    pub nis: f64,
202    /// Number of measurement rows entering the update.
203    pub rows: usize,
204    /// Number of rows accepted by any configured innovation screen.
205    pub accepted_rows: usize,
206    /// Number of rows rejected by any configured innovation screen.
207    pub rejected_rows: usize,
208    /// Full EKF correction report from the shared correction primitive.
209    pub ekf: EkfCorrectionReport,
210}
211
212impl FusionUpdate {
213    fn from_report(rows: usize, report: EkfCorrectionReport) -> Self {
214        Self {
215            applied: report.applied,
216            nis: report.normalized_innovation_squared,
217            rows,
218            accepted_rows: report.accepted_rows,
219            rejected_rows: report.rejected_rows,
220            ekf: report,
221        }
222    }
223}
224
225/// Closed-loop INS filter with loose GNSS PVT updates.
226#[derive(Debug, Clone, PartialEq)]
227pub struct InertialFilter {
228    pub(super) state: InsFilterState,
229    pub(super) config: InertialFilterConfig,
230    pub(super) last_body_rate_wrt_ecef_rps: [f64; 3],
231    pub(super) time_sync: TimeSyncHistory,
232    pub(super) tight: TightFusionState,
233}
234
235impl InertialFilter {
236    /// Build a filter with default calibration and loose-coupling settings.
237    pub fn new(state: InsFilterState, imu_spec: ImuSpec) -> Result<Self, FusionError> {
238        let config = InertialFilterConfig::new(imu_spec)?;
239        Self::with_config(state, config)
240    }
241
242    /// Build a filter with explicit configuration.
243    pub fn with_config(
244        state: InsFilterState,
245        config: InertialFilterConfig,
246    ) -> Result<Self, FusionError> {
247        state.validate()?;
248        config.validate()?;
249        let tight = TightFusionState::from_filter_state(&state, config.tight)?;
250        let time_sync = TimeSyncHistory::from_initial(&state, &tight);
251        Ok(Self {
252            state,
253            config,
254            last_body_rate_wrt_ecef_rps: [0.0; 3],
255            time_sync,
256            tight,
257        })
258    }
259
260    /// Borrow the current INS filter state.
261    pub const fn state(&self) -> &InsFilterState {
262        &self.state
263    }
264
265    /// Mutably borrow the current INS filter state.
266    pub fn state_mut(&mut self) -> &mut InsFilterState {
267        &mut self.state
268    }
269
270    /// Borrow the immutable filter configuration.
271    pub const fn config(&self) -> &InertialFilterConfig {
272        &self.config
273    }
274
275    /// Return the most recent body angular rate relative to ECEF, resolved in body axes.
276    pub const fn last_body_rate_wrt_ecef_rps(&self) -> [f64; 3] {
277        self.last_body_rate_wrt_ecef_rps
278    }
279
280    /// Propagate the nominal INS state and error covariance with one IMU sample.
281    pub fn propagate(&mut self, sample: ImuSample) -> Result<&InsFilterState, FusionError> {
282        let previous_t_j2000_s = self.state.nominal.t_j2000_s;
283        self.time_sync
284            .validate_next_imu(previous_t_j2000_s, sample)?;
285        self.propagate_core(sample)?;
286        self.time_sync.push_imu(previous_t_j2000_s, sample);
287        Ok(&self.state)
288    }
289
290    pub(super) fn propagate_core(&mut self, sample: ImuSample) -> Result<(), FusionError> {
291        self.state.validate()?;
292        self.config.validate()?;
293        self.tight.align_with_filter_state(&self.state)?;
294
295        let previous = self.state.nominal;
296        let imu_model = self.effective_imu_model()?;
297        let increment = imu_model
298            .correct_sample(&sample, previous.t_j2000_s)
299            .map_err(FusionError::from)?;
300        let kinematics = ErrorStateImuKinematics::new(
301            scale3(increment.delta_velocity_mps, 1.0 / increment.dt_s),
302            scale3(increment.delta_theta_rad, 1.0 / increment.dt_s),
303        )?;
304        let linearization = linearize_error_state_ecef(
305            &previous,
306            kinematics,
307            &self.config.imu_spec,
308            increment.dt_s,
309            self.state.layout(),
310        )?;
311        let next_nominal = mechanize_ecef(&previous, &increment, self.config.mechanization)
312            .map_err(FusionError::from)?;
313        let body_rate_wrt_ecef_rps = body_rate_relative_to_ecef(
314            &next_nominal.attitude_body_to_ecef,
315            kinematics.angular_rate_body_rps,
316        );
317
318        predict_error_state_covariance(
319            &mut self.state.covariance,
320            &linearization.phi,
321            &linearization.q_d,
322        )?;
323        self.tight.predict_covariance(
324            &linearization.phi,
325            &linearization.q_d,
326            increment.dt_s,
327            self.config.tight,
328        )?;
329        self.tight.copy_base_covariance_to_state(&mut self.state)?;
330        self.state.nominal = next_nominal;
331        self.state.reset_error_state();
332        self.last_body_rate_wrt_ecef_rps = body_rate_wrt_ecef_rps;
333        self.state.validate()?;
334        Ok(())
335    }
336
337    /// Apply a loose GNSS PVT update at the current propagated epoch.
338    ///
339    /// GNSS epochs must be strictly increasing across the filter's stateful
340    /// update surface, matching the standalone time-sync order validator;
341    /// duplicate or regressed epochs are rejected rather than fused twice.
342    pub fn update_loose(
343        &mut self,
344        measurement: &GnssFixMeasurement,
345    ) -> Result<FusionUpdate, FusionError> {
346        if let Some(last) = self.time_sync.last_measurement_t_j2000_s() {
347            if measurement.t_j2000_s <= last {
348                return Err(invalid_input(
349                    "t_j2000_s",
350                    "GNSS measurement epochs must be strictly increasing",
351                ));
352            }
353        }
354        let update = self.update_loose_core(measurement)?;
355        let snapshot = self.snapshot();
356        self.time_sync
357            .push_loose_measurement_and_checkpoint(measurement.clone(), snapshot);
358        Ok(update)
359    }
360
361    pub(super) fn update_loose_core(
362        &mut self,
363        measurement: &GnssFixMeasurement,
364    ) -> Result<FusionUpdate, FusionError> {
365        let correction = loose_coupling_correction(
366            &self.state,
367            measurement,
368            self.config.loose.lever_arm_body_m,
369            self.last_body_rate_wrt_ecef_rps,
370        )?;
371        let rows = correction.row_count();
372        let report = ekf_correct_closed_loop(
373            &mut self.state,
374            &correction,
375            self.config.loose.update_options,
376        )?;
377        self.tight
378            .replace_base_covariance_and_clear_cross(&self.state.covariance)?;
379        Ok(FusionUpdate::from_report(rows, report))
380    }
381
382    fn effective_imu_model(&self) -> Result<ImuErrorModel, FusionError> {
383        let mut bias = self.config.imu_model.bias;
384        for axis in 0..3 {
385            bias.accel_mps2[axis] += self.state.nominal.accel_bias_mps2[axis];
386            bias.gyro_rps[axis] += self.state.nominal.gyro_bias_rps[axis];
387        }
388        let calibration = effective_calibration(
389            self.config.imu_model.calibration,
390            self.state.accel_scale_factor,
391            self.state.gyro_scale_factor,
392        )?;
393        let model = ImuErrorModel { bias, calibration };
394        model.bias.validate().map_err(FusionError::from)?;
395        model.calibration.validate().map_err(FusionError::from)?;
396        Ok(model)
397    }
398}
399
400/// Build the loose-coupled GNSS EKF correction for an INS state.
401///
402/// The returned design matrix follows the nominal-error convention used by
403/// [`InsFilterState`]: navigation errors are subtracted during closed-loop reset,
404/// while bias errors are added to the closed-loop bias estimates.
405///
406/// `body_rate_wrt_ecef_rps` is the body angular rate relative to ECEF, resolved
407/// in body axes. A body fixed in ECEF supplies zero for this rate even though
408/// its gyroscopes measure Earth rate.
409pub fn loose_coupling_correction(
410    state: &InsFilterState,
411    measurement: &GnssFixMeasurement,
412    lever_arm_body_m: [f64; 3],
413    body_rate_wrt_ecef_rps: [f64; 3],
414) -> Result<EkfCorrection, FusionError> {
415    state.validate()?;
416    measurement.validate()?;
417    validate_vec3(lever_arm_body_m, "lever_arm_body_m").map_err(FusionError::from)?;
418    validate_vec3(body_rate_wrt_ecef_rps, "body_rate_wrt_ecef_rps").map_err(FusionError::from)?;
419    if measurement.t_j2000_s != state.nominal.t_j2000_s {
420        return Err(invalid_input("t_j2000_s", "must equal nominal state epoch"));
421    }
422
423    let dimension = state.dimension();
424    let c_b_e = state.nominal.attitude_body_to_ecef;
425    let lever_ecef_m = mul_vec3(&c_b_e, lever_arm_body_m);
426    let antenna_position_ecef_m = add3(state.nominal.position_ecef_m, lever_ecef_m);
427    let lever_velocity_body_mps = cross3(body_rate_wrt_ecef_rps, lever_arm_body_m);
428    let lever_velocity_ecef_mps = mul_vec3(&c_b_e, lever_velocity_body_mps);
429    let antenna_velocity_ecef_mps = add3(state.nominal.velocity_ecef_mps, lever_velocity_ecef_mps);
430
431    let mut innovation = Vec::with_capacity(measurement.row_count());
432    let mut design = Vec::with_capacity(measurement.row_count());
433    let position_residual = sub3(measurement.position_ecef_m, antenna_position_ecef_m);
434    let lever_position_skew = skew(lever_ecef_m);
435    for axis in 0..3 {
436        let mut row = vec![0.0; dimension];
437        row[ERROR_POSITION_INDEX + axis] = -1.0;
438        row[ERROR_ATTITUDE_INDEX..ERROR_ATTITUDE_INDEX + 3]
439            .copy_from_slice(&lever_position_skew[axis]);
440        innovation.push(position_residual[axis]);
441        design.push(row);
442    }
443
444    if let Some(velocity_ecef_mps) = measurement.velocity_ecef_mps {
445        let velocity_residual = sub3(velocity_ecef_mps, antenna_velocity_ecef_mps);
446        let lever_velocity_skew = skew(lever_velocity_ecef_mps);
447        let gyro_bias_block = inline_rxr(&c_b_e, &skew(lever_arm_body_m));
448        for axis in 0..3 {
449            let mut row = vec![0.0; dimension];
450            row[ERROR_VELOCITY_INDEX + axis] = -1.0;
451            row[ERROR_ATTITUDE_INDEX..ERROR_ATTITUDE_INDEX + 3]
452                .copy_from_slice(&lever_velocity_skew[axis]);
453            row[ERROR_GYRO_BIAS_INDEX..ERROR_GYRO_BIAS_INDEX + 3]
454                .copy_from_slice(&gyro_bias_block[axis]);
455            innovation.push(velocity_residual[axis]);
456            design.push(row);
457        }
458    }
459
460    EkfCorrection::new(innovation, design, measurement.covariance.clone())
461}
462
463fn body_rate_relative_to_ecef(
464    attitude_body_to_ecef: &Mat3,
465    inertial_body_rate_rps: [f64; 3],
466) -> [f64; 3] {
467    let attitude_ecef_to_body = inline_tr(attitude_body_to_ecef);
468    let earth_rate_body_rps = mul_vec3(&attitude_ecef_to_body, [0.0, 0.0, OMEGA_E_DOT_RAD_S]);
469    sub3(inertial_body_rate_rps, earth_rate_body_rps)
470}
471
472fn effective_calibration(
473    base: ImuCalibration,
474    accel_scale_factor: [f64; 3],
475    gyro_scale_factor: [f64; 3],
476) -> Result<ImuCalibration, FusionError> {
477    let mut calibration = base;
478    for axis in 0..3 {
479        calibration.accel_scale_misalignment[axis][axis] += accel_scale_factor[axis];
480        calibration.gyro_scale_misalignment[axis][axis] += gyro_scale_factor[axis];
481    }
482    calibration.validate().map_err(FusionError::from)?;
483    Ok(calibration)
484}
485
486fn mat3_to_rows(matrix: [[f64; 3]; 3]) -> Vec<Vec<f64>> {
487    matrix.into_iter().map(Vec::from).collect()
488}
489
490#[cfg(test)]
491mod tests {
492    //! Provenance: loose-coupled GNSS/INS equations follow Groves, Principles
493    //! of GNSS, Inertial, and Multisensor Integrated Navigation Systems, 2nd
494    //! ed., Chapter 14, with the lever-arm position and velocity model stated
495    //! in the build spec. Synthetic noise uses the SplitMix64 sequence pattern
496    //! from `astro/propagator/covariance.rs`. NEES/NIS consistency bands use
497    //! the Bar-Shalom two-sided chi-square test.
498
499    use super::*;
500    use crate::astro::constants::earth::{OMEGA_E_DOT_RAD_S, WGS84_A_M};
501    use crate::astro::math::mat3::{inline_tr, Mat3};
502    use crate::astro::math::vec3::{dot3, norm3};
503    use crate::fusion::state::{
504        ERROR_ACCEL_BIAS_INDEX, ERROR_GYRO_BIAS_INDEX, ERROR_STATE_DIMENSION_15,
505    };
506    use crate::inertial::frames::gravity_ecef_mps2;
507    use crate::inertial::state::{mat3_identity, mat3_mul, mat3_mul_vec, reorthonormalize_dcm};
508    use crate::inertial::{CorrectedImuIncrement, NavState};
509    use nalgebra::{DMatrix, DVector};
510
511    fn assert_close(actual: f64, expected: f64, tolerance: f64) {
512        assert!(
513            (actual - expected).abs() <= tolerance,
514            "actual {actual:.17e}, expected {expected:.17e}, tolerance {tolerance:.17e}"
515        );
516    }
517
518    fn covariance_from_diag(diagonal: &[f64]) -> Vec<Vec<f64>> {
519        let mut covariance = vec![vec![0.0; diagonal.len()]; diagonal.len()];
520        for (idx, value) in diagonal.iter().enumerate() {
521            covariance[idx][idx] = *value;
522        }
523        covariance
524    }
525
526    fn reference_filter_state(
527        nominal: NavState,
528        diagonal: &[f64],
529    ) -> Result<InsFilterState, FusionError> {
530        InsFilterState::from_diagonal(
531            nominal,
532            super::super::state::ErrorStateLayout::Fifteen,
533            diagonal,
534        )
535    }
536
537    #[test]
538    fn loose_correction_builds_lever_arm_rows_and_keeps_input_covariance() {
539        let state = reference_filter_state(
540            NavState::new(10.0, [10.0, 20.0, 30.0], [1.0, 2.0, 3.0], mat3_identity())
541                .expect("state"),
542            &[1.0; ERROR_STATE_DIMENSION_15],
543        )
544        .expect("filter state");
545        let lever = [0.5, -1.0, 2.0];
546        let omega = [0.1, 0.2, -0.3];
547        let lever_position = lever;
548        let lever_velocity = cross3(omega, lever);
549        let position_residual = [1.0, -2.0, 3.0];
550        let velocity_residual = [0.4, -0.5, 0.6];
551        let covariance = covariance_from_diag(&[4.0, 5.0, 6.0, 0.7, 0.8, 0.9]);
552        let measurement = GnssFixMeasurement::position_velocity(
553            10.0,
554            add3(
555                add3(state.nominal.position_ecef_m, lever_position),
556                position_residual,
557            ),
558            add3(
559                add3(state.nominal.velocity_ecef_mps, lever_velocity),
560                velocity_residual,
561            ),
562            covariance.clone(),
563            6,
564        )
565        .expect("measurement");
566
567        let correction =
568            loose_coupling_correction(&state, &measurement, lever, omega).expect("correction");
569
570        for axis in 0..3 {
571            assert_close(
572                correction.innovation[axis],
573                position_residual[axis],
574                2.0e-16,
575            );
576            assert_close(
577                correction.innovation[3 + axis],
578                velocity_residual[axis],
579                2.0e-16,
580            );
581        }
582        assert_eq!(correction.measurement_covariance, covariance);
583        assert_eq!(
584            correction.design[0][ERROR_POSITION_INDEX].to_bits(),
585            (-1.0_f64).to_bits()
586        );
587        assert_eq!(
588            correction.design[1][ERROR_POSITION_INDEX + 1].to_bits(),
589            (-1.0_f64).to_bits()
590        );
591        let lever_skew = skew(lever);
592        for (row, expected_row) in lever_skew.iter().enumerate() {
593            for (col, expected) in expected_row.iter().enumerate() {
594                assert_eq!(
595                    correction.design[row][ERROR_ATTITUDE_INDEX + col].to_bits(),
596                    expected.to_bits()
597                );
598            }
599        }
600        let gyro_block = skew(lever);
601        for (row, expected_row) in gyro_block.iter().enumerate() {
602            for (col, expected) in expected_row.iter().enumerate() {
603                assert_eq!(
604                    correction.design[3 + row][ERROR_GYRO_BIAS_INDEX + col].to_bits(),
605                    expected.to_bits()
606                );
607            }
608        }
609    }
610
611    #[test]
612    fn propagated_static_ecef_body_reports_zero_lever_velocity() {
613        let lever = [1.0, 0.5, -0.25];
614        let truth =
615            NavState::new(0.0, [WGS84_A_M, 0.0, 0.0], [0.0; 3], mat3_identity()).expect("truth");
616        let state =
617            reference_filter_state(truth, &[1.0; ERROR_STATE_DIMENSION_15]).expect("filter state");
618        let spec = ImuSpec::datasheet(
619            0.0,
620            0.0,
621            0.0,
622            0.0,
623            crate::inertial::config::RANDOM_WALK_BIAS_TAU_S,
624            crate::inertial::config::RANDOM_WALK_BIAS_TAU_S,
625            None,
626            None,
627        );
628        let mut config = InertialFilterConfig::new(spec).expect("config");
629        config.loose.lever_arm_body_m = lever;
630        let mut filter = InertialFilter::with_config(state, config).expect("filter");
631        let (truth_next, sample, truth_body_rate_wrt_ecef) =
632            inverted_static_sample(&truth, 1.0, 1.0, [0.0; 3], [0.0; 3]);
633
634        for value in truth_body_rate_wrt_ecef {
635            assert_close(value, 0.0, 0.0);
636        }
637        filter.propagate(sample).expect("propagate");
638        for value in filter.last_body_rate_wrt_ecef_rps() {
639            assert_close(value, 0.0, 0.0);
640        }
641
642        let antenna_position = add3(
643            truth_next.position_ecef_m,
644            mul_vec3(&truth_next.attitude_body_to_ecef, lever),
645        );
646        let measurement = GnssFixMeasurement::position_velocity(
647            truth_next.t_j2000_s,
648            antenna_position,
649            truth_next.velocity_ecef_mps,
650            covariance_from_diag(&[1.0, 1.0, 1.0, 1.0e-6, 1.0e-6, 1.0e-6]),
651            8,
652        )
653        .expect("measurement");
654        let correction = loose_coupling_correction(
655            filter.state(),
656            &measurement,
657            lever,
658            filter.last_body_rate_wrt_ecef_rps(),
659        )
660        .expect("correction");
661        for axis in 0..3 {
662            assert_close(correction.innovation[3 + axis], 0.0, 0.0);
663        }
664    }
665
666    #[test]
667    fn loose_update_rejects_failed_or_short_gnss_fix() {
668        let measurement = GnssFixMeasurement {
669            t_j2000_s: 0.0,
670            position_ecef_m: [WGS84_A_M, 0.0, 0.0],
671            velocity_ecef_mps: None,
672            covariance: covariance_from_diag(&[1.0, 1.0, 1.0]),
673            satellites_used: 3,
674            solution_valid: true,
675        };
676        assert!(matches!(
677            measurement.validate(),
678            Err(FusionError::InvalidInput {
679                field: "satellites_used",
680                reason: "at least 4 satellites required"
681            })
682        ));
683
684        let failed = GnssFixMeasurement {
685            satellites_used: 6,
686            solution_valid: false,
687            ..measurement
688        };
689        assert!(matches!(
690            failed.validate(),
691            Err(FusionError::InvalidInput {
692                field: "solution_valid",
693                reason: "GNSS fix must be successful"
694            })
695        ));
696    }
697
698    #[test]
699    fn synthetic_static_truth_recovers_within_three_sigma_and_biases_converge() {
700        let dt_s = 1.0;
701        let steps = 20usize;
702        let lever = [1.0, 0.5, -0.25];
703        let accel_bias = [0.0015, -0.0010, 0.0020];
704        let gyro_bias = [0.000009765625, -0.000009765625, 0.00001953125];
705        let mut truth =
706            NavState::new(0.0, [WGS84_A_M, 0.0, 0.0], [0.0; 3], mat3_identity()).expect("truth");
707        let nominal = NavState::new(
708            0.0,
709            [WGS84_A_M + 2.0, -1.0, 0.5],
710            [0.3, -0.2, 0.1],
711            mat3_identity(),
712        )
713        .expect("nominal");
714        let mut diagonal = vec![0.0; ERROR_STATE_DIMENSION_15];
715        for axis in 0..3 {
716            diagonal[ERROR_POSITION_INDEX + axis] = 25.0;
717            diagonal[ERROR_VELOCITY_INDEX + axis] = 1.0;
718            diagonal[ERROR_ATTITUDE_INDEX + axis] = 0.05 * 0.05;
719            diagonal[ERROR_ACCEL_BIAS_INDEX + axis] = 0.05 * 0.05;
720            diagonal[ERROR_GYRO_BIAS_INDEX + axis] = 0.003 * 0.003;
721        }
722        let state = reference_filter_state(nominal, &diagonal).expect("filter state");
723        let spec = ImuSpec::datasheet(0.02, 0.001, 0.004, 2.0e-4, 300.0, 300.0, None, None);
724        let mut config = InertialFilterConfig::new(spec).expect("config");
725        config.loose.lever_arm_body_m = lever;
726        let mut filter = InertialFilter::with_config(state, config).expect("filter");
727        let mut rng = SplitMix64::new(0x4c4f_4f53_455f_0001);
728        let position_sigma_m = 0.20;
729        let velocity_sigma_mps = 0.030;
730        let covariance = covariance_from_diag(&[
731            position_sigma_m * position_sigma_m,
732            position_sigma_m * position_sigma_m,
733            position_sigma_m * position_sigma_m,
734            velocity_sigma_mps * velocity_sigma_mps,
735            velocity_sigma_mps * velocity_sigma_mps,
736            velocity_sigma_mps * velocity_sigma_mps,
737        ]);
738
739        for step in 1..=steps {
740            let (truth_next, sample, true_body_rate_wrt_ecef) =
741                inverted_static_sample(&truth, step as f64 * dt_s, dt_s, accel_bias, gyro_bias);
742            truth = truth_next;
743            filter.propagate(sample).expect("propagate");
744            let antenna_position = add3(
745                truth.position_ecef_m,
746                mul_vec3(&truth.attitude_body_to_ecef, lever),
747            );
748            let antenna_velocity = add3(
749                truth.velocity_ecef_mps,
750                mul_vec3(
751                    &truth.attitude_body_to_ecef,
752                    cross3(true_body_rate_wrt_ecef, lever),
753                ),
754            );
755            let measurement = GnssFixMeasurement::position_velocity(
756                truth.t_j2000_s,
757                add_noise3(antenna_position, position_sigma_m, &mut rng),
758                add_noise3(antenna_velocity, velocity_sigma_mps, &mut rng),
759                covariance.clone(),
760                8,
761            )
762            .expect("measurement");
763            let update = filter.update_loose(&measurement).expect("loose update");
764            assert!(update.applied);
765            assert_eq!(
766                update.nis.to_bits(),
767                update.ekf.normalized_innovation_squared.to_bits()
768            );
769        }
770
771        let state = filter.state();
772        for (axis, expected_accel_bias) in accel_bias.iter().enumerate() {
773            let position_error = state.nominal.position_ecef_m[axis] - truth.position_ecef_m[axis];
774            let velocity_error =
775                state.nominal.velocity_ecef_mps[axis] - truth.velocity_ecef_mps[axis];
776            let position_bound = 3.0
777                * state.covariance[ERROR_POSITION_INDEX + axis][ERROR_POSITION_INDEX + axis].sqrt();
778            assert!(
779                position_error.abs() <= position_bound,
780                "position axis {axis} error {position_error:.17e}, bound {position_bound:.17e}"
781            );
782            assert!(
783                velocity_error.abs()
784                    <= 3.0
785                        * state.covariance[ERROR_VELOCITY_INDEX + axis]
786                            [ERROR_VELOCITY_INDEX + axis]
787                            .sqrt(),
788                "velocity axis {axis} error {velocity_error:.17e}"
789            );
790            let accel_bias_error = state.nominal.accel_bias_mps2[axis] - *expected_accel_bias;
791            let accel_bias_bound = 3.0
792                * state.covariance[ERROR_ACCEL_BIAS_INDEX + axis][ERROR_ACCEL_BIAS_INDEX + axis]
793                    .sqrt();
794            assert!(
795                accel_bias_error.abs() <= accel_bias_bound,
796                "accelerometer bias axis {axis} error {accel_bias_error:.17e}, bound {accel_bias_bound:.17e}"
797            );
798        }
799    }
800
801    #[test]
802    fn lever_velocity_update_converges_observable_gyro_bias_components() {
803        let dt_s = 0.1;
804        let lever = [1.0, 0.5, -0.25];
805        let gyro_bias = [0.0009765625, -0.0009765625, 0.001953125];
806        let truth =
807            NavState::new(0.0, [WGS84_A_M, 0.0, 0.0], [0.0; 3], mat3_identity()).expect("truth");
808        let mut diagonal = vec![0.0; ERROR_STATE_DIMENSION_15];
809        for axis in 0..3 {
810            diagonal[ERROR_POSITION_INDEX + axis] = 1.0;
811            diagonal[ERROR_VELOCITY_INDEX + axis] = 1.0e-10;
812            diagonal[ERROR_ATTITUDE_INDEX + axis] = 1.0e-10;
813            diagonal[ERROR_ACCEL_BIAS_INDEX + axis] = 1.0e-10;
814            diagonal[ERROR_GYRO_BIAS_INDEX + axis] = 1.0e-4;
815        }
816        let state = reference_filter_state(truth, &diagonal).expect("filter state");
817        let spec = ImuSpec::datasheet(
818            0.0,
819            0.0,
820            0.0,
821            0.0,
822            crate::inertial::config::RANDOM_WALK_BIAS_TAU_S,
823            crate::inertial::config::RANDOM_WALK_BIAS_TAU_S,
824            None,
825            None,
826        );
827        let mut config = InertialFilterConfig::new(spec).expect("config");
828        config.loose.lever_arm_body_m = lever;
829        let mut filter = InertialFilter::with_config(state, config).expect("filter");
830        let (truth_next, sample, true_body_rate_wrt_ecef) =
831            inverted_static_sample(&truth, dt_s, dt_s, [0.0; 3], gyro_bias);
832        filter.propagate(sample).expect("propagate");
833
834        let antenna_position = add3(
835            truth_next.position_ecef_m,
836            mul_vec3(&truth_next.attitude_body_to_ecef, lever),
837        );
838        let antenna_velocity = add3(
839            truth_next.velocity_ecef_mps,
840            mul_vec3(
841                &truth_next.attitude_body_to_ecef,
842                cross3(true_body_rate_wrt_ecef, lever),
843            ),
844        );
845        let measurement = GnssFixMeasurement::position_velocity(
846            truth_next.t_j2000_s,
847            antenna_position,
848            antenna_velocity,
849            covariance_from_diag(&[1.0e6, 1.0e6, 1.0e6, 1.0e-8, 1.0e-8, 1.0e-8]),
850            8,
851        )
852        .expect("measurement");
853        let update = filter.update_loose(&measurement).expect("loose update");
854        assert!(update.applied);
855
856        let state = filter.state();
857        for (axis, expected_gyro_bias) in gyro_bias.iter().enumerate() {
858            let error = state.nominal.gyro_bias_rps[axis] - *expected_gyro_bias;
859            let bound = 3.0
860                * state.covariance[ERROR_GYRO_BIAS_INDEX + axis][ERROR_GYRO_BIAS_INDEX + axis]
861                    .sqrt();
862            assert!(
863                error.abs() <= bound,
864                "gyroscope bias axis {axis} error {error:.17e}, bound {bound:.17e}"
865            );
866        }
867    }
868
869    #[test]
870    fn loose_nees_and_nis_land_inside_bar_shalom_chi_square_bands() {
871        let trials = 40usize;
872        let alpha = 0.05;
873        let p_diag: [f64; 6] = [9.0, 4.0, 16.0, 0.25, 0.36, 0.49];
874        let r_diag: [f64; 6] = [1.0, 1.44, 0.64, 0.04, 0.09, 0.16];
875        let truth =
876            NavState::new(20.0, [WGS84_A_M, 0.0, 0.0], [0.0; 3], mat3_identity()).expect("truth");
877        let mut rng = SplitMix64::new(0x4241_5253_4841_4c4f);
878        let mut nees_sum = 0.0;
879        let mut nis_sum = 0.0;
880
881        for _ in 0..trials {
882            let mut initial_error = [0.0; 6];
883            let mut measurement_noise = [0.0; 6];
884            for idx in 0..6 {
885                initial_error[idx] = p_diag[idx].sqrt() * rng.standard_normal();
886                measurement_noise[idx] = r_diag[idx].sqrt() * rng.standard_normal();
887            }
888            let nominal = NavState::new(
889                20.0,
890                [
891                    truth.position_ecef_m[0] + initial_error[0],
892                    truth.position_ecef_m[1] + initial_error[1],
893                    truth.position_ecef_m[2] + initial_error[2],
894                ],
895                [
896                    truth.velocity_ecef_mps[0] + initial_error[3],
897                    truth.velocity_ecef_mps[1] + initial_error[4],
898                    truth.velocity_ecef_mps[2] + initial_error[5],
899                ],
900                mat3_identity(),
901            )
902            .expect("nominal");
903            let mut diagonal = vec![0.0; ERROR_STATE_DIMENSION_15];
904            diagonal[..6].copy_from_slice(&p_diag);
905            for value in diagonal.iter_mut().take(ERROR_STATE_DIMENSION_15).skip(6) {
906                *value = 1.0;
907            }
908            let state = reference_filter_state(nominal, &diagonal).expect("filter state");
909            let spec = ImuSpec::datasheet(
910                0.0,
911                0.0,
912                0.0,
913                0.0,
914                crate::inertial::config::RANDOM_WALK_BIAS_TAU_S,
915                crate::inertial::config::RANDOM_WALK_BIAS_TAU_S,
916                None,
917                None,
918            );
919            let mut filter = InertialFilter::new(state, spec).expect("filter");
920            let measurement = GnssFixMeasurement::position_velocity(
921                20.0,
922                [
923                    truth.position_ecef_m[0] + measurement_noise[0],
924                    truth.position_ecef_m[1] + measurement_noise[1],
925                    truth.position_ecef_m[2] + measurement_noise[2],
926                ],
927                [
928                    truth.velocity_ecef_mps[0] + measurement_noise[3],
929                    truth.velocity_ecef_mps[1] + measurement_noise[4],
930                    truth.velocity_ecef_mps[2] + measurement_noise[5],
931                ],
932                covariance_from_diag(&r_diag),
933                8,
934            )
935            .expect("measurement");
936            let expected_nis = (0..6)
937                .map(|idx| {
938                    let innovation = measurement_noise[idx] - initial_error[idx];
939                    innovation * innovation / (p_diag[idx] + r_diag[idx])
940                })
941                .sum::<f64>();
942            let update = filter.update_loose(&measurement).expect("loose update");
943            assert_close(update.nis, expected_nis, 1.0e-9);
944            nis_sum += update.nis;
945
946            let updated = filter.state();
947            for idx in 0..6 {
948                let expected_variance = p_diag[idx] * r_diag[idx] / (p_diag[idx] + r_diag[idx]);
949                assert_close(updated.covariance[idx][idx], expected_variance, 5.0e-15);
950            }
951            let dx = [
952                updated.nominal.position_ecef_m[0] - truth.position_ecef_m[0],
953                updated.nominal.position_ecef_m[1] - truth.position_ecef_m[1],
954                updated.nominal.position_ecef_m[2] - truth.position_ecef_m[2],
955                updated.nominal.velocity_ecef_mps[0] - truth.velocity_ecef_mps[0],
956                updated.nominal.velocity_ecef_mps[1] - truth.velocity_ecef_mps[1],
957                updated.nominal.velocity_ecef_mps[2] - truth.velocity_ecef_mps[2],
958            ];
959            nees_sum += quadratic_form(&updated.covariance, &dx, 6);
960        }
961
962        let nis_average = nis_sum / trials as f64;
963        let nees_average = nees_sum / trials as f64;
964        let dof = trials * 6;
965        let lower = crate::quality::chi2_inv(alpha * 0.5, dof).expect("lower") / trials as f64;
966        let upper =
967            crate::quality::chi2_inv(1.0 - alpha * 0.5, dof).expect("upper") / trials as f64;
968        assert!(
969            (lower..=upper).contains(&nis_average),
970            "NIS average {nis_average:.17e}, band [{lower:.17e}, {upper:.17e}]"
971        );
972        assert!(
973            (lower..=upper).contains(&nees_average),
974            "NEES average {nees_average:.17e}, band [{lower:.17e}, {upper:.17e}]"
975        );
976    }
977
978    fn inverted_static_sample(
979        state: &NavState,
980        t_j2000_s: f64,
981        dt_s: f64,
982        accel_bias_mps2: [f64; 3],
983        gyro_bias_rps: [f64; 3],
984    ) -> (NavState, ImuSample, [f64; 3]) {
985        let true_delta_theta_rad = [0.0, 0.0, OMEGA_E_DOT_RAD_S * dt_s];
986        let true_delta_velocity_mps =
987            inverse_delta_velocity(state, [0.0; 3], true_delta_theta_rad, dt_s);
988        let increment = CorrectedImuIncrement {
989            t_j2000_s,
990            delta_velocity_mps: true_delta_velocity_mps,
991            delta_theta_rad: true_delta_theta_rad,
992            dt_s,
993        };
994        let truth_next =
995            mechanize_ecef(state, &increment, MechanizationConfig::default()).expect("truth step");
996        let sample = ImuSample::increment(
997            t_j2000_s,
998            add3(true_delta_velocity_mps, scale3(accel_bias_mps2, dt_s)),
999            add3(true_delta_theta_rad, scale3(gyro_bias_rps, dt_s)),
1000            dt_s,
1001        );
1002        let true_body_rate_wrt_ecef = body_rate_relative_to_ecef(
1003            &truth_next.attitude_body_to_ecef,
1004            scale3(true_delta_theta_rad, 1.0 / dt_s),
1005        );
1006        (truth_next, sample, true_body_rate_wrt_ecef)
1007    }
1008
1009    fn inverse_delta_velocity(
1010        state: &NavState,
1011        target_velocity_ecef_mps: [f64; 3],
1012        delta_theta_rad: [f64; 3],
1013        dt_s: f64,
1014    ) -> [f64; 3] {
1015        let c_avg = mid_interval_dcm(&state.attitude_body_to_ecef, delta_theta_rad, dt_s);
1016        let c_avg_t = inline_tr(&c_avg);
1017        let gravity = gravity_ecef_mps2(state.position_ecef_m).expect("gravity");
1018        let required_ecef = sub3(
1019            sub3(target_velocity_ecef_mps, state.velocity_ecef_mps),
1020            scale3(gravity, dt_s),
1021        );
1022        mat3_mul_vec(&c_avg_t, required_ecef)
1023    }
1024
1025    fn mid_interval_dcm(
1026        attitude_body_to_ecef: &Mat3,
1027        delta_theta_rad: [f64; 3],
1028        dt_s: f64,
1029    ) -> Mat3 {
1030        let earth_half = earth_rotation_first_order(0.5 * dt_s);
1031        let body_half =
1032            crate::inertial::rodrigues_delta_dcm(scale3(delta_theta_rad, 0.5)).expect("body half");
1033        reorthonormalize_dcm(&mat3_mul(
1034            &mat3_mul(&earth_half, attitude_body_to_ecef),
1035            &body_half,
1036        ))
1037        .expect("mid dcm")
1038    }
1039
1040    fn earth_rotation_first_order(dt_s: f64) -> Mat3 {
1041        [
1042            [1.0, OMEGA_E_DOT_RAD_S * dt_s, 0.0],
1043            [-OMEGA_E_DOT_RAD_S * dt_s, 1.0, 0.0],
1044            [0.0, 0.0, 1.0],
1045        ]
1046    }
1047
1048    fn add_noise3(value: [f64; 3], sigma: f64, rng: &mut SplitMix64) -> [f64; 3] {
1049        [
1050            value[0] + sigma * rng.symmetric_unit(),
1051            value[1] + sigma * rng.symmetric_unit(),
1052            value[2] + sigma * rng.symmetric_unit(),
1053        ]
1054    }
1055
1056    fn quadratic_form(covariance: &[Vec<f64>], dx: &[f64], dimension: usize) -> f64 {
1057        let mut data = Vec::with_capacity(dimension * dimension);
1058        for row in covariance.iter().take(dimension) {
1059            data.extend(row.iter().take(dimension));
1060        }
1061        let matrix = DMatrix::from_row_slice(dimension, dimension, &data);
1062        let vector = DVector::from_column_slice(dx);
1063        let solved = matrix.cholesky().expect("covariance SPD").solve(&vector);
1064        dot_slice(dx, solved.as_slice())
1065    }
1066
1067    fn dot_slice(a: &[f64], b: &[f64]) -> f64 {
1068        a.iter().zip(b).map(|(x, y)| x * y).sum()
1069    }
1070
1071    struct SplitMix64 {
1072        state: u64,
1073    }
1074
1075    impl SplitMix64 {
1076        fn new(seed: u64) -> Self {
1077            Self { state: seed }
1078        }
1079
1080        fn next_u64(&mut self) -> u64 {
1081            self.state = self.state.wrapping_add(0x9e37_79b9_7f4a_7c15);
1082            let mut z = self.state;
1083            z = (z ^ (z >> 30)).wrapping_mul(0xbf58_476d_1ce4_e5b9);
1084            z = (z ^ (z >> 27)).wrapping_mul(0x94d0_49bb_1331_11eb);
1085            z ^ (z >> 31)
1086        }
1087
1088        fn unit_f64(&mut self) -> f64 {
1089            let bits = 0x3ff0_0000_0000_0000 | (self.next_u64() >> 12);
1090            f64::from_bits(bits) - 1.0
1091        }
1092
1093        fn symmetric_unit(&mut self) -> f64 {
1094            2.0 * self.unit_f64() - 1.0
1095        }
1096
1097        fn standard_normal(&mut self) -> f64 {
1098            let u1 = self.unit_f64().max(f64::MIN_POSITIVE);
1099            let u2 = self.unit_f64();
1100            (-2.0 * u1.ln()).sqrt() * (2.0 * core::f64::consts::PI * u2).cos()
1101        }
1102    }
1103
1104    #[test]
1105    fn splitmix_sequence_matches_covariance_fixture_pattern_bits() {
1106        let mut rng = SplitMix64::new(0x9876_5432_10fe_dcba);
1107        assert_eq!(rng.next_u64(), 0xaf45_24ce_f491_bb91);
1108        assert_eq!(rng.next_u64(), 0x25fc_5376_94a6_001c);
1109        let mut rng = SplitMix64::new(0x9876_5432_10fe_dcba);
1110        assert_eq!(rng.unit_f64().to_bits(), 0x3fe5_e8a4_99de_9236);
1111    }
1112
1113    #[test]
1114    fn gyro_bias_test_vector_is_observable_for_non_axis_lever() {
1115        let lever = [1.0, 0.5, -0.25];
1116        let gyro_bias = [0.000009765625, -0.000009765625, 0.00001953125];
1117        assert_eq!(dot3(lever, gyro_bias).to_bits(), 0.0_f64.to_bits());
1118        assert!(norm3(cross3(gyro_bias, lever)) > 0.0);
1119    }
1120}
sidereon_core/fusion/loose.rs

sidereon_core/fusion/
loose.rs
