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

sidereon_core/fusion/
loose.rs
