niva_components/data_filter/
kalman.rs

1/// Basic implementation of Kalman filter
2pub struct Kalman {
3    estimate: f32,
4    error_covariance: f32,
5    process_noise: f32,
6    measurement_noise: f32,
7}
8
9impl Kalman {
10    /// If your ADC measurements are very noisy:
11    /// Use a larger R (e.g., 0.1 to 1.0) and a smaller Q (e.g., 0.01).
12    ///
13    /// If your ADC measurements are stable:
14    /// Use a smaller R (e.g., 0.001 to 0.01) and a larger Q (e.g., 0.1).
15    #[allow(unused)]
16    pub fn new(process_noise: f32, measurement_noise: f32) -> Self {
17        Self {
18            estimate: 0.0,
19            error_covariance: 1.0,
20            process_noise,
21            measurement_noise,
22        }
23    }
24
25    /// Call this method in loop for get calibrated value
26    #[allow(unused)]
27    pub fn update(&mut self, measurement: f32) -> f32 {
28        let kalman_gain = self.error_covariance / (self.error_covariance + self.measurement_noise);
29        self.estimate += kalman_gain * (measurement - self.estimate);
30        self.error_covariance = (1.0 - kalman_gain) * self.error_covariance + self.process_noise;
31        self.estimate
32    }
33}
niva_components/data_filter/kalman.rs

niva_components/data_filter/
kalman.rs
