sidereon_core/fusion/
state.rs

1//! Error-state layout, filter state, and covariance validation.
2
3use nalgebra::DMatrix;
4
5use crate::astro::math::portable;
6use crate::inertial::{validate_finite, NavState};
7
8/// Number of states in the position, velocity, attitude, and bias layout.
9pub const ERROR_STATE_DIMENSION_15: usize = 15;
10/// Number of states after adding accelerometer and gyroscope scale factors.
11pub const ERROR_STATE_DIMENSION_21: usize = 21;
12/// Start index of ECEF position error states.
13pub const ERROR_POSITION_INDEX: usize = 0;
14/// Start index of ECEF velocity error states.
15pub const ERROR_VELOCITY_INDEX: usize = 3;
16/// Start index of ECEF attitude error states.
17pub const ERROR_ATTITUDE_INDEX: usize = 6;
18/// Start index of accelerometer bias error states.
19pub const ERROR_ACCEL_BIAS_INDEX: usize = 9;
20/// Start index of gyroscope bias error states.
21pub const ERROR_GYRO_BIAS_INDEX: usize = 12;
22/// Start index of accelerometer scale-factor error states in the 21-state layout.
23pub const ERROR_ACCEL_SCALE_INDEX: usize = 15;
24/// Start index of gyroscope scale-factor error states in the 21-state layout.
25pub const ERROR_GYRO_SCALE_INDEX: usize = 18;
26/// Reserved start index for future mounting-misalignment error states.
27pub const ERROR_MOUNTING_MISALIGNMENT_INDEX: usize = 21;
28/// Reserved mounting-misalignment state count for a later layout.
29pub const ERROR_MOUNTING_MISALIGNMENT_STATE_COUNT: usize = 3;
30
31const PSD_REL_TOLERANCE: f64 = 128.0 * f64::EPSILON;
32
33/// Error returned by GNSS/INS fusion primitives.
34#[derive(Debug, Clone, PartialEq, Eq, thiserror::Error)]
35pub enum FusionError {
36    /// A public input was non-finite or outside its documented domain.
37    #[error("invalid fusion input {field}: {reason}")]
38    InvalidInput {
39        /// Name of the invalid field.
40        field: &'static str,
41        /// Short reason suitable for logs and tests.
42        reason: &'static str,
43    },
44    /// A matrix or vector dimension did not match the selected state layout.
45    #[error("invalid fusion dimension {field}: expected {expected}, got {actual}")]
46    DimensionMismatch {
47        /// Name of the invalid field.
48        field: &'static str,
49        /// Expected element, row, or column count.
50        expected: usize,
51        /// Actual element, row, or column count.
52        actual: usize,
53    },
54    /// An innovation covariance could not be factored as positive definite.
55    #[error("fusion innovation covariance is singular")]
56    SingularInnovation,
57    /// A covariance was not positive semidefinite under numerical bounds.
58    #[error("fusion covariance {field} is not positive semidefinite")]
59    NonPositiveSemidefinite {
60        /// Name of the covariance field.
61        field: &'static str,
62    },
63    /// A covariance was not positive definite under numerical bounds.
64    #[error("fusion covariance {field} is not positive definite")]
65    NonPositiveDefinite {
66        /// Name of the covariance field.
67        field: &'static str,
68    },
69    /// A nominal inertial state failed validation.
70    #[error("invalid nominal inertial state")]
71    NominalState,
72}
73
74impl From<crate::inertial::InertialError> for FusionError {
75    fn from(_: crate::inertial::InertialError) -> Self {
76        Self::NominalState
77    }
78}
79
80/// Fusion filter family selector.
81#[derive(Debug, Clone, Copy, PartialEq, Eq)]
82pub enum FusionFilterKind {
83    /// Extended Kalman filter using the linearized error-state model.
84    Ekf,
85    /// Unscented Kalman filter using scaled sigma points.
86    Ukf,
87}
88
89/// Error-state covariance layout.
90#[derive(Debug, Clone, Copy, PartialEq, Eq)]
91pub enum ErrorStateLayout {
92    /// Fifteen-state layout `dr, dv, psi, b_a, b_g`.
93    Fifteen,
94    /// Twenty-one-state layout adding `s_a, s_g` after the first 15 states.
95    TwentyOne,
96}
97
98impl ErrorStateLayout {
99    /// Return the state dimension for this layout.
100    pub const fn dimension(self) -> usize {
101        match self {
102            Self::Fifteen => ERROR_STATE_DIMENSION_15,
103            Self::TwentyOne => ERROR_STATE_DIMENSION_21,
104        }
105    }
106
107    /// Return whether this layout carries IMU scale-factor error states.
108    pub const fn includes_scale_factors(self) -> bool {
109        matches!(self, Self::TwentyOne)
110    }
111
112    /// Validate a vector length against this layout.
113    pub fn validate_len(self, len: usize, field: &'static str) -> Result<(), FusionError> {
114        let expected = self.dimension();
115        if len == expected {
116            Ok(())
117        } else {
118            Err(FusionError::DimensionMismatch {
119                field,
120                expected,
121                actual: len,
122            })
123        }
124    }
125}
126
127/// Indirect filter error vector.
128#[derive(Debug, Clone, PartialEq)]
129pub struct ErrorStateVector {
130    layout: ErrorStateLayout,
131    values: Vec<f64>,
132}
133
134impl ErrorStateVector {
135    /// Build a zero error vector for `layout`.
136    pub fn zeros(layout: ErrorStateLayout) -> Self {
137        Self {
138            layout,
139            values: vec![0.0; layout.dimension()],
140        }
141    }
142
143    /// Build an error vector from owned values.
144    pub fn from_vec(layout: ErrorStateLayout, values: Vec<f64>) -> Result<Self, FusionError> {
145        layout.validate_len(values.len(), "error_state")?;
146        validate_finite_slice(&values, "error_state")?;
147        Ok(Self { layout, values })
148    }
149
150    /// Return the state layout.
151    pub const fn layout(&self) -> ErrorStateLayout {
152        self.layout
153    }
154
155    /// Return the vector dimension.
156    pub fn dimension(&self) -> usize {
157        self.values.len()
158    }
159
160    /// Borrow the error-state values.
161    pub fn as_slice(&self) -> &[f64] {
162        &self.values
163    }
164
165    /// Mutably borrow the error-state values.
166    pub fn as_mut_slice(&mut self) -> &mut [f64] {
167        &mut self.values
168    }
169
170    /// Reset all error-state values to zero.
171    pub fn reset(&mut self) {
172        self.values.fill(0.0);
173    }
174
175    /// Validate the vector shape and finite values.
176    pub fn validate(&self) -> Result<(), FusionError> {
177        self.layout.validate_len(self.values.len(), "error_state")?;
178        validate_finite_slice(&self.values, "error_state")
179    }
180}
181
182/// Closed-loop indirect INS filter state.
183#[derive(Debug, Clone, PartialEq)]
184pub struct InsFilterState {
185    /// Nonlinear mechanized navigation state.
186    pub nominal: NavState,
187    /// Small error-state estimate, reset to zero after closed-loop correction.
188    pub error_state: ErrorStateVector,
189    /// Error-state covariance matrix in row-major nested-vector form.
190    pub covariance: Vec<Vec<f64>>,
191    /// Fractional accelerometer scale-factor estimates for the 21-state layout.
192    pub accel_scale_factor: [f64; 3],
193    /// Fractional gyroscope scale-factor estimates for the 21-state layout.
194    pub gyro_scale_factor: [f64; 3],
195}
196
197impl InsFilterState {
198    /// Build a filter state from a nominal state and covariance.
199    pub fn new(
200        nominal: NavState,
201        layout: ErrorStateLayout,
202        covariance: Vec<Vec<f64>>,
203    ) -> Result<Self, FusionError> {
204        nominal.validate()?;
205        validate_covariance_matrix(&covariance, layout.dimension(), "covariance")?;
206        Ok(Self {
207            nominal,
208            error_state: ErrorStateVector::zeros(layout),
209            covariance,
210            accel_scale_factor: [0.0; 3],
211            gyro_scale_factor: [0.0; 3],
212        })
213    }
214
215    /// Build a filter state from diagonal covariance entries.
216    pub fn from_diagonal(
217        nominal: NavState,
218        layout: ErrorStateLayout,
219        diagonal: &[f64],
220    ) -> Result<Self, FusionError> {
221        layout.validate_len(diagonal.len(), "covariance_diagonal")?;
222        let mut covariance = vec![vec![0.0; layout.dimension()]; layout.dimension()];
223        for (idx, value) in diagonal.iter().enumerate() {
224            validate_finite(*value, "covariance_diagonal").map_err(FusionError::from)?;
225            if *value < 0.0 {
226                return Err(FusionError::InvalidInput {
227                    field: "covariance_diagonal",
228                    reason: "must be non-negative",
229                });
230            }
231            covariance[idx][idx] = *value;
232        }
233        Self::new(nominal, layout, covariance)
234    }
235
236    /// Return the selected error-state layout.
237    pub const fn layout(&self) -> ErrorStateLayout {
238        self.error_state.layout()
239    }
240
241    /// Return the state dimension.
242    pub fn dimension(&self) -> usize {
243        self.error_state.dimension()
244    }
245
246    /// Reset the indirect error estimate to zero.
247    pub fn reset_error_state(&mut self) {
248        self.error_state.reset();
249    }
250
251    /// Validate nominal state, error vector, and covariance.
252    pub fn validate(&self) -> Result<(), FusionError> {
253        self.nominal.validate()?;
254        self.error_state.validate()?;
255        validate_scale_factors(
256            self.layout(),
257            self.accel_scale_factor,
258            self.gyro_scale_factor,
259        )?;
260        validate_covariance_matrix(&self.covariance, self.dimension(), "covariance")
261    }
262}
263
264/// Validate covariance shape, finiteness, symmetry, and PSD.
265pub fn validate_covariance_matrix(
266    covariance: &[Vec<f64>],
267    dimension: usize,
268    field: &'static str,
269) -> Result<(), FusionError> {
270    validate_square_matrix(covariance, dimension, field)?;
271    if covariance_is_positive_semidefinite(covariance)? {
272        Ok(())
273    } else {
274        Err(FusionError::NonPositiveSemidefinite { field })
275    }
276}
277
278/// Test whether a covariance is positive semidefinite under numerical bounds.
279#[allow(clippy::needless_range_loop)]
280pub fn covariance_is_positive_semidefinite(covariance: &[Vec<f64>]) -> Result<bool, FusionError> {
281    let dimension = covariance.len();
282    validate_square_matrix(covariance, dimension, "covariance")?;
283    let scale = covariance
284        .iter()
285        .flatten()
286        .fold(0.0_f64, |acc, value| acc.max(value.abs()));
287    let symmetry_tolerance = psd_tolerance(dimension, scale);
288    for row in 0..dimension {
289        for col in (row + 1)..dimension {
290            if (covariance[row][col] - covariance[col][row]).abs() > symmetry_tolerance {
291                return Ok(false);
292            }
293        }
294    }
295    let matrix = dmatrix_from_rows(covariance);
296    let (eigenvectors, eigenvalues) = portable::symmetric_eigen_dynamic(&matrix);
297    for (idx, value) in eigenvalues.iter().enumerate() {
298        if !value.is_finite() {
299            return Ok(false);
300        }
301        let tolerance = covariance_eigenvalue_tolerance(covariance, &eigenvectors, idx);
302        if *value < -tolerance {
303            return Ok(false);
304        }
305    }
306    Ok(true)
307}
308
309/// Symmetrize and reproject a covariance onto the PSD cone under numerical bounds.
310pub fn reproject_covariance_psd(
311    covariance: &mut [Vec<f64>],
312    field: &'static str,
313) -> Result<(), FusionError> {
314    let dimension = covariance.len();
315    validate_square_matrix(covariance, dimension, field)?;
316    symmetrize_in_place(covariance);
317    let matrix = dmatrix_from_rows(covariance);
318    let (eigenvectors, eigenvalues) = portable::symmetric_eigen_dynamic(&matrix);
319    let mut needs_repair = false;
320    for (idx, value) in eigenvalues.iter().enumerate() {
321        if !value.is_finite() {
322            return Err(FusionError::NonPositiveSemidefinite { field });
323        }
324        let tolerance = covariance_eigenvalue_tolerance(covariance, &eigenvectors, idx);
325        if *value < -tolerance {
326            return Err(FusionError::NonPositiveSemidefinite { field });
327        }
328        needs_repair |= *value < 0.0;
329    }
330
331    if needs_repair {
332        let mut diagonal = DMatrix::<f64>::zeros(dimension, dimension);
333        for idx in 0..dimension {
334            diagonal[(idx, idx)] = eigenvalues[idx].max(0.0);
335        }
336        let repaired = portable::product(
337            &portable::product(&eigenvectors, &diagonal),
338            &eigenvectors.transpose(),
339        );
340        for row in 0..dimension {
341            for col in 0..dimension {
342                covariance[row][col] = repaired[(row, col)];
343            }
344        }
345        symmetrize_in_place(covariance);
346    }
347    validate_covariance_matrix(covariance, dimension, field)
348}
349
350pub(crate) fn invalid_input(field: &'static str, reason: &'static str) -> FusionError {
351    FusionError::InvalidInput { field, reason }
352}
353
354pub(crate) fn validate_positive(value: f64, field: &'static str) -> Result<(), FusionError> {
355    validate_finite(value, field).map_err(FusionError::from)?;
356    if value > 0.0 {
357        Ok(())
358    } else {
359        Err(invalid_input(field, "must be positive"))
360    }
361}
362
363pub(crate) fn validate_nonnegative(value: f64, field: &'static str) -> Result<(), FusionError> {
364    validate_finite(value, field).map_err(FusionError::from)?;
365    if value >= 0.0 {
366        Ok(())
367    } else {
368        Err(invalid_input(field, "must be non-negative"))
369    }
370}
371
372pub(crate) fn validate_finite_slice(
373    values: &[f64],
374    field: &'static str,
375) -> Result<(), FusionError> {
376    for value in values {
377        validate_finite(*value, field).map_err(FusionError::from)?;
378    }
379    Ok(())
380}
381
382pub(crate) fn validate_scale_factors(
383    layout: ErrorStateLayout,
384    accel_scale_factor: [f64; 3],
385    gyro_scale_factor: [f64; 3],
386) -> Result<(), FusionError> {
387    for value in accel_scale_factor {
388        validate_finite(value, "accel_scale_factor").map_err(FusionError::from)?;
389    }
390    for value in gyro_scale_factor {
391        validate_finite(value, "gyro_scale_factor").map_err(FusionError::from)?;
392    }
393    if !layout.includes_scale_factors()
394        && (accel_scale_factor.iter().any(|value| *value != 0.0)
395            || gyro_scale_factor.iter().any(|value| *value != 0.0))
396    {
397        return Err(invalid_input(
398            "scale_factor",
399            "requires the 21-state layout",
400        ));
401    }
402    Ok(())
403}
404
405pub(crate) fn validate_square_matrix(
406    matrix: &[Vec<f64>],
407    dimension: usize,
408    field: &'static str,
409) -> Result<(), FusionError> {
410    if matrix.len() != dimension {
411        return Err(FusionError::DimensionMismatch {
412            field,
413            expected: dimension,
414            actual: matrix.len(),
415        });
416    }
417    for row in matrix {
418        if row.len() != dimension {
419            return Err(FusionError::DimensionMismatch {
420                field,
421                expected: dimension,
422                actual: row.len(),
423            });
424        }
425        validate_finite_slice(row, field)?;
426    }
427    Ok(())
428}
429
430pub(crate) fn validate_matrix_cols(
431    matrix: &[Vec<f64>],
432    cols: usize,
433    field: &'static str,
434) -> Result<(), FusionError> {
435    for row in matrix {
436        if row.len() != cols {
437            return Err(FusionError::DimensionMismatch {
438                field,
439                expected: cols,
440                actual: row.len(),
441            });
442        }
443        validate_finite_slice(row, field)?;
444    }
445    Ok(())
446}
447
448pub(crate) fn identity(dimension: usize) -> Vec<Vec<f64>> {
449    let mut matrix = vec![vec![0.0; dimension]; dimension];
450    for (idx, row) in matrix.iter_mut().enumerate() {
451        row[idx] = 1.0;
452    }
453    matrix
454}
455
456pub(crate) fn transpose(matrix: &[Vec<f64>]) -> Result<Vec<Vec<f64>>, FusionError> {
457    let rows = matrix.len();
458    if rows == 0 {
459        return Ok(Vec::new());
460    }
461    let cols = matrix[0].len();
462    validate_matrix_cols(matrix, cols, "matrix")?;
463    let mut out = vec![vec![0.0; rows]; cols];
464    for row in 0..rows {
465        for col in 0..cols {
466            out[col][row] = matrix[row][col];
467        }
468    }
469    Ok(out)
470}
471
472pub(crate) fn matmul(a: &[Vec<f64>], b: &[Vec<f64>]) -> Result<Vec<Vec<f64>>, FusionError> {
473    if a.is_empty() || b.is_empty() {
474        return Err(invalid_input("matrix", "must not be empty"));
475    }
476    let inner = a[0].len();
477    validate_matrix_cols(a, inner, "matrix_a")?;
478    if b.len() != inner {
479        return Err(FusionError::DimensionMismatch {
480            field: "matrix_b",
481            expected: inner,
482            actual: b.len(),
483        });
484    }
485    let cols = b[0].len();
486    validate_matrix_cols(b, cols, "matrix_b")?;
487    let mut out = vec![vec![0.0; cols]; a.len()];
488    for row in 0..a.len() {
489        for col in 0..cols {
490            let mut sum = 0.0;
491            for k in 0..inner {
492                sum += a[row][k] * b[k][col];
493            }
494            out[row][col] = sum;
495        }
496    }
497    Ok(out)
498}
499
500pub(crate) fn matvec(matrix: &[Vec<f64>], vector: &[f64]) -> Result<Vec<f64>, FusionError> {
501    if matrix.is_empty() {
502        return Err(invalid_input("matrix", "must not be empty"));
503    }
504    let cols = vector.len();
505    validate_matrix_cols(matrix, cols, "matrix")?;
506    validate_finite_slice(vector, "vector")?;
507    let mut out = vec![0.0; matrix.len()];
508    for row in 0..matrix.len() {
509        let mut sum = 0.0;
510        for (col, value) in vector.iter().enumerate() {
511            sum += matrix[row][col] * value;
512        }
513        out[row] = sum;
514    }
515    Ok(out)
516}
517
518pub(crate) fn matrix_add(a: &[Vec<f64>], b: &[Vec<f64>]) -> Result<Vec<Vec<f64>>, FusionError> {
519    same_shape(a, b, "matrix_add")?;
520    let mut out = vec![vec![0.0; a[0].len()]; a.len()];
521    for row in 0..a.len() {
522        for col in 0..a[0].len() {
523            out[row][col] = a[row][col] + b[row][col];
524        }
525    }
526    Ok(out)
527}
528
529pub(crate) fn matrix_sub(a: &[Vec<f64>], b: &[Vec<f64>]) -> Result<Vec<Vec<f64>>, FusionError> {
530    same_shape(a, b, "matrix_sub")?;
531    let mut out = vec![vec![0.0; a[0].len()]; a.len()];
532    for row in 0..a.len() {
533        for col in 0..a[0].len() {
534            out[row][col] = a[row][col] - b[row][col];
535        }
536    }
537    Ok(out)
538}
539
540#[allow(clippy::needless_range_loop)]
541pub(crate) fn symmetrize_in_place(matrix: &mut [Vec<f64>]) {
542    let dimension = matrix.len();
543    for row in 0..dimension {
544        for col in (row + 1)..dimension {
545            let value = 0.5 * (matrix[row][col] + matrix[col][row]);
546            matrix[row][col] = value;
547            matrix[col][row] = value;
548        }
549    }
550}
551
552pub(crate) fn solve_spd(
553    matrix: &[Vec<f64>],
554    rhs: &[f64],
555    scratch: &mut crate::astro::math::linear::FlatCholeskySolveScratch,
556) -> Result<Vec<f64>, FusionError> {
557    validate_square_matrix(matrix, rhs.len(), "spd_matrix")?;
558    validate_finite_slice(rhs, "spd_rhs")?;
559    let flat = flatten(matrix);
560    crate::astro::math::linear::solve_flat_normal_square_root_into(&flat, rhs, scratch)
561        .map(<[f64]>::to_vec)
562        .ok_or(FusionError::SingularInnovation)
563}
564
565pub(crate) fn flatten(matrix: &[Vec<f64>]) -> Vec<f64> {
566    let rows = matrix.len();
567    let cols = if rows == 0 { 0 } else { matrix[0].len() };
568    let mut out = Vec::with_capacity(rows * cols);
569    for row in matrix {
570        out.extend(row);
571    }
572    out
573}
574
575pub(crate) fn dmatrix_from_rows(rows: &[Vec<f64>]) -> DMatrix<f64> {
576    let nrows = rows.len();
577    let ncols = if nrows == 0 { 0 } else { rows[0].len() };
578    DMatrix::from_row_slice(nrows, ncols, &flatten(rows))
579}
580
581fn same_shape(a: &[Vec<f64>], b: &[Vec<f64>], field: &'static str) -> Result<(), FusionError> {
582    if a.is_empty() || b.is_empty() {
583        return Err(invalid_input(field, "must not be empty"));
584    }
585    validate_matrix_cols(a, a[0].len(), field)?;
586    validate_matrix_cols(b, b[0].len(), field)?;
587    if a.len() != b.len() {
588        return Err(FusionError::DimensionMismatch {
589            field,
590            expected: a.len(),
591            actual: b.len(),
592        });
593    }
594    if a[0].len() != b[0].len() {
595        return Err(FusionError::DimensionMismatch {
596            field,
597            expected: a[0].len(),
598            actual: b[0].len(),
599        });
600    }
601    Ok(())
602}
603
604fn psd_tolerance(dimension: usize, scale: f64) -> f64 {
605    let dimension_scale = dimension.max(1) as f64;
606    PSD_REL_TOLERANCE * dimension_scale * scale
607}
608
609pub(crate) fn covariance_eigenvalue_tolerance(
610    covariance: &[Vec<f64>],
611    eigenvectors: &DMatrix<f64>,
612    mode: usize,
613) -> f64 {
614    let dimension = covariance.len();
615    let mut scale = 0.0_f64;
616    for row in 0..dimension {
617        let row_weight = eigenvectors[(row, mode)].abs();
618        for col in 0..dimension {
619            scale += row_weight * covariance[row][col].abs() * eigenvectors[(col, mode)].abs();
620        }
621    }
622    psd_tolerance(dimension, scale)
623}
624
625#[cfg(test)]
626mod tests {
627    //! Provenance: PSD validation tests lock the fusion safety contract that
628    //! invalid covariance input remains flagged instead of being repaired into a
629    //! false covariance.
630
631    use super::*;
632
633    #[test]
634    fn symmetry_tolerance_is_matrix_relative_for_tiny_off_diagonals() {
635        // A rotated block-diagonal covariance: off-diagonal pairs are float dust
636        // around zero whose absolute difference dwarfs their own magnitude but is
637        // negligible against the matrix scale. Element-relative symmetry
638        // tolerances reject this PSD matrix; the tolerance must be
639        // matrix-scale-relative.
640        let mut covariance = vec![vec![0.0; 6]; 6];
641        for (idx, variance) in [2.25, 2.25, 9.0, 0.0025, 0.0025, 0.0025].iter().enumerate() {
642            covariance[idx][idx] = *variance;
643        }
644        covariance[0][3] = 1.0e-19;
645        covariance[3][0] = -3.0e-19;
646        assert!(covariance_is_positive_semidefinite(&covariance).expect("validate"));
647    }
648
649    #[test]
650    fn zero_covariance_is_psd() {
651        let covariance = vec![vec![0.0]];
652        assert!(covariance_is_positive_semidefinite(&covariance).expect("psd"));
653    }
654
655    #[test]
656    fn tiny_negative_variance_is_rejected_not_repaired() {
657        let covariance = vec![vec![-1.0e-15]];
658        assert!(!covariance_is_positive_semidefinite(&covariance).expect("psd"));
659
660        let mut covariance = covariance;
661        let err = reproject_covariance_psd(&mut covariance, "covariance")
662            .expect_err("negative variance must remain flagged");
663        assert!(matches!(
664            err,
665            FusionError::NonPositiveSemidefinite {
666                field: "covariance"
667            }
668        ));
669        assert_eq!(covariance[0][0].to_bits(), (-1.0e-15_f64).to_bits());
670    }
671
672    #[test]
673    fn unrelated_large_variance_does_not_hide_negative_mode() {
674        let covariance = vec![vec![1.0e16, 0.0], vec![0.0, -100.0]];
675        assert!(!covariance_is_positive_semidefinite(&covariance).expect("psd"));
676
677        let mut covariance = covariance;
678        let err = reproject_covariance_psd(&mut covariance, "covariance")
679            .expect_err("negative mode must remain flagged");
680        assert!(matches!(
681            err,
682            FusionError::NonPositiveSemidefinite {
683                field: "covariance"
684            }
685        ));
686        assert_eq!(covariance[1][1].to_bits(), (-100.0_f64).to_bits());
687    }
688}
sidereon_core/fusion/state.rs

sidereon_core/fusion/
state.rs
