sidereon_core/inertial/
config.rs

1//! Configuration and stochastic error parameters for inertial propagation.
2
3use super::{invalid_input, validate_nonnegative, validate_positive, InertialError};
4
5/// Sentinel time constant for a random-walk bias model.
6pub const RANDOM_WALK_BIAS_TAU_S: f64 = f64::INFINITY;
7
8/// Coarse IMU class used to select a built-in [`ImuSpec`] preset.
9#[derive(Debug, Clone, Copy, PartialEq, Eq)]
10pub enum ImuGrade {
11    /// Low-cost MEMS class.
12    Mems,
13    /// Tactical class.
14    Tactical,
15    /// Navigation class.
16    Navigation,
17}
18
19/// Datasheet-level IMU stochastic parameters.
20///
21/// Noise densities are one-sigma values in SI units per square-root second.
22/// Bias instabilities are one-sigma bias magnitudes. A bias time constant of
23/// [`RANDOM_WALK_BIAS_TAU_S`] selects the random-walk limit.
24#[derive(Debug, Clone, Copy, PartialEq)]
25pub struct ImuSpec {
26    /// Accelerometer velocity random walk in m/s per square-root second.
27    pub accel_vrw_mps_sqrt_s: f64,
28    /// Gyroscope angular random walk in rad per square-root second.
29    pub gyro_arw_rad_sqrt_s: f64,
30    /// Accelerometer bias instability in m/s^2.
31    pub accel_bias_instab_mps2: f64,
32    /// Gyroscope bias instability in rad/s.
33    pub gyro_bias_instab_rps: f64,
34    /// Accelerometer first-order Gauss-Markov bias time constant in seconds.
35    pub accel_bias_tau_s: f64,
36    /// Gyroscope first-order Gauss-Markov bias time constant in seconds.
37    pub gyro_bias_tau_s: f64,
38    /// Optional accelerometer scale instability in parts per million.
39    pub accel_scale_instab_ppm: Option<f64>,
40    /// Optional gyroscope scale instability in parts per million.
41    pub gyro_scale_instab_ppm: Option<f64>,
42}
43
44impl ImuSpec {
45    /// Build an IMU specification from datasheet values.
46    #[allow(clippy::too_many_arguments)]
47    pub const fn datasheet(
48        accel_vrw_mps_sqrt_s: f64,
49        gyro_arw_rad_sqrt_s: f64,
50        accel_bias_instab_mps2: f64,
51        gyro_bias_instab_rps: f64,
52        accel_bias_tau_s: f64,
53        gyro_bias_tau_s: f64,
54        accel_scale_instab_ppm: Option<f64>,
55        gyro_scale_instab_ppm: Option<f64>,
56    ) -> Self {
57        Self {
58            accel_vrw_mps_sqrt_s,
59            gyro_arw_rad_sqrt_s,
60            accel_bias_instab_mps2,
61            gyro_bias_instab_rps,
62            accel_bias_tau_s,
63            gyro_bias_tau_s,
64            accel_scale_instab_ppm,
65            gyro_scale_instab_ppm,
66        }
67    }
68
69    /// Return the built-in preset for an IMU grade.
70    pub const fn preset(grade: ImuGrade) -> Self {
71        match grade {
72            ImuGrade::Mems => Self::mems(),
73            ImuGrade::Tactical => Self::tactical(),
74            ImuGrade::Navigation => Self::navigation(),
75        }
76    }
77
78    /// Representative low-cost MEMS preset.
79    pub const fn mems() -> Self {
80        Self::datasheet(
81            5.0e-2,
82            5.0e-3,
83            5.0e-2,
84            5.0e-4,
85            3_600.0,
86            3_600.0,
87            Some(500.0),
88            Some(500.0),
89        )
90    }
91
92    /// Representative tactical preset.
93    pub const fn tactical() -> Self {
94        Self::datasheet(
95            5.0e-3,
96            2.0e-4,
97            5.0e-3,
98            2.0e-5,
99            7_200.0,
100            7_200.0,
101            Some(50.0),
102            Some(50.0),
103        )
104    }
105
106    /// Representative navigation preset.
107    pub const fn navigation() -> Self {
108        Self::datasheet(
109            5.0e-4,
110            2.0e-5,
111            2.0e-4,
112            1.0e-6,
113            14_400.0,
114            14_400.0,
115            Some(5.0),
116            Some(5.0),
117        )
118    }
119
120    /// Validate finite, non-negative stochastic parameters and valid bias time constants.
121    pub fn validate(&self) -> Result<(), InertialError> {
122        validate_nonnegative(self.accel_vrw_mps_sqrt_s, "accel_vrw_mps_sqrt_s")?;
123        validate_nonnegative(self.gyro_arw_rad_sqrt_s, "gyro_arw_rad_sqrt_s")?;
124        validate_nonnegative(self.accel_bias_instab_mps2, "accel_bias_instab_mps2")?;
125        validate_nonnegative(self.gyro_bias_instab_rps, "gyro_bias_instab_rps")?;
126        validate_bias_tau(self.accel_bias_tau_s, "accel_bias_tau_s")?;
127        validate_bias_tau(self.gyro_bias_tau_s, "gyro_bias_tau_s")?;
128        if let Some(scale) = self.accel_scale_instab_ppm {
129            validate_nonnegative(scale, "accel_scale_instab_ppm")?;
130        }
131        if let Some(scale) = self.gyro_scale_instab_ppm {
132            validate_nonnegative(scale, "gyro_scale_instab_ppm")?;
133        }
134        Ok(())
135    }
136
137    /// Accelerometer bias decay over `dt_s`.
138    pub fn accel_bias_decay(&self, dt_s: f64) -> Result<f64, InertialError> {
139        gauss_markov_bias_decay(dt_s, self.accel_bias_tau_s)
140    }
141
142    /// Gyroscope bias decay over `dt_s`.
143    pub fn gyro_bias_decay(&self, dt_s: f64) -> Result<f64, InertialError> {
144        gauss_markov_bias_decay(dt_s, self.gyro_bias_tau_s)
145    }
146
147    /// Accelerometer bias variance increment over `dt_s`.
148    pub fn accel_bias_variance_increment(&self, dt_s: f64) -> Result<f64, InertialError> {
149        gauss_markov_bias_variance_increment(
150            self.accel_bias_instab_mps2,
151            dt_s,
152            self.accel_bias_tau_s,
153        )
154    }
155
156    /// Gyroscope bias variance increment over `dt_s`.
157    pub fn gyro_bias_variance_increment(&self, dt_s: f64) -> Result<f64, InertialError> {
158        gauss_markov_bias_variance_increment(self.gyro_bias_instab_rps, dt_s, self.gyro_bias_tau_s)
159    }
160}
161
162/// Strapdown coning-correction setting.
163#[derive(Debug, Clone, Copy, PartialEq, Eq)]
164pub enum ConingCorrection {
165    /// Do not apply coning correction.
166    Off,
167}
168
169/// Configuration for ECEF strapdown mechanization.
170#[derive(Debug, Clone, Copy, PartialEq, Eq)]
171pub struct MechanizationConfig {
172    /// Coning correction mode. The default is off.
173    pub coning_correction: ConingCorrection,
174}
175
176impl Default for MechanizationConfig {
177    fn default() -> Self {
178        Self {
179            coning_correction: ConingCorrection::Off,
180        }
181    }
182}
183
184/// First-order Gauss-Markov bias decay factor over `dt_s`.
185///
186/// Infinite `tau_s` returns the random-walk limit decay of `1.0`.
187pub fn gauss_markov_bias_decay(dt_s: f64, tau_s: f64) -> Result<f64, InertialError> {
188    validate_nonnegative(dt_s, "dt_s")?;
189    validate_bias_tau(tau_s, "tau_s")?;
190    if tau_s.is_infinite() {
191        Ok(1.0)
192    } else {
193        Ok((-dt_s / tau_s).exp())
194    }
195}
196
197/// Bias process variance increment for a Gauss-Markov bias over `dt_s`.
198///
199/// For finite `tau_s`, `instability` is treated as the stationary one-sigma
200/// bias. For infinite `tau_s`, it is treated as the random-walk one-sigma
201/// density, giving `instability^2 * dt_s`.
202pub fn gauss_markov_bias_variance_increment(
203    instability: f64,
204    dt_s: f64,
205    tau_s: f64,
206) -> Result<f64, InertialError> {
207    validate_nonnegative(instability, "instability")?;
208    validate_nonnegative(dt_s, "dt_s")?;
209    validate_bias_tau(tau_s, "tau_s")?;
210    let variance = instability * instability;
211    if tau_s.is_infinite() {
212        Ok(variance * dt_s)
213    } else {
214        Ok(variance * (1.0 - (-2.0 * dt_s / tau_s).exp()))
215    }
216}
217
218fn validate_bias_tau(value: f64, field: &'static str) -> Result<(), InertialError> {
219    if value.is_infinite() && value.is_sign_positive() {
220        Ok(())
221    } else if value.is_finite() {
222        validate_positive(value, field)
223    } else {
224        Err(invalid_input(field, "must be positive or infinite"))
225    }
226}
227
228#[cfg(test)]
229mod tests {
230    //! Provenance: IMU stochastic model follows Groves, Principles of GNSS,
231    //! Inertial, and Multisensor Integrated Navigation Systems, 2nd ed.,
232    //! Chapter 4.4. The test pins the closed-form Gauss-Markov transition and
233    //! the random-walk limit used by the public helpers.
234
235    use super::*;
236
237    #[test]
238    fn gauss_markov_decay_matches_closed_form() {
239        let decay = gauss_markov_bias_decay(12.5, 100.0).expect("decay");
240        assert_eq!(decay.to_bits(), (-0.125_f64).exp().to_bits());
241        assert_eq!(
242            gauss_markov_bias_decay(12.5, RANDOM_WALK_BIAS_TAU_S)
243                .expect("random walk")
244                .to_bits(),
245            1.0_f64.to_bits()
246        );
247    }
248
249    #[test]
250    fn gauss_markov_variance_matches_closed_form() {
251        let increment = gauss_markov_bias_variance_increment(0.02, 10.0, 200.0).expect("variance");
252        let expected = 0.02_f64 * 0.02 * (1.0 - (-0.1_f64).exp());
253        assert_eq!(increment.to_bits(), expected.to_bits());
254
255        let random_walk = gauss_markov_bias_variance_increment(0.02, 10.0, RANDOM_WALK_BIAS_TAU_S)
256            .expect("random-walk variance");
257        assert_eq!(random_walk.to_bits(), (0.02_f64 * 0.02 * 10.0).to_bits());
258    }
259
260    #[test]
261    fn presets_validate() {
262        ImuSpec::preset(ImuGrade::Mems).validate().expect("mems");
263        ImuSpec::preset(ImuGrade::Tactical)
264            .validate()
265            .expect("tactical");
266        ImuSpec::preset(ImuGrade::Navigation)
267            .validate()
268            .expect("navigation");
269    }
270}
sidereon_core/inertial/config.rs

sidereon_core/inertial/
config.rs
