sidereon_core/inertial/
state.rs

1//! Navigation state and attitude conversion helpers.
2
3use crate::astro::math::mat3::{inline_rxr, mul_vec3, Mat3};
4use crate::astro::math::vec3::{cross3, dot3, norm3, scale3, sub3};
5
6use super::{invalid_input, validate_finite, validate_mat3, validate_vec3, InertialError};
7
8/// Navigation state used by the ECEF strapdown mechanizer.
9#[derive(Debug, Clone, Copy, PartialEq)]
10pub struct NavState {
11    /// State time in seconds since J2000 on the caller's GNSS time scale.
12    pub t_j2000_s: f64,
13    /// IMU position in ECEF meters.
14    pub position_ecef_m: [f64; 3],
15    /// IMU velocity in ECEF meters per second.
16    pub velocity_ecef_mps: [f64; 3],
17    /// Direction cosine matrix from body axes to ECEF axes.
18    pub attitude_body_to_ecef: Mat3,
19    /// Closed-loop accelerometer bias estimate in m/s^2.
20    pub accel_bias_mps2: [f64; 3],
21    /// Closed-loop gyroscope bias estimate in rad/s.
22    pub gyro_bias_rps: [f64; 3],
23}
24
25impl NavState {
26    /// Build a navigation state with zero IMU biases.
27    pub fn new(
28        t_j2000_s: f64,
29        position_ecef_m: [f64; 3],
30        velocity_ecef_mps: [f64; 3],
31        attitude_body_to_ecef: Mat3,
32    ) -> Result<Self, InertialError> {
33        let state = Self {
34            t_j2000_s,
35            position_ecef_m,
36            velocity_ecef_mps,
37            attitude_body_to_ecef,
38            accel_bias_mps2: [0.0; 3],
39            gyro_bias_rps: [0.0; 3],
40        };
41        state.validate()?;
42        Ok(state)
43    }
44
45    /// Return this state with closed-loop IMU bias estimates.
46    pub fn with_biases(
47        mut self,
48        accel_bias_mps2: [f64; 3],
49        gyro_bias_rps: [f64; 3],
50    ) -> Result<Self, InertialError> {
51        self.accel_bias_mps2 = accel_bias_mps2;
52        self.gyro_bias_rps = gyro_bias_rps;
53        self.validate()?;
54        Ok(self)
55    }
56
57    /// Validate finite state fields and a nondegenerate attitude matrix.
58    pub fn validate(&self) -> Result<(), InertialError> {
59        validate_finite(self.t_j2000_s, "t_j2000_s")?;
60        validate_vec3(self.position_ecef_m, "position_ecef_m")?;
61        validate_vec3(self.velocity_ecef_mps, "velocity_ecef_mps")?;
62        validate_mat3(&self.attitude_body_to_ecef, "attitude_body_to_ecef")?;
63        validate_vec3(self.accel_bias_mps2, "accel_bias_mps2")?;
64        validate_vec3(self.gyro_bias_rps, "gyro_bias_rps")?;
65        validate_dcm_orthonormal(&self.attitude_body_to_ecef, "attitude_body_to_ecef")?;
66        Ok(())
67    }
68
69    /// Body-to-ECEF attitude as a unit quaternion.
70    pub fn attitude_quaternion_body_to_ecef(&self) -> Result<AttitudeQuaternion, InertialError> {
71        dcm_to_quaternion(&self.attitude_body_to_ecef)
72    }
73
74    /// Body-to-ECEF attitude as yaw, pitch, and roll angles in radians.
75    pub fn attitude_yaw_pitch_roll_rad(&self) -> [f64; 3] {
76        attitude_yaw_pitch_roll_rad(&self.attitude_body_to_ecef)
77    }
78}
79
80/// Unit quaternion using scalar-first `(w, x, y, z)` storage.
81#[derive(Debug, Clone, Copy, PartialEq)]
82pub struct AttitudeQuaternion {
83    /// Scalar component.
84    pub w: f64,
85    /// X vector component.
86    pub x: f64,
87    /// Y vector component.
88    pub y: f64,
89    /// Z vector component.
90    pub z: f64,
91}
92
93impl AttitudeQuaternion {
94    /// Build and normalize a quaternion.
95    pub fn new(w: f64, x: f64, y: f64, z: f64) -> Result<Self, InertialError> {
96        validate_finite(w, "quaternion.w")?;
97        validate_finite(x, "quaternion.x")?;
98        validate_finite(y, "quaternion.y")?;
99        validate_finite(z, "quaternion.z")?;
100        let norm = (w * w + x * x + y * y + z * z).sqrt();
101        if norm <= 0.0 {
102            return Err(InertialError::DegenerateAttitude);
103        }
104        Ok(Self {
105            w: w / norm,
106            x: x / norm,
107            y: y / norm,
108            z: z / norm,
109        })
110    }
111}
112
113/// Convert a body-to-ECEF DCM to a unit quaternion.
114pub fn dcm_to_quaternion(dcm: &Mat3) -> Result<AttitudeQuaternion, InertialError> {
115    validate_mat3(dcm, "dcm")?;
116    let dcm = reorthonormalize_dcm(dcm)?;
117    let trace = dcm[0][0] + dcm[1][1] + dcm[2][2];
118    if trace > 0.0 {
119        let s = (trace + 1.0).sqrt() * 2.0;
120        AttitudeQuaternion::new(
121            0.25 * s,
122            (dcm[2][1] - dcm[1][2]) / s,
123            (dcm[0][2] - dcm[2][0]) / s,
124            (dcm[1][0] - dcm[0][1]) / s,
125        )
126    } else if dcm[0][0] > dcm[1][1] && dcm[0][0] > dcm[2][2] {
127        let s = (1.0 + dcm[0][0] - dcm[1][1] - dcm[2][2]).sqrt() * 2.0;
128        AttitudeQuaternion::new(
129            (dcm[2][1] - dcm[1][2]) / s,
130            0.25 * s,
131            (dcm[0][1] + dcm[1][0]) / s,
132            (dcm[0][2] + dcm[2][0]) / s,
133        )
134    } else if dcm[1][1] > dcm[2][2] {
135        let s = (1.0 + dcm[1][1] - dcm[0][0] - dcm[2][2]).sqrt() * 2.0;
136        AttitudeQuaternion::new(
137            (dcm[0][2] - dcm[2][0]) / s,
138            (dcm[0][1] + dcm[1][0]) / s,
139            0.25 * s,
140            (dcm[1][2] + dcm[2][1]) / s,
141        )
142    } else {
143        let s = (1.0 + dcm[2][2] - dcm[0][0] - dcm[1][1]).sqrt() * 2.0;
144        AttitudeQuaternion::new(
145            (dcm[1][0] - dcm[0][1]) / s,
146            (dcm[0][2] + dcm[2][0]) / s,
147            (dcm[1][2] + dcm[2][1]) / s,
148            0.25 * s,
149        )
150    }
151}
152
153/// Convert a unit quaternion to a body-to-ECEF DCM.
154pub fn quaternion_to_dcm(quaternion: AttitudeQuaternion) -> Mat3 {
155    let w = quaternion.w;
156    let x = quaternion.x;
157    let y = quaternion.y;
158    let z = quaternion.z;
159    [
160        [
161            1.0 - 2.0 * (y * y + z * z),
162            2.0 * (x * y - z * w),
163            2.0 * (x * z + y * w),
164        ],
165        [
166            2.0 * (x * y + z * w),
167            1.0 - 2.0 * (x * x + z * z),
168            2.0 * (y * z - x * w),
169        ],
170        [
171            2.0 * (x * z - y * w),
172            2.0 * (y * z + x * w),
173            1.0 - 2.0 * (x * x + y * y),
174        ],
175    ]
176}
177
178/// Convert a body-to-ECEF DCM to yaw, pitch, roll radians using a ZYX sequence.
179pub fn attitude_yaw_pitch_roll_rad(dcm: &Mat3) -> [f64; 3] {
180    let pitch = (-dcm[2][0]).asin();
181    let roll = dcm[2][1].atan2(dcm[2][2]);
182    let yaw = dcm[1][0].atan2(dcm[0][0]);
183    [yaw, pitch, roll]
184}
185
186/// Maximum elementwise departure of `C * C^T` from identity accepted by
187/// [`validate_dcm_orthonormal`]. Numerical attitude drift under per-step
188/// renormalization sits many orders of magnitude below this; scaled, sheared,
189/// or otherwise non-orthonormal frames are rejected rather than repaired.
190pub(crate) const DCM_ORTHONORMALITY_TOL: f64 = 1.0e-6;
191
192/// Validate that `dcm` is an orthonormal, right-handed rotation within
193/// [`DCM_ORTHONORMALITY_TOL`]. Rejects inputs that only become rotations
194/// after re-orthonormalization, so callers cannot feed a degenerate attitude
195/// and receive a precise result.
196pub(crate) fn validate_dcm_orthonormal(
197    dcm: &Mat3,
198    field: &'static str,
199) -> Result<(), InertialError> {
200    validate_mat3(dcm, field)?;
201    for i in 0..3 {
202        for j in 0..3 {
203            let mut g = 0.0;
204            for row in dcm {
205                g += row[i] * row[j];
206            }
207            let expected = if i == j { 1.0 } else { 0.0 };
208            if (g - expected).abs() > DCM_ORTHONORMALITY_TOL {
209                return Err(invalid_input(field, "matrix is not orthonormal"));
210            }
211        }
212    }
213    let det = dcm[0][0] * (dcm[1][1] * dcm[2][2] - dcm[1][2] * dcm[2][1])
214        - dcm[0][1] * (dcm[1][0] * dcm[2][2] - dcm[1][2] * dcm[2][0])
215        + dcm[0][2] * (dcm[1][0] * dcm[2][1] - dcm[1][1] * dcm[2][0]);
216    if det <= 0.5 {
217        return Err(invalid_input(
218            field,
219            "matrix is not a right-handed rotation",
220        ));
221    }
222    Ok(())
223}
224
225/// Re-orthonormalize a direction-cosine matrix into a right-handed frame.
226pub fn reorthonormalize_dcm(dcm: &Mat3) -> Result<Mat3, InertialError> {
227    validate_mat3(dcm, "dcm")?;
228    let c0 = [dcm[0][0], dcm[1][0], dcm[2][0]];
229    let c1_raw = [dcm[0][1], dcm[1][1], dcm[2][1]];
230    let e0 = unit(c0)?;
231    let c1_orth = sub3(c1_raw, scale3(e0, dot3(e0, c1_raw)));
232    let e1 = unit(c1_orth)?;
233    let e2 = unit(cross3(e0, e1))?;
234    let e1 = unit(cross3(e2, e0))?;
235    Ok([
236        [e0[0], e1[0], e2[0]],
237        [e0[1], e1[1], e2[1]],
238        [e0[2], e1[2], e2[2]],
239    ])
240}
241
242pub(crate) fn mat3_identity() -> Mat3 {
243    [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
244}
245
246pub(crate) fn mat3_add(a: &Mat3, b: &Mat3) -> Mat3 {
247    let mut out = [[0.0; 3]; 3];
248    for i in 0..3 {
249        for j in 0..3 {
250            out[i][j] = a[i][j] + b[i][j];
251        }
252    }
253    out
254}
255
256pub(crate) fn mat3_scale(a: &Mat3, scale: f64) -> Mat3 {
257    let mut out = [[0.0; 3]; 3];
258    for i in 0..3 {
259        for j in 0..3 {
260            out[i][j] = a[i][j] * scale;
261        }
262    }
263    out
264}
265
266pub(crate) fn mat3_mul_vec(a: &Mat3, v: [f64; 3]) -> [f64; 3] {
267    mul_vec3(a, v)
268}
269
270pub(crate) fn mat3_mul(a: &Mat3, b: &Mat3) -> Mat3 {
271    inline_rxr(a, b)
272}
273
274pub(crate) fn skew(v: [f64; 3]) -> Mat3 {
275    [[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]]
276}
277
278fn unit(v: [f64; 3]) -> Result<[f64; 3], InertialError> {
279    let norm = norm3(v);
280    if norm > 0.0 {
281        Ok(scale3(v, 1.0 / norm))
282    } else {
283        Err(InertialError::DegenerateAttitude)
284    }
285}
286
287#[cfg(test)]
288mod tests {
289    use super::*;
290
291    #[test]
292    fn quaternion_roundtrip_preserves_right_angle_yaw() {
293        let half = core::f64::consts::FRAC_1_SQRT_2;
294        let q = AttitudeQuaternion::new(half, 0.0, 0.0, half).expect("quaternion");
295        let dcm = quaternion_to_dcm(q);
296        let ypr = attitude_yaw_pitch_roll_rad(&dcm);
297        assert!((ypr[0] - core::f64::consts::FRAC_PI_2).abs() <= 4.0e-16);
298        assert!(ypr[1].abs() <= 1.0e-16);
299        assert!(ypr[2].abs() <= 1.0e-16);
300
301        let back = dcm_to_quaternion(&dcm).expect("back");
302        assert!((back.w - half).abs() <= 2.0e-16);
303        assert!((back.z - half).abs() <= 2.0e-16);
304    }
305}
306
307#[cfg(test)]
308mod orthonormality_tests {
309    //! Regression for the review finding that a scaled non-rotation attitude
310    //! was accepted and silently repaired instead of rejected.
311    use super::*;
312
313    fn state_with_attitude(attitude: Mat3) -> NavState {
314        NavState {
315            t_j2000_s: 0.0,
316            position_ecef_m: [6_378_137.0, 0.0, 0.0],
317            velocity_ecef_mps: [0.0, 0.0, 0.0],
318            attitude_body_to_ecef: attitude,
319            accel_bias_mps2: [0.0, 0.0, 0.0],
320            gyro_bias_rps: [0.0, 0.0, 0.0],
321        }
322    }
323
324    #[test]
325    fn scaled_non_rotation_attitude_is_rejected() {
326        let scaled = [[2.0, 0.0, 0.0], [0.0, 3.0, 0.0], [0.0, 0.0, 4.0]];
327        assert!(validate_dcm_orthonormal(&scaled, "attitude").is_err());
328        assert!(state_with_attitude(scaled).validate().is_err());
329    }
330
331    #[test]
332    fn left_handed_frame_is_rejected() {
333        let reflected = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, -1.0]];
334        assert!(validate_dcm_orthonormal(&reflected, "attitude").is_err());
335    }
336
337    #[test]
338    fn identity_and_small_drift_are_accepted() {
339        assert!(validate_dcm_orthonormal(&mat3_identity(), "attitude").is_ok());
340        let mut drifted = mat3_identity();
341        drifted[0][0] += 1.0e-9;
342        assert!(validate_dcm_orthonormal(&drifted, "attitude").is_ok());
343    }
344}
sidereon_core/inertial/state.rs

sidereon_core/inertial/
state.rs
