sidereon_core/fusion/
state.rs

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

sidereon_core/fusion/
state.rs
