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.
37    /// * `initial` - the starting estimate.
38    ///
39    /// # Returns
40    ///
41    /// A filter seeded with `initial`.
42    pub fn new(alpha: f32, initial: f32) -> Self {
43        Self {
44            estimate: initial,
45            alpha: unit_interval(alpha),
46        }
47    }
48
49    /// Fuses a rate and an absolute reading over a time step and returns the new estimate.
50    ///
51    /// # Arguments
52    ///
53    /// * `rate` - the rate of change, such as degrees per second from a gyroscope.
54    /// * `absolute` - the absolute reading, such as a tilt from an accelerometer.
55    /// * `dt` - the time since the previous update.
56    ///
57    /// # Returns
58    ///
59    /// The fused estimate, `alpha * (estimate + rate * dt) + (1 - alpha) * absolute`.
60    pub fn update(&mut self, rate: f32, absolute: f32, dt: f32) -> f32 {
61        let integrated = self.estimate + rate * dt;
62        self.estimate = self.alpha * integrated + (1.0 - self.alpha) * absolute;
63        self.estimate
64    }
65
66    /// Returns the current fused estimate.
67    pub fn estimate(&self) -> f32 {
68        self.estimate
69    }
70}
71
72// `f32::clamp` lives in `std`, so this `no_std` crate clamps by hand.
73#[allow(clippy::manual_clamp)]
74fn unit_interval(value: f32) -> f32 {
75    if value < 0.0 {
76        0.0
77    } else if value > 1.0 {
78        1.0
79    } else {
80        value
81    }
82}
83
84#[cfg(test)]
85mod tests {
86    use super::*;
87
88    #[test]
89    fn alpha_one_integrates_the_rate_only() {
90        let mut filter = Complementary::new(1.0, 0.0);
91        assert!((filter.update(10.0, 999.0, 1.0) - 10.0).abs() < 1e-6); // ignores absolute
92        assert!((filter.update(10.0, 999.0, 1.0) - 20.0).abs() < 1e-6);
93    }
94
95    #[test]
96    fn alpha_zero_follows_the_absolute_reading() {
97        let mut filter = Complementary::new(0.0, 0.0);
98        assert!((filter.update(10.0, 3.0, 1.0) - 3.0).abs() < 1e-6);
99    }
100
101    #[test]
102    fn it_blends_rate_and_absolute() {
103        let mut filter = Complementary::new(0.9, 0.0);
104        // integrated = 0 + 2 * 1 = 2; 0.9 * 2 + 0.1 * 0 = 1.8
105        assert!((filter.update(2.0, 0.0, 1.0) - 1.8).abs() < 1e-6);
106    }
107
108    #[test]
109    fn alpha_is_clamped_to_the_unit_interval() {
110        let mut filter = Complementary::new(5.0, 0.0); // clamps to 1.0
111        assert!((filter.update(4.0, 100.0, 1.0) - 4.0).abs() < 1e-6);
112    }
113}
pamoja_kit/complementary.rs

pamoja_kit/
complementary.rs
