pamoja_kit/
kalman.rs

1//! Getting a steady value from a jittery sensor.
2
3/// Estimates a steady value from noisy readings with a one-dimensional Kalman filter.
4///
5/// Where [`Smoother`](crate::Smoother) blends with a fixed weight, a Kalman filter sets the
6/// blend from how much it trusts its estimate versus each reading, so it settles quickly and
7/// then holds steady. It tracks an estimate and its uncertainty: each step it grows the
8/// uncertainty by the process noise (how much the true value may drift between readings),
9/// then pulls the estimate toward the new reading by a gain derived from that uncertainty
10/// and the measurement noise (how noisy the sensor is). It suits a slowly changing quantity
11/// read by a noisy sensor: a battery voltage, a tank level, a temperature.
12///
13/// # Examples
14///
15/// ```
16/// use pamoja_kit::Kalman;
17///
18/// // Low process noise, higher measurement noise: trust history, smooth hard.
19/// let mut level = Kalman::new(0.01, 1.0, 0.0);
20/// let mut value = 0.0;
21/// for reading in [10.0, 9.0, 11.0, 10.0, 10.0] {
22///     value = level.update(reading);
23/// }
24/// assert!((value - 10.0).abs() < 1.0); // settles near the true 10
25/// ```
26#[derive(Clone, Copy, Debug)]
27pub struct Kalman {
28    estimate: f32,
29    error: f32,
30    process: f32,
31    measurement: f32,
32    started: bool,
33}
34
35impl Kalman {
36    /// Creates a filter.
37    ///
38    /// # Arguments
39    ///
40    /// * `process_noise` - how much the true value may change between readings; larger
41    ///   tracks faster, smaller smooths harder. Its magnitude is used.
42    /// * `measurement_noise` - how noisy each reading is; larger trusts readings less. Its
43    ///   magnitude is used.
44    /// * `initial` - the starting estimate, used until the first reading replaces it.
45    ///
46    /// # Returns
47    ///
48    /// A filter awaiting its first reading.
49    pub fn new(process_noise: f32, measurement_noise: f32, initial: f32) -> Self {
50        Self {
51            estimate: initial,
52            error: 1.0,
53            process: magnitude(process_noise),
54            measurement: magnitude(measurement_noise),
55            started: false,
56        }
57    }
58
59    /// Folds in a reading and returns the updated estimate.
60    ///
61    /// The first reading seeds the estimate and is returned unchanged.
62    ///
63    /// # Arguments
64    ///
65    /// * `reading` - the latest measurement.
66    ///
67    /// # Returns
68    ///
69    /// The filtered estimate after this reading.
70    pub fn update(&mut self, reading: f32) -> f32 {
71        if !self.started {
72            self.estimate = reading;
73            self.started = true;
74            return self.estimate;
75        }
76        let predicted_error = self.error + self.process;
77        let gain = predicted_error / (predicted_error + self.measurement);
78        self.estimate += gain * (reading - self.estimate);
79        self.error = (1.0 - gain) * predicted_error;
80        self.estimate
81    }
82
83    /// Returns the current estimate.
84    pub fn estimate(&self) -> f32 {
85        self.estimate
86    }
87}
88
89// `f32::abs` lives in `std`, so this `no_std` crate takes the magnitude by hand.
90fn magnitude(value: f32) -> f32 {
91    if value < 0.0 {
92        -value
93    } else {
94        value
95    }
96}
97
98#[cfg(test)]
99mod tests {
100    use super::*;
101
102    #[test]
103    fn the_first_reading_seeds_the_estimate() {
104        let mut kalman = Kalman::new(0.1, 0.1, 0.0);
105        assert_eq!(kalman.update(42.0), 42.0);
106    }
107
108    #[test]
109    fn it_tracks_toward_a_new_level() {
110        let mut kalman = Kalman::new(0.1, 1.0, 0.0);
111        kalman.update(0.0); // seed at zero
112        let mut value = 0.0;
113        for _ in 0..50 {
114            value = kalman.update(10.0);
115        }
116        assert!((value - 10.0).abs() < 0.5);
117    }
118
119    #[test]
120    fn it_smooths_a_noisy_signal_toward_the_mean() {
121        let mut kalman = Kalman::new(0.01, 1.0, 0.0);
122        let mut value = 0.0;
123        for reading in [10.0, 8.0, 12.0, 9.0, 11.0, 10.0, 10.0, 9.5, 10.5, 10.0] {
124            value = kalman.update(reading);
125        }
126        assert!((value - 10.0).abs() < 1.0);
127    }
128
129    #[test]
130    fn zero_measurement_noise_follows_the_reading() {
131        let mut kalman = Kalman::new(1.0, 0.0, 0.0);
132        kalman.update(5.0); // seeds
133        assert_eq!(kalman.update(8.0), 8.0); // full trust in the reading
134    }
135}
pamoja_kit/kalman.rs

pamoja_kit/
kalman.rs
