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 placeholder for a later nonlinear pass.
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 = matrix_scale(covariance);
283    let tolerance = psd_tolerance(dimension, scale);
284    for row in 0..dimension {
285        for col in (row + 1)..dimension {
286            if (covariance[row][col] - covariance[col][row]).abs() > tolerance {
287                return Ok(false);
288            }
289        }
290    }
291    let matrix = dmatrix_from_rows(covariance);
292    let eigen = matrix.symmetric_eigen();
293    Ok(eigen
294        .eigenvalues
295        .iter()
296        .all(|value| value.is_finite() && *value >= -tolerance))
297}
298
299/// Symmetrize and reproject a covariance onto the PSD cone under numerical bounds.
300pub fn reproject_covariance_psd(
301    covariance: &mut [Vec<f64>],
302    field: &'static str,
303) -> Result<(), FusionError> {
304    let dimension = covariance.len();
305    validate_square_matrix(covariance, dimension, field)?;
306    symmetrize_in_place(covariance);
307    let scale = matrix_scale(covariance);
308    let tolerance = psd_tolerance(dimension, scale);
309    let matrix = dmatrix_from_rows(covariance);
310    let eigen = matrix.symmetric_eigen();
311    let min_eigenvalue = eigen
312        .eigenvalues
313        .iter()
314        .fold(f64::INFINITY, |acc, value| acc.min(*value));
315    if !min_eigenvalue.is_finite() || min_eigenvalue < -tolerance {
316        return Err(FusionError::NonPositiveSemidefinite { field });
317    }
318
319    if min_eigenvalue < 0.0 {
320        let mut diagonal = DMatrix::<f64>::zeros(dimension, dimension);
321        for idx in 0..dimension {
322            diagonal[(idx, idx)] = eigen.eigenvalues[idx].max(0.0);
323        }
324        let repaired = &eigen.eigenvectors * diagonal * eigen.eigenvectors.transpose();
325        for row in 0..dimension {
326            for col in 0..dimension {
327                covariance[row][col] = repaired[(row, col)];
328            }
329        }
330        symmetrize_in_place(covariance);
331    }
332    validate_covariance_matrix(covariance, dimension, field)
333}
334
335pub(crate) fn invalid_input(field: &'static str, reason: &'static str) -> FusionError {
336    FusionError::InvalidInput { field, reason }
337}
338
339pub(crate) fn validate_positive(value: f64, field: &'static str) -> Result<(), FusionError> {
340    validate_finite(value, field).map_err(FusionError::from)?;
341    if value > 0.0 {
342        Ok(())
343    } else {
344        Err(invalid_input(field, "must be positive"))
345    }
346}
347
348pub(crate) fn validate_nonnegative(value: f64, field: &'static str) -> Result<(), FusionError> {
349    validate_finite(value, field).map_err(FusionError::from)?;
350    if value >= 0.0 {
351        Ok(())
352    } else {
353        Err(invalid_input(field, "must be non-negative"))
354    }
355}
356
357pub(crate) fn validate_finite_slice(
358    values: &[f64],
359    field: &'static str,
360) -> Result<(), FusionError> {
361    for value in values {
362        validate_finite(*value, field).map_err(FusionError::from)?;
363    }
364    Ok(())
365}
366
367pub(crate) fn validate_scale_factors(
368    layout: ErrorStateLayout,
369    accel_scale_factor: [f64; 3],
370    gyro_scale_factor: [f64; 3],
371) -> Result<(), FusionError> {
372    for value in accel_scale_factor {
373        validate_finite(value, "accel_scale_factor").map_err(FusionError::from)?;
374    }
375    for value in gyro_scale_factor {
376        validate_finite(value, "gyro_scale_factor").map_err(FusionError::from)?;
377    }
378    if !layout.includes_scale_factors()
379        && (accel_scale_factor.iter().any(|value| *value != 0.0)
380            || gyro_scale_factor.iter().any(|value| *value != 0.0))
381    {
382        return Err(invalid_input(
383            "scale_factor",
384            "requires the 21-state layout",
385        ));
386    }
387    Ok(())
388}
389
390pub(crate) fn validate_square_matrix(
391    matrix: &[Vec<f64>],
392    dimension: usize,
393    field: &'static str,
394) -> Result<(), FusionError> {
395    if matrix.len() != dimension {
396        return Err(FusionError::DimensionMismatch {
397            field,
398            expected: dimension,
399            actual: matrix.len(),
400        });
401    }
402    for row in matrix {
403        if row.len() != dimension {
404            return Err(FusionError::DimensionMismatch {
405                field,
406                expected: dimension,
407                actual: row.len(),
408            });
409        }
410        validate_finite_slice(row, field)?;
411    }
412    Ok(())
413}
414
415pub(crate) fn validate_matrix_cols(
416    matrix: &[Vec<f64>],
417    cols: usize,
418    field: &'static str,
419) -> Result<(), FusionError> {
420    for row in matrix {
421        if row.len() != cols {
422            return Err(FusionError::DimensionMismatch {
423                field,
424                expected: cols,
425                actual: row.len(),
426            });
427        }
428        validate_finite_slice(row, field)?;
429    }
430    Ok(())
431}
432
433pub(crate) fn identity(dimension: usize) -> Vec<Vec<f64>> {
434    let mut matrix = vec![vec![0.0; dimension]; dimension];
435    for (idx, row) in matrix.iter_mut().enumerate() {
436        row[idx] = 1.0;
437    }
438    matrix
439}
440
441pub(crate) fn transpose(matrix: &[Vec<f64>]) -> Result<Vec<Vec<f64>>, FusionError> {
442    let rows = matrix.len();
443    if rows == 0 {
444        return Ok(Vec::new());
445    }
446    let cols = matrix[0].len();
447    validate_matrix_cols(matrix, cols, "matrix")?;
448    let mut out = vec![vec![0.0; rows]; cols];
449    for row in 0..rows {
450        for col in 0..cols {
451            out[col][row] = matrix[row][col];
452        }
453    }
454    Ok(out)
455}
456
457pub(crate) fn matmul(a: &[Vec<f64>], b: &[Vec<f64>]) -> Result<Vec<Vec<f64>>, FusionError> {
458    if a.is_empty() || b.is_empty() {
459        return Err(invalid_input("matrix", "must not be empty"));
460    }
461    let inner = a[0].len();
462    validate_matrix_cols(a, inner, "matrix_a")?;
463    if b.len() != inner {
464        return Err(FusionError::DimensionMismatch {
465            field: "matrix_b",
466            expected: inner,
467            actual: b.len(),
468        });
469    }
470    let cols = b[0].len();
471    validate_matrix_cols(b, cols, "matrix_b")?;
472    let mut out = vec![vec![0.0; cols]; a.len()];
473    for row in 0..a.len() {
474        for col in 0..cols {
475            let mut sum = 0.0;
476            for k in 0..inner {
477                sum += a[row][k] * b[k][col];
478            }
479            out[row][col] = sum;
480        }
481    }
482    Ok(out)
483}
484
485pub(crate) fn matvec(matrix: &[Vec<f64>], vector: &[f64]) -> Result<Vec<f64>, FusionError> {
486    if matrix.is_empty() {
487        return Err(invalid_input("matrix", "must not be empty"));
488    }
489    let cols = vector.len();
490    validate_matrix_cols(matrix, cols, "matrix")?;
491    validate_finite_slice(vector, "vector")?;
492    let mut out = vec![0.0; matrix.len()];
493    for row in 0..matrix.len() {
494        let mut sum = 0.0;
495        for (col, value) in vector.iter().enumerate() {
496            sum += matrix[row][col] * value;
497        }
498        out[row] = sum;
499    }
500    Ok(out)
501}
502
503pub(crate) fn matrix_add(a: &[Vec<f64>], b: &[Vec<f64>]) -> Result<Vec<Vec<f64>>, FusionError> {
504    same_shape(a, b, "matrix_add")?;
505    let mut out = vec![vec![0.0; a[0].len()]; a.len()];
506    for row in 0..a.len() {
507        for col in 0..a[0].len() {
508            out[row][col] = a[row][col] + b[row][col];
509        }
510    }
511    Ok(out)
512}
513
514pub(crate) fn matrix_sub(a: &[Vec<f64>], b: &[Vec<f64>]) -> Result<Vec<Vec<f64>>, FusionError> {
515    same_shape(a, b, "matrix_sub")?;
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_scale(a: &[Vec<f64>]) -> f64 {
526    a.iter()
527        .flat_map(|row| row.iter())
528        .fold(0.0_f64, |acc, value| acc.max(value.abs()))
529}
530
531#[allow(clippy::needless_range_loop)]
532pub(crate) fn symmetrize_in_place(matrix: &mut [Vec<f64>]) {
533    let dimension = matrix.len();
534    for row in 0..dimension {
535        for col in (row + 1)..dimension {
536            let value = 0.5 * (matrix[row][col] + matrix[col][row]);
537            matrix[row][col] = value;
538            matrix[col][row] = value;
539        }
540    }
541}
542
543pub(crate) fn solve_spd(
544    matrix: &[Vec<f64>],
545    rhs: &[f64],
546    scratch: &mut crate::astro::math::linear::FlatCholeskySolveScratch,
547) -> Result<Vec<f64>, FusionError> {
548    validate_square_matrix(matrix, rhs.len(), "spd_matrix")?;
549    validate_finite_slice(rhs, "spd_rhs")?;
550    let flat = flatten(matrix);
551    crate::astro::math::linear::solve_flat_normal_square_root_into(&flat, rhs, scratch)
552        .map(<[f64]>::to_vec)
553        .ok_or(FusionError::SingularInnovation)
554}
555
556pub(crate) fn flatten(matrix: &[Vec<f64>]) -> Vec<f64> {
557    let rows = matrix.len();
558    let cols = if rows == 0 { 0 } else { matrix[0].len() };
559    let mut out = Vec::with_capacity(rows * cols);
560    for row in matrix {
561        out.extend(row);
562    }
563    out
564}
565
566pub(crate) fn dmatrix_from_rows(rows: &[Vec<f64>]) -> DMatrix<f64> {
567    let nrows = rows.len();
568    let ncols = if nrows == 0 { 0 } else { rows[0].len() };
569    DMatrix::from_row_slice(nrows, ncols, &flatten(rows))
570}
571
572fn same_shape(a: &[Vec<f64>], b: &[Vec<f64>], field: &'static str) -> Result<(), FusionError> {
573    if a.is_empty() || b.is_empty() {
574        return Err(invalid_input(field, "must not be empty"));
575    }
576    validate_matrix_cols(a, a[0].len(), field)?;
577    validate_matrix_cols(b, b[0].len(), field)?;
578    if a.len() != b.len() {
579        return Err(FusionError::DimensionMismatch {
580            field,
581            expected: a.len(),
582            actual: b.len(),
583        });
584    }
585    if a[0].len() != b[0].len() {
586        return Err(FusionError::DimensionMismatch {
587            field,
588            expected: a[0].len(),
589            actual: b[0].len(),
590        });
591    }
592    Ok(())
593}
594
595fn psd_tolerance(dimension: usize, scale: f64) -> f64 {
596    let dimension_scale = dimension.max(1) as f64;
597    PSD_REL_TOLERANCE * dimension_scale * scale
598}
599
600#[cfg(test)]
601mod tests {
602    //! Provenance: PSD validation tests lock the fusion safety contract that
603    //! invalid covariance input remains flagged instead of being repaired into a
604    //! false covariance.
605
606    use super::*;
607
608    #[test]
609    fn zero_covariance_is_psd() {
610        let covariance = vec![vec![0.0]];
611        assert!(covariance_is_positive_semidefinite(&covariance).expect("psd"));
612    }
613
614    #[test]
615    fn tiny_negative_variance_is_rejected_not_repaired() {
616        let covariance = vec![vec![-1.0e-15]];
617        assert!(!covariance_is_positive_semidefinite(&covariance).expect("psd"));
618
619        let mut covariance = covariance;
620        let err = reproject_covariance_psd(&mut covariance, "covariance")
621            .expect_err("negative variance must remain flagged");
622        assert!(matches!(
623            err,
624            FusionError::NonPositiveSemidefinite {
625                field: "covariance"
626            }
627        ));
628        assert_eq!(covariance[0][0].to_bits(), (-1.0e-15_f64).to_bits());
629    }
630}
sidereon_core/fusion/state.rs

sidereon_core/fusion/
state.rs
