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

sidereon_core/inertial/
mod.rs
