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, and one that is not a
42    ///   finite number is taken as zero.
43    /// * `measurement_noise` - how noisy each reading is; larger trusts readings less. Its
44    ///   magnitude is used, and one that is not a finite number is taken as zero, which
45    ///   trusts every reading.
46    /// * `initial` - the starting estimate, used until the first reading replaces it.
47    ///
48    /// # Returns
49    ///
50    /// A filter awaiting its first reading.
51    pub fn new(process_noise: f32, measurement_noise: f32, initial: f32) -> Self {
52        Self {
53            estimate: initial,
54            error: 1.0,
55            process: noise(process_noise),
56            measurement: noise(measurement_noise),
57            started: false,
58        }
59    }
60
61    /// Folds in a reading and returns the updated estimate.
62    ///
63    /// The first reading seeds the estimate and is returned unchanged. A reading that is not
64    /// a finite number, such as the NaN a failed sensor reports, is ignored and the estimate
65    /// returned as it stood.
66    ///
67    /// # Arguments
68    ///
69    /// * `reading` - the latest measurement.
70    ///
71    /// # Returns
72    ///
73    /// The filtered estimate after this reading.
74    pub fn update(&mut self, reading: f32) -> f32 {
75        if !reading.is_finite() {
76            return self.estimate;
77        }
78        if !self.started {
79            self.estimate = reading;
80            self.started = true;
81            return self.estimate;
82        }
83        let predicted_error = self.error + self.process;
84        let gain = if self.measurement == 0.0 {
85            1.0
86        } else {
87            predicted_error / (predicted_error + self.measurement)
88        };
89        self.estimate += gain * (reading - self.estimate);
90        self.error = (1.0 - gain) * predicted_error;
91        self.estimate
92    }
93
94    /// Returns the current estimate.
95    pub fn estimate(&self) -> f32 {
96        self.estimate
97    }
98}
99
100// The magnitude of a noise, taken by hand because `f32::abs` lives in `std`, with one that
101// is not a finite number read as zero.
102fn noise(value: f32) -> f32 {
103    if !value.is_finite() {
104        0.0
105    } else if value < 0.0 {
106        -value
107    } else {
108        value
109    }
110}
111
112#[cfg(test)]
113mod tests {
114    use super::*;
115
116    #[test]
117    fn the_first_reading_seeds_the_estimate() {
118        let mut kalman = Kalman::new(0.1, 0.1, 0.0);
119        assert_eq!(kalman.update(42.0), 42.0);
120    }
121
122    #[test]
123    fn it_tracks_toward_a_new_level() {
124        let mut kalman = Kalman::new(0.1, 1.0, 0.0);
125        kalman.update(0.0); // seed at zero
126        let mut value = 0.0;
127        for _ in 0..50 {
128            value = kalman.update(10.0);
129        }
130        assert!((value - 10.0).abs() < 0.5);
131    }
132
133    #[test]
134    fn it_smooths_a_noisy_signal_toward_the_mean() {
135        let mut kalman = Kalman::new(0.01, 1.0, 0.0);
136        let mut value = 0.0;
137        for reading in [10.0, 8.0, 12.0, 9.0, 11.0, 10.0, 10.0, 9.5, 10.5, 10.0] {
138            value = kalman.update(reading);
139        }
140        assert!((value - 10.0).abs() < 1.0);
141    }
142
143    #[test]
144    fn zero_measurement_noise_follows_the_reading() {
145        let mut kalman = Kalman::new(1.0, 0.0, 0.0);
146        kalman.update(5.0); // seeds
147        assert_eq!(kalman.update(8.0), 8.0); // full trust in the reading
148    }
149
150    #[test]
151    fn no_noise_at_all_still_gives_a_number() {
152        let mut kalman = Kalman::new(0.0, 0.0, 0.0);
153        for reading in [5.0, 6.0, 7.0, 8.0] {
154            assert_eq!(kalman.update(reading), reading);
155        }
156    }
157
158    #[test]
159    fn a_reading_that_is_not_a_number_leaves_the_estimate_where_it_was() {
160        let mut kalman = Kalman::new(0.1, 1.0, 0.0);
161        let settled = kalman.update(10.0);
162        assert_eq!(kalman.update(f32::NAN), settled);
163        assert_eq!(kalman.update(f32::NEG_INFINITY), settled);
164        assert!(kalman.update(11.0).is_finite());
165        assert_eq!(Kalman::new(0.1, 1.0, 3.0).update(f32::NAN), 3.0);
166    }
167
168    #[test]
169    fn a_noise_that_is_not_a_number_is_taken_as_zero() {
170        let mut kalman = Kalman::new(f32::NAN, f32::INFINITY, 0.0);
171        kalman.update(5.0);
172        assert_eq!(kalman.update(8.0), 8.0);
173    }
174}
pamoja_kit/kalman.rs

pamoja_kit/
kalman.rs
