sidereon_core/inertial/
mod.rs

1//! Inertial navigation primitives for ECEF strapdown propagation.
2//!
3//! This module contains the frame, mechanization, and IMU error-model pieces
4//! needed before the GNSS update layers are introduced. It owns no I/O and has
5//! no feature-gated behavior.
6
7pub mod config;
8pub mod frames;
9pub mod imu;
10pub mod mechanization;
11pub mod state;
12
13pub use config::{
14    gauss_markov_bias_decay, gauss_markov_bias_variance_increment, ConingCorrection, ImuGrade,
15    ImuSpec, MechanizationConfig,
16};
17pub use frames::{
18    gravity_ecef_mps2, normal_gravity_mps2, WGS84_NORMAL_GRAVITY_EQUATOR_MPS2,
19    WGS84_NORMAL_GRAVITY_POLE_MPS2, WGS84_SOMIGLIANA_K,
20};
21pub use imu::{
22    CorrectedImuIncrement, ImuBias, ImuCalibration, ImuErrorModel, ImuSample, ImuSampleKind,
23};
24pub use mechanization::{mechanize_ecef, rodrigues_delta_dcm, StrapdownMechanizer};
25pub use state::{
26    attitude_yaw_pitch_roll_rad, dcm_to_quaternion, quaternion_to_dcm, reorthonormalize_dcm,
27    AttitudeQuaternion, NavState,
28};
29
30/// Error returned by inertial frame, IMU, and mechanization entry points.
31#[derive(Debug, Clone, Copy, PartialEq, Eq, thiserror::Error)]
32pub enum InertialError {
33    /// A public input was non-finite or outside its documented domain.
34    #[error("invalid inertial input {field}: {reason}")]
35    InvalidInput {
36        /// Name of the invalid field.
37        field: &'static str,
38        /// Short reason suitable for logs and tests.
39        reason: &'static str,
40    },
41    /// A sample timestamp did not advance from the previous state timestamp.
42    #[error("IMU sample time must be strictly increasing")]
43    NonMonotonicSample,
44    /// A scale or misalignment matrix could not be inverted.
45    #[error("IMU calibration matrix is singular")]
46    SingularCalibration,
47    /// An attitude matrix could not be normalized into a right-handed frame.
48    #[error("attitude matrix is degenerate")]
49    DegenerateAttitude,
50}
51
52pub(crate) const fn invalid_input(field: &'static str, reason: &'static str) -> InertialError {
53    InertialError::InvalidInput { field, reason }
54}
55
56pub(crate) fn validate_finite(value: f64, field: &'static str) -> Result<(), InertialError> {
57    if value.is_finite() {
58        Ok(())
59    } else {
60        Err(invalid_input(field, "must be finite"))
61    }
62}
63
64pub(crate) fn validate_nonnegative(value: f64, field: &'static str) -> Result<(), InertialError> {
65    validate_finite(value, field)?;
66    if value >= 0.0 {
67        Ok(())
68    } else {
69        Err(invalid_input(field, "must be non-negative"))
70    }
71}
72
73pub(crate) fn validate_positive(value: f64, field: &'static str) -> Result<(), InertialError> {
74    validate_finite(value, field)?;
75    if value > 0.0 {
76        Ok(())
77    } else {
78        Err(invalid_input(field, "must be positive"))
79    }
80}
81
82pub(crate) fn validate_vec3(value: [f64; 3], field: &'static str) -> Result<(), InertialError> {
83    for component in value {
84        validate_finite(component, field)?;
85    }
86    Ok(())
87}
88
89pub(crate) fn validate_mat3(
90    value: &[[f64; 3]; 3],
91    field: &'static str,
92) -> Result<(), InertialError> {
93    for row in value {
94        for component in row {
95            validate_finite(*component, field)?;
96        }
97    }
98    Ok(())
99}
sidereon_core/inertial/mod.rs

sidereon_core/inertial/
mod.rs
