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::{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        reorthonormalize_dcm(&self.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/// Re-orthonormalize a direction-cosine matrix into a right-handed frame.
187pub fn reorthonormalize_dcm(dcm: &Mat3) -> Result<Mat3, InertialError> {
188    validate_mat3(dcm, "dcm")?;
189    let c0 = [dcm[0][0], dcm[1][0], dcm[2][0]];
190    let c1_raw = [dcm[0][1], dcm[1][1], dcm[2][1]];
191    let e0 = unit(c0)?;
192    let c1_orth = sub3(c1_raw, scale3(e0, dot3(e0, c1_raw)));
193    let e1 = unit(c1_orth)?;
194    let e2 = unit(cross3(e0, e1))?;
195    let e1 = unit(cross3(e2, e0))?;
196    Ok([
197        [e0[0], e1[0], e2[0]],
198        [e0[1], e1[1], e2[1]],
199        [e0[2], e1[2], e2[2]],
200    ])
201}
202
203pub(crate) fn mat3_identity() -> Mat3 {
204    [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
205}
206
207pub(crate) fn mat3_add(a: &Mat3, b: &Mat3) -> Mat3 {
208    let mut out = [[0.0; 3]; 3];
209    for i in 0..3 {
210        for j in 0..3 {
211            out[i][j] = a[i][j] + b[i][j];
212        }
213    }
214    out
215}
216
217pub(crate) fn mat3_scale(a: &Mat3, scale: f64) -> Mat3 {
218    let mut out = [[0.0; 3]; 3];
219    for i in 0..3 {
220        for j in 0..3 {
221            out[i][j] = a[i][j] * scale;
222        }
223    }
224    out
225}
226
227pub(crate) fn mat3_mul_vec(a: &Mat3, v: [f64; 3]) -> [f64; 3] {
228    mul_vec3(a, v)
229}
230
231pub(crate) fn mat3_mul(a: &Mat3, b: &Mat3) -> Mat3 {
232    inline_rxr(a, b)
233}
234
235pub(crate) fn skew(v: [f64; 3]) -> Mat3 {
236    [[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]]
237}
238
239fn unit(v: [f64; 3]) -> Result<[f64; 3], InertialError> {
240    let norm = norm3(v);
241    if norm > 0.0 {
242        Ok(scale3(v, 1.0 / norm))
243    } else {
244        Err(InertialError::DegenerateAttitude)
245    }
246}
247
248#[cfg(test)]
249mod tests {
250    use super::*;
251
252    #[test]
253    fn quaternion_roundtrip_preserves_right_angle_yaw() {
254        let half = core::f64::consts::FRAC_1_SQRT_2;
255        let q = AttitudeQuaternion::new(half, 0.0, 0.0, half).expect("quaternion");
256        let dcm = quaternion_to_dcm(q);
257        let ypr = attitude_yaw_pitch_roll_rad(&dcm);
258        assert!((ypr[0] - core::f64::consts::FRAC_PI_2).abs() <= 4.0e-16);
259        assert!(ypr[1].abs() <= 1.0e-16);
260        assert!(ypr[2].abs() <= 1.0e-16);
261
262        let back = dcm_to_quaternion(&dcm).expect("back");
263        assert!((back.w - half).abs() <= 2.0e-16);
264        assert!((back.z - half).abs() <= 2.0e-16);
265    }
266}
sidereon_core/inertial/state.rs

sidereon_core/inertial/
state.rs
