pamoja_kit/
complementary.rs

1//! Fusing a fast rate sensor with a slow absolute one.
2
3/// Blends a drifting rate measurement with a noisy absolute one into a steady estimate.
4///
5/// A gyroscope gives a smooth rate of turn but drifts over time; an accelerometer gives an
6/// absolute tilt that is right on average but noisy. A complementary filter trusts the rate
7/// over the short term and the absolute reading over the long term, so the result is both
8/// smooth and drift-free. Each step it integrates the rate onto its estimate, then nudges
9/// that toward the absolute reading by an amount set by `alpha`. The same filter fuses any
10/// fast-rate-plus-slow-absolute pair, not only an IMU.
11///
12/// # Examples
13///
14/// ```
15/// use pamoja_kit::Complementary;
16///
17/// // Heavily trust the integrated rate, lightly correct toward the absolute reading.
18/// let mut tilt = Complementary::new(0.98, 0.0);
19/// // The rate reads +10 per second for 0.1 s while the absolute reads about 1.
20/// let angle = tilt.update(10.0, 1.0, 0.1);
21/// assert!((angle - 1.0).abs() < 0.05); // about 0.98 * 1 + 0.02 * 1
22/// ```
23#[derive(Clone, Copy, Debug)]
24pub struct Complementary {
25    estimate: f32,
26    alpha: f32,
27}
28
29impl Complementary {
30    /// Creates a filter.
31    ///
32    /// # Arguments
33    ///
34    /// * `alpha` - the weight on the integrated rate, in `[0.0, 1.0]`; near `1.0` trusts the
35    ///   rate and corrects slowly, near `0.0` follows the absolute reading. Clamped to the
36    ///   unit interval, and one that is not a number is taken as `0.0`, which follows the
37    ///   absolute reading since that one cannot drift.
38    /// * `initial` - the starting estimate.
39    ///
40    /// # Returns
41    ///
42    /// A filter seeded with `initial`.
43    pub fn new(alpha: f32, initial: f32) -> Self {
44        Self {
45            estimate: initial,
46            alpha: unit_interval(alpha),
47        }
48    }
49
50    /// Fuses a rate and an absolute reading over a time step and returns the new estimate.
51    ///
52    /// If the rate, the absolute reading, or the time step is not a finite number, such as the
53    /// NaN a failed sensor reports, the update is ignored and the estimate returned as it stood.
54    ///
55    /// # Arguments
56    ///
57    /// * `rate` - the rate of change, such as degrees per second from a gyroscope.
58    /// * `absolute` - the absolute reading, such as a tilt from an accelerometer.
59    /// * `dt` - the time since the previous update.
60    ///
61    /// # Returns
62    ///
63    /// The fused estimate, `alpha * (estimate + rate * dt) + (1 - alpha) * absolute`.
64    pub fn update(&mut self, rate: f32, absolute: f32, dt: f32) -> f32 {
65        if !rate.is_finite() || !absolute.is_finite() || !dt.is_finite() {
66            return self.estimate;
67        }
68        let integrated = self.estimate + rate * dt;
69        self.estimate = self.alpha * integrated + (1.0 - self.alpha) * absolute;
70        self.estimate
71    }
72
73    /// Returns the current fused estimate.
74    pub fn estimate(&self) -> f32 {
75        self.estimate
76    }
77}
78
79// `f32::clamp` lives in `std`, so this `no_std` crate clamps by hand.
80#[allow(clippy::manual_clamp)]
81fn unit_interval(value: f32) -> f32 {
82    if value.is_nan() || value < 0.0 {
83        0.0
84    } else if value > 1.0 {
85        1.0
86    } else {
87        value
88    }
89}
90
91#[cfg(test)]
92mod tests {
93    use super::*;
94
95    #[test]
96    fn alpha_one_integrates_the_rate_only() {
97        let mut filter = Complementary::new(1.0, 0.0);
98        assert!((filter.update(10.0, 999.0, 1.0) - 10.0).abs() < 1e-6); // ignores absolute
99        assert!((filter.update(10.0, 999.0, 1.0) - 20.0).abs() < 1e-6);
100    }
101
102    #[test]
103    fn alpha_zero_follows_the_absolute_reading() {
104        let mut filter = Complementary::new(0.0, 0.0);
105        assert!((filter.update(10.0, 3.0, 1.0) - 3.0).abs() < 1e-6);
106    }
107
108    #[test]
109    fn it_blends_rate_and_absolute() {
110        let mut filter = Complementary::new(0.9, 0.0);
111        // integrated = 0 + 2 * 1 = 2; 0.9 * 2 + 0.1 * 0 = 1.8
112        assert!((filter.update(2.0, 0.0, 1.0) - 1.8).abs() < 1e-6);
113    }
114
115    #[test]
116    fn alpha_is_clamped_to_the_unit_interval() {
117        let mut filter = Complementary::new(5.0, 0.0); // clamps to 1.0
118        assert!((filter.update(4.0, 100.0, 1.0) - 4.0).abs() < 1e-6);
119    }
120
121    #[test]
122    fn an_input_that_is_not_a_number_is_ignored() {
123        let mut tilt = Complementary::new(0.98, 0.0);
124        let settled = tilt.update(10.0, 1.0, 0.1);
125        assert_eq!(tilt.update(f32::NAN, 1.0, 0.1), settled);
126        assert_eq!(tilt.update(10.0, f32::INFINITY, 0.1), settled);
127        assert_eq!(tilt.update(10.0, 1.0, f32::NAN), settled);
128        assert!(tilt.update(10.0, 1.0, 0.1).is_finite());
129    }
130
131    #[test]
132    fn an_alpha_that_is_not_a_number_follows_the_absolute_reading() {
133        let mut tilt = Complementary::new(f32::NAN, 0.0);
134        assert_eq!(tilt.update(10.0, 3.0, 0.1), 3.0);
135    }
136}
pamoja_kit/complementary.rs

pamoja_kit/
complementary.rs
