kinavis_ins/
imu.rs

1//! IMU samples and noise model.
2
3use core::time::Duration;
4
5use kinavis_kernel::error::{ensure_finite, ensure_range, Result};
6use kinavis_kernel::math;
7
8/// One IMU sample: angular rate and specific force in the body frame, averaged
9/// over the interval since the previous sample.
10///
11/// Angular rate about forward, right, down in rad/s; specific force along them
12/// in m/s². At rest the specific force is minus gravity, about `[0, 0, −9.8]`
13/// (down positive).
14#[derive(Debug, Clone, Copy, PartialEq)]
15#[cfg_attr(
16    feature = "serde",
17    derive(serde::Serialize, serde::Deserialize),
18    serde(try_from = "StoredImuSample", into = "StoredImuSample")
19)]
20pub struct ImuSample {
21    angular_rate: [f64; 3],
22    specific_force: [f64; 3],
23    over: Duration,
24}
25
26impl ImuSample {
27    /// Sample from angular rate, specific force and interval.
28    ///
29    /// # Errors
30    ///
31    /// [`kinavis_kernel::KernelError::NotFinite`] for a non-finite component;
32    /// [`kinavis_kernel::KernelError::OutOfRange`] for an interval of zero or
33    /// over one second.
34    pub fn new(angular_rate: [f64; 3], specific_force: [f64; 3], over: Duration) -> Result<Self> {
35        for value in angular_rate {
36            ensure_finite("angular rate", value)?;
37        }
38        for value in specific_force {
39            ensure_finite("specific force", value)?;
40        }
41        ensure_range("IMU interval", over.as_secs_f64(), 1e-6, 1.0)?;
42        Ok(Self {
43            angular_rate,
44            specific_force,
45            over,
46        })
47    }
48
49    /// Angular rate about forward, right, down, rad/s.
50    #[must_use]
51    pub const fn angular_rate(&self) -> [f64; 3] {
52        self.angular_rate
53    }
54
55    /// Specific force along forward, right, down, m/s².
56    #[must_use]
57    pub const fn specific_force(&self) -> [f64; 3] {
58        self.specific_force
59    }
60
61    /// Sample interval.
62    #[must_use]
63    pub const fn over(&self) -> Duration {
64        self.over
65    }
66
67    /// Sample interval, s.
68    pub(crate) fn seconds(&self) -> f64 {
69        self.over.as_secs_f64()
70    }
71}
72
73/// IMU error model from the datasheet: white noise per channel and bias random
74/// walk.
75///
76/// White noise is given as random walk (1σ angle or velocity accumulated over 1
77/// s); bias instability is modelled as a random walk (1σ change per √s), as
78/// used by the error-state filter.
79#[derive(Debug, Clone, Copy, PartialEq)]
80#[cfg_attr(
81    feature = "serde",
82    derive(serde::Serialize, serde::Deserialize),
83    serde(try_from = "StoredImuNoise", into = "StoredImuNoise")
84)]
85pub struct ImuNoise {
86    /// rad/√s.
87    arw: f64,
88    /// m/s/√s.
89    vrw: f64,
90    /// rad/s/√s.
91    gyro_bias: f64,
92    /// m/s²/√s.
93    accel_bias: f64,
94}
95
96impl ImuNoise {
97    /// Noise model from four 1σ-per-√s figures: angle random walk (rad),
98    /// velocity random walk (m/s), gyro bias walk (rad/s), accelerometer bias
99    /// walk (m/s²).
100    ///
101    /// # Errors
102    ///
103    /// [`kinavis_kernel::KernelError::OutOfRange`] for a figure that is not
104    /// positive and finite.
105    pub fn new(
106        angle_random_walk: f64,
107        velocity_random_walk: f64,
108        gyro_bias_walk: f64,
109        accel_bias_walk: f64,
110    ) -> Result<Self> {
111        ensure_range(
112            "angle random walk",
113            angle_random_walk,
114            f64::MIN_POSITIVE,
115            1.0,
116        )?;
117        ensure_range(
118            "velocity random walk",
119            velocity_random_walk,
120            f64::MIN_POSITIVE,
121            10.0,
122        )?;
123        ensure_range("gyro bias walk", gyro_bias_walk, f64::MIN_POSITIVE, 1.0)?;
124        ensure_range(
125            "accelerometer bias walk",
126            accel_bias_walk,
127            f64::MIN_POSITIVE,
128            10.0,
129        )?;
130        Ok(Self {
131            arw: angle_random_walk,
132            vrw: velocity_random_walk,
133            gyro_bias: gyro_bias_walk,
134            accel_bias: accel_bias_walk,
135        })
136    }
137
138    /// Consumer MEMS: ARW 0.3°/√h, VRW 0.1 m/s/√h, bias walk 10°/h and 1 mg per
139    /// hour.
140    #[must_use]
141    pub fn mems() -> Self {
142        Self {
143            arw: math::to_radians(0.3) / 60.0,
144            vrw: 0.1 / 60.0,
145            gyro_bias: math::to_radians(10.0) / 3600.0 / 60.0,
146            accel_bias: 1e-3 * 9.80665 / 60.0,
147        }
148    }
149
150    /// Tactical-grade FOG: ARW 0.05°/√h, VRW 0.03 m/s/√h, bias walk 1°/h and
151    /// 0.1 mg per hour.
152    #[must_use]
153    pub fn tactical() -> Self {
154        Self {
155            arw: math::to_radians(0.05) / 60.0,
156            vrw: 0.03 / 60.0,
157            gyro_bias: math::to_radians(1.0) / 3600.0 / 60.0,
158            accel_bias: 1e-4 * 9.80665 / 60.0,
159        }
160    }
161
162    /// Angle random walk, rad/√s.
163    #[must_use]
164    pub const fn angle_random_walk(&self) -> f64 {
165        self.arw
166    }
167
168    /// Velocity random walk, m/s/√s.
169    #[must_use]
170    pub const fn velocity_random_walk(&self) -> f64 {
171        self.vrw
172    }
173
174    /// Gyro bias walk, rad/s/√s.
175    #[must_use]
176    pub const fn gyro_bias_walk(&self) -> f64 {
177        self.gyro_bias
178    }
179
180    /// Accelerometer bias walk, m/s²/√s.
181    #[must_use]
182    pub const fn accel_bias_walk(&self) -> f64 {
183        self.accel_bias
184    }
185}
186
187/// Serialised form. Deserialisation goes through [`ImuSample::new`], rejecting
188/// `NaN` rates and invalid intervals.
189#[cfg(feature = "serde")]
190#[derive(serde::Serialize, serde::Deserialize)]
191struct StoredImuSample {
192    angular_rate: [f64; 3],
193    specific_force: [f64; 3],
194    over: Duration,
195}
196
197#[cfg(feature = "serde")]
198impl TryFrom<StoredImuSample> for ImuSample {
199    type Error = kinavis_kernel::KernelError;
200
201    fn try_from(stored: StoredImuSample) -> Result<Self> {
202        Self::new(stored.angular_rate, stored.specific_force, stored.over)
203    }
204}
205
206#[cfg(feature = "serde")]
207impl From<ImuSample> for StoredImuSample {
208    fn from(sample: ImuSample) -> Self {
209        Self {
210            angular_rate: sample.angular_rate,
211            specific_force: sample.specific_force,
212            over: sample.over,
213        }
214    }
215}
216
217/// Serialised form. Deserialisation goes through [`ImuNoise::new`], rejecting
218/// zero or negative figures (singular noise matrix).
219#[cfg(feature = "serde")]
220#[derive(serde::Serialize, serde::Deserialize)]
221struct StoredImuNoise {
222    arw: f64,
223    vrw: f64,
224    gyro_bias: f64,
225    accel_bias: f64,
226}
227
228#[cfg(feature = "serde")]
229impl TryFrom<StoredImuNoise> for ImuNoise {
230    type Error = kinavis_kernel::KernelError;
231
232    fn try_from(stored: StoredImuNoise) -> Result<Self> {
233        Self::new(stored.arw, stored.vrw, stored.gyro_bias, stored.accel_bias)
234    }
235}
236
237#[cfg(feature = "serde")]
238impl From<ImuNoise> for StoredImuNoise {
239    fn from(noise: ImuNoise) -> Self {
240        Self {
241            arw: noise.arw,
242            vrw: noise.vrw,
243            gyro_bias: noise.gyro_bias,
244            accel_bias: noise.accel_bias,
245        }
246    }
247}
248
249#[cfg(test)]
250mod tests {
251    #![allow(clippy::unwrap_used, clippy::float_cmp)]
252
253    use super::*;
254
255    #[test]
256    fn a_sample_is_finite_and_of_a_sensible_interval() {
257        let sample = ImuSample::new(
258            [0.0, 0.0, 0.01],
259            [0.0, 0.0, -9.8],
260            Duration::from_millis(10),
261        )
262        .unwrap();
263        assert_eq!(sample.angular_rate(), [0.0, 0.0, 0.01]);
264        assert_eq!(sample.specific_force(), [0.0, 0.0, -9.8]);
265        assert_eq!(sample.over(), Duration::from_millis(10));
266        assert!(ImuSample::new([f64::NAN, 0.0, 0.0], [0.0; 3], Duration::from_millis(10)).is_err());
267        assert!(ImuSample::new(
268            [0.0; 3],
269            [0.0, f64::INFINITY, 0.0],
270            Duration::from_millis(10)
271        )
272        .is_err());
273        assert!(ImuSample::new([0.0; 3], [0.0; 3], Duration::ZERO).is_err());
274        assert!(ImuSample::new([0.0; 3], [0.0; 3], Duration::from_secs(2)).is_err());
275    }
276
277    #[test]
278    fn the_presets_are_in_the_units_the_sheets_use() {
279        let mems = ImuNoise::mems();
280        // 0.3°/√h = 0.005°/√s.
281        assert!((math::to_degrees(mems.angle_random_walk()) - 0.005).abs() < 1e-12);
282        assert!(ImuNoise::tactical().angle_random_walk() < mems.angle_random_walk());
283        assert!(ImuNoise::tactical().gyro_bias_walk() < mems.gyro_bias_walk());
284        assert!(ImuNoise::new(1e-4, 1e-3, 1e-7, 1e-5).is_ok());
285        assert!(ImuNoise::new(0.0, 1e-3, 1e-7, 1e-5).is_err());
286        assert!(ImuNoise::new(1e-4, f64::NAN, 1e-7, 1e-5).is_err());
287        let custom = ImuNoise::new(1e-4, 1e-3, 1e-7, 1e-5).unwrap();
288        assert_eq!(custom.velocity_random_walk(), 1e-3);
289        assert_eq!(custom.accel_bias_walk(), 1e-5);
290    }
291}
kinavis_ins/imu.rs

kinavis_ins/
imu.rs
