sidereon_core/inertial/
mechanization.rs

1//! ECEF strapdown inertial mechanization.
2
3use crate::astro::constants::earth::OMEGA_E_DOT_RAD_S;
4use crate::astro::math::mat3::Mat3;
5use crate::astro::math::vec3::{add3, dot3, scale3};
6
7use super::config::MechanizationConfig;
8use super::frames::gravity_ecef_mps2;
9use super::imu::{CorrectedImuIncrement, ImuErrorModel, ImuSample};
10use super::state::{
11    mat3_add, mat3_identity, mat3_mul, mat3_mul_vec, mat3_scale, reorthonormalize_dcm, skew,
12    NavState,
13};
14use super::{validate_finite, validate_vec3, InertialError};
15
16/// Streaming ECEF strapdown mechanizer.
17#[derive(Debug, Clone, Copy, PartialEq)]
18pub struct StrapdownMechanizer {
19    state: NavState,
20    imu_model: ImuErrorModel,
21    config: MechanizationConfig,
22}
23
24impl StrapdownMechanizer {
25    /// Build a mechanizer from an initial navigation state.
26    pub fn new(state: NavState) -> Result<Self, InertialError> {
27        state.validate()?;
28        Ok(Self {
29            state,
30            imu_model: ImuErrorModel::default(),
31            config: MechanizationConfig::default(),
32        })
33    }
34
35    /// Return a copy with a different IMU error model.
36    pub fn with_imu_model(mut self, imu_model: ImuErrorModel) -> Result<Self, InertialError> {
37        imu_model.bias.validate()?;
38        imu_model.calibration.validate()?;
39        self.imu_model = imu_model;
40        Ok(self)
41    }
42
43    /// Return a copy with a different mechanization config.
44    pub const fn with_config(mut self, config: MechanizationConfig) -> Self {
45        self.config = config;
46        self
47    }
48
49    /// Current navigation state.
50    pub const fn state(&self) -> &NavState {
51        &self.state
52    }
53
54    /// Propagate the state with one raw IMU sample.
55    pub fn propagate(&mut self, sample: ImuSample) -> Result<&NavState, InertialError> {
56        let increment = self
57            .imu_model
58            .correct_sample(&sample, self.state.t_j2000_s)?;
59        self.state = mechanize_ecef(&self.state, &increment, self.config)?;
60        Ok(&self.state)
61    }
62}
63
64/// Propagate an ECEF navigation state by one corrected IMU increment.
65pub fn mechanize_ecef(
66    state: &NavState,
67    increment: &CorrectedImuIncrement,
68    _config: MechanizationConfig,
69) -> Result<NavState, InertialError> {
70    state.validate()?;
71    validate_increment(increment)?;
72    if increment.t_j2000_s <= state.t_j2000_s {
73        return Err(InertialError::NonMonotonicSample);
74    }
75
76    let dt_s = increment.dt_s;
77    let body_delta = rodrigues_delta_dcm(increment.delta_theta_rad)?;
78    let earth_delta = earth_rotation_first_order(dt_s);
79    let attitude_raw = mat3_mul(
80        &mat3_mul(&earth_delta, &state.attitude_body_to_ecef),
81        &body_delta,
82    );
83    let attitude_body_to_ecef = reorthonormalize_dcm(&attitude_raw)?;
84
85    let c_avg = mid_interval_dcm(
86        &state.attitude_body_to_ecef,
87        increment.delta_theta_rad,
88        dt_s,
89    )?;
90    let delta_v_ecef = mat3_mul_vec(&c_avg, increment.delta_velocity_mps);
91    let gravity = gravity_ecef_mps2(state.position_ecef_m)?;
92    let coriolis = scale3(earth_rate_cross(state.velocity_ecef_mps), -2.0);
93    let acceleration = add3(gravity, coriolis);
94    let velocity_ecef_mps = add3(
95        add3(state.velocity_ecef_mps, delta_v_ecef),
96        scale3(acceleration, dt_s),
97    );
98    let avg_velocity = scale3(add3(state.velocity_ecef_mps, velocity_ecef_mps), 0.5);
99    let position_ecef_m = add3(state.position_ecef_m, scale3(avg_velocity, dt_s));
100
101    NavState {
102        t_j2000_s: increment.t_j2000_s,
103        position_ecef_m,
104        velocity_ecef_mps,
105        attitude_body_to_ecef,
106        accel_bias_mps2: state.accel_bias_mps2,
107        gyro_bias_rps: state.gyro_bias_rps,
108    }
109    .with_biases(state.accel_bias_mps2, state.gyro_bias_rps)
110}
111
112/// Exact Rodrigues direction-cosine matrix for a body delta angle.
113pub fn rodrigues_delta_dcm(delta_theta_rad: [f64; 3]) -> Result<Mat3, InertialError> {
114    validate_vec3(delta_theta_rad, "delta_theta_rad")?;
115    let phi2 = dot3(delta_theta_rad, delta_theta_rad);
116    let phi = phi2.sqrt();
117    let phi4 = phi2 * phi2;
118    let (a, b) = if phi < 1.0e-8 {
119        (
120            1.0 - phi2 / 6.0 + phi4 / 120.0,
121            0.5 - phi2 / 24.0 + phi4 / 720.0,
122        )
123    } else {
124        (phi.sin() / phi, (1.0 - phi.cos()) / phi2)
125    };
126    let k = skew(delta_theta_rad);
127    let k2 = mat3_mul(&k, &k);
128    Ok(mat3_add(
129        &mat3_add(&mat3_identity(), &mat3_scale(&k, a)),
130        &mat3_scale(&k2, b),
131    ))
132}
133
134pub(crate) fn validate_increment(increment: &CorrectedImuIncrement) -> Result<(), InertialError> {
135    validate_finite(increment.t_j2000_s, "increment.t_j2000_s")?;
136    validate_vec3(increment.delta_velocity_mps, "increment.delta_velocity_mps")?;
137    validate_vec3(increment.delta_theta_rad, "increment.delta_theta_rad")?;
138    validate_finite(increment.dt_s, "increment.dt_s")?;
139    if increment.dt_s > 0.0 {
140        Ok(())
141    } else {
142        Err(super::invalid_input("increment.dt_s", "must be positive"))
143    }
144}
145
146pub(crate) fn mid_interval_dcm(
147    attitude_body_to_ecef: &Mat3,
148    delta_theta_rad: [f64; 3],
149    dt_s: f64,
150) -> Result<Mat3, InertialError> {
151    let earth_half = earth_rotation_first_order(0.5 * dt_s);
152    let body_half = rodrigues_delta_dcm(scale3(delta_theta_rad, 0.5))?;
153    reorthonormalize_dcm(&mat3_mul(
154        &mat3_mul(&earth_half, attitude_body_to_ecef),
155        &body_half,
156    ))
157}
158
159pub(crate) fn earth_rotation_first_order(dt_s: f64) -> Mat3 {
160    [
161        [1.0, OMEGA_E_DOT_RAD_S * dt_s, 0.0],
162        [-OMEGA_E_DOT_RAD_S * dt_s, 1.0, 0.0],
163        [0.0, 0.0, 1.0],
164    ]
165}
166
167pub(crate) fn earth_rate_cross(v: [f64; 3]) -> [f64; 3] {
168    [-OMEGA_E_DOT_RAD_S * v[1], OMEGA_E_DOT_RAD_S * v[0], 0.0]
169}
170
171#[cfg(test)]
172mod tests {
173    //! Provenance: mechanization tests follow Groves, Principles of GNSS,
174    //! Inertial, and Multisensor Integrated Navigation Systems, 2nd ed.,
175    //! Chapter 5.4. The synthetic trajectory tests invert the implemented ECEF
176    //! mechanization equations for the exact body-frame increments that produce
177    //! the analytic target state.
178
179    use super::*;
180    use crate::astro::constants::earth::{WGS84_A_M, WGS84_F};
181    use crate::astro::math::mat3::inline_tr;
182    use crate::astro::math::vec3::sub3;
183
184    fn assert_close(actual: f64, expected: f64, tolerance: f64) {
185        assert!(
186            (actual - expected).abs() <= tolerance,
187            "actual {actual:.17e}, expected {expected:.17e}, tolerance {tolerance:.17e}"
188        );
189    }
190
191    fn assert_vec_close(actual: [f64; 3], expected: [f64; 3], tolerance: f64) {
192        for i in 0..3 {
193            assert_close(actual[i], expected[i], tolerance);
194        }
195    }
196
197    fn inverse_delta_velocity(
198        state: &NavState,
199        target_velocity_ecef_mps: [f64; 3],
200        delta_theta_rad: [f64; 3],
201        dt_s: f64,
202    ) -> [f64; 3] {
203        let c_avg = mid_interval_dcm(&state.attitude_body_to_ecef, delta_theta_rad, dt_s)
204            .expect("mid-interval dcm");
205        let c_avg_t = inline_tr(&c_avg);
206        let gravity = gravity_ecef_mps2(state.position_ecef_m).expect("gravity");
207        let coriolis = scale3(earth_rate_cross(state.velocity_ecef_mps), -2.0);
208        let acceleration = add3(gravity, coriolis);
209        let required_ecef = sub3(
210            sub3(target_velocity_ecef_mps, state.velocity_ecef_mps),
211            scale3(acceleration, dt_s),
212        );
213        mat3_mul_vec(&c_avg_t, required_ecef)
214    }
215
216    #[test]
217    fn rodrigues_hits_right_angle_rotation() {
218        let dcm = rodrigues_delta_dcm([0.0, 0.0, core::f64::consts::FRAC_PI_2]).expect("rodrigues");
219        assert_close(dcm[0][0], 0.0, 1.2e-16);
220        assert_close(dcm[0][1], -1.0, 0.0);
221        assert_close(dcm[1][0], 1.0, 0.0);
222        assert_close(dcm[1][1], 0.0, 1.2e-16);
223        assert_close(dcm[2][2], 1.0, 0.0);
224    }
225
226    #[test]
227    fn attitude_mechanization_matches_closed_form_update() {
228        let state =
229            NavState::new(0.0, [WGS84_A_M, 0.0, 0.0], [0.0; 3], mat3_identity()).expect("state");
230        let dt_s = 0.25;
231        let delta_theta_rad = [0.0, 0.0, 0.9];
232        let delta_velocity_mps = inverse_delta_velocity(&state, [0.0; 3], delta_theta_rad, dt_s);
233        let increment = CorrectedImuIncrement {
234            t_j2000_s: dt_s,
235            delta_velocity_mps,
236            delta_theta_rad,
237            dt_s,
238        };
239        let propagated =
240            mechanize_ecef(&state, &increment, MechanizationConfig::default()).expect("step");
241
242        let expected = reorthonormalize_dcm(&mat3_mul(
243            &earth_rotation_first_order(dt_s),
244            &rodrigues_delta_dcm(delta_theta_rad).expect("delta dcm"),
245        ))
246        .expect("expected");
247        for (i, row) in expected.iter().enumerate() {
248            for (j, expected_value) in row.iter().enumerate() {
249                assert_close(
250                    propagated.attitude_body_to_ecef[i][j],
251                    *expected_value,
252                    3.0e-16,
253                );
254            }
255        }
256    }
257
258    #[test]
259    fn inverted_static_trajectory_holds_velocity_and_position() {
260        let mut state =
261            NavState::new(0.0, [WGS84_A_M, 0.0, 0.0], [0.0; 3], mat3_identity()).expect("state");
262        let dt_s = 0.01;
263        for step in 1..=200 {
264            let delta_theta_rad = [0.0; 3];
265            let delta_velocity_mps =
266                inverse_delta_velocity(&state, [0.0; 3], delta_theta_rad, dt_s);
267            let increment = CorrectedImuIncrement {
268                t_j2000_s: step as f64 * dt_s,
269                delta_velocity_mps,
270                delta_theta_rad,
271                dt_s,
272            };
273            state =
274                mechanize_ecef(&state, &increment, MechanizationConfig::default()).expect("step");
275        }
276        assert_vec_close(state.velocity_ecef_mps, [0.0; 3], 2.0e-14);
277        assert_vec_close(state.position_ecef_m, [WGS84_A_M, 0.0, 0.0], 2.0e-13);
278    }
279
280    #[test]
281    fn inverted_constant_acceleration_trajectory_hits_position() {
282        let b_m = WGS84_A_M * (1.0 - WGS84_F);
283        let state = NavState::new(
284            100.0,
285            [4_510_000.0, 120_000.0, b_m / 2.0],
286            [12.0, -4.0, 1.5],
287            mat3_identity(),
288        )
289        .expect("state");
290        let dt_s = 0.2;
291        let target_velocity = [12.3, -4.1, 1.55];
292        let delta_theta_rad = [0.01, -0.02, 0.03];
293        let delta_velocity_mps =
294            inverse_delta_velocity(&state, target_velocity, delta_theta_rad, dt_s);
295        let increment = CorrectedImuIncrement {
296            t_j2000_s: 100.2,
297            delta_velocity_mps,
298            delta_theta_rad,
299            dt_s,
300        };
301        let propagated =
302            mechanize_ecef(&state, &increment, MechanizationConfig::default()).expect("step");
303        let expected_position = add3(
304            state.position_ecef_m,
305            scale3(add3(state.velocity_ecef_mps, target_velocity), 0.5 * dt_s),
306        );
307        assert_vec_close(propagated.velocity_ecef_mps, target_velocity, 2.0e-14);
308        assert_vec_close(propagated.position_ecef_m, expected_position, 3.0e-10);
309    }
310}
sidereon_core/inertial/mechanization.rs

sidereon_core/inertial/
mechanization.rs
