nabled_sensor/
imu.rs

1//! IMU strapdown integration.
2
3use nabled_core::scalar::NabledReal;
4use nabled_linalg::geometry::{Quat, quat};
5use ndarray::{Array1, ArrayView1};
6
7use crate::SensorError;
8
9/// Integrate orientation with gyro `[wx, wy, wz]` over `dt` (quaternion `[w,x,y,z]`).
10pub fn strapdown_predict<T: NabledReal>(
11    orientation: &Array1<T>,
12    gyro: &Array1<T>,
13    dt: T,
14) -> Result<Array1<T>, SensorError> {
15    strapdown_predict_view(&orientation.view(), &gyro.view(), dt)
16}
17
18/// Integrate orientation with gyro from joint views (no copy on input).
19pub fn strapdown_predict_view<T: NabledReal>(
20    orientation: &ArrayView1<'_, T>,
21    gyro: &ArrayView1<'_, T>,
22    dt: T,
23) -> Result<Array1<T>, SensorError> {
24    let mut out = orientation.to_owned();
25    strapdown_predict_view_into(orientation, gyro, dt, &mut out)?;
26    Ok(out)
27}
28
29pub fn strapdown_predict_into<T: NabledReal>(
30    orientation: &Array1<T>,
31    gyro: &Array1<T>,
32    dt: T,
33    output: &mut Array1<T>,
34) -> Result<(), SensorError> {
35    strapdown_predict_view_into(&orientation.view(), &gyro.view(), dt, output)
36}
37
38/// Integrate orientation in-place from joint views and write to `output`.
39pub fn strapdown_predict_view_into<T: NabledReal>(
40    orientation: &ArrayView1<'_, T>,
41    gyro: &ArrayView1<'_, T>,
42    dt: T,
43    output: &mut Array1<T>,
44) -> Result<(), SensorError> {
45    if orientation.len() != 4 || gyro.len() != 3 || output.len() != 4 {
46        return Err(SensorError::DimensionMismatch);
47    }
48    let q = Quat { w: orientation[0], x: orientation[1], y: orientation[2], z: orientation[3] };
49    let delta = quat::exp(&ndarray::arr1(&[gyro[0] * dt, gyro[1] * dt, gyro[2] * dt]).view());
50    let next = quat::mul(&delta, &q);
51    let normalized = quat::normalize(&next);
52    output[0] = normalized.w;
53    output[1] = normalized.x;
54    output[2] = normalized.y;
55    output[3] = normalized.z;
56    Ok(())
57}
58
59#[cfg(test)]
60mod tests {
61    use approx::assert_relative_eq;
62    use nabled_linalg::geometry::{AxisAngle, quat};
63
64    use super::*;
65
66    #[test]
67    fn strapdown_matches_small_angle_exp() {
68        let q0 = quat::identity::<f64>();
69        let orientation = ndarray::arr1(&[q0.w, q0.x, q0.y, q0.z]);
70        let gyro = ndarray::arr1(&[0.0_f64, 0.0, 0.1]);
71        let dt = 0.01;
72        let predicted = strapdown_predict(&orientation, &gyro, dt).unwrap();
73        let expected = quat::from_axis_angle(&AxisAngle { axis: [0.0, 0.0, 1.0], angle: 0.001 });
74        assert_relative_eq!(predicted[0], expected.w, epsilon = 1e-6);
75        assert_relative_eq!(predicted[3], expected.z, epsilon = 1e-6);
76    }
77}
nabled_sensor/imu.rs

nabled_sensor/
imu.rs
