irox_stats/
abg.rs

1// SPDX-License-Identifier: MIT
2// Copyright 2024 IROX Contributors
3//
4
5use crate::sampling::Sample;
6use irox_time::epoch::Timestamp;
7
8#[derive(Debug, Copy, Clone)]
9pub struct ABStep<T: Copy> {
10    pub time: Timestamp<T>,
11    pub predicted_value: f64,
12    pub estimated_value: f64,
13    pub estimated_velocity: f64,
14    pub residual_error: f64,
15}
16
17pub struct AlphaBetaFilter<T: Copy> {
18    alpha_coefficient: f64,
19    beta_coefficient: f64,
20
21    last_step: Option<ABStep<T>>,
22}
23
24impl<T: Copy> AlphaBetaFilter<T> {
25    pub fn add_sample(&mut self, sample: Sample<T>) -> Option<ABStep<T>> {
26        self.insert(sample.time, sample.value)
27    }
28    pub fn insert(&mut self, time: Timestamp<T>, value: f64) -> Option<ABStep<T>> {
29        let Some(last_step) = &self.last_step else {
30            self.last_step = Some(ABStep {
31                time,
32                predicted_value: value,
33                estimated_value: value,
34                estimated_velocity: 0.0,
35                residual_error: 0.0,
36            });
37            return None;
38        };
39        let dt = (time - last_step.time).as_seconds_f64();
40
41        // predict step, predict the current (measured) value
42        let predicted_value = last_step.estimated_value + last_step.estimated_velocity * dt;
43
44        // update step, calculate residual error of the prediction
45        let residual_error = value - predicted_value;
46
47        // update step, update estimates for next iteration
48        let estimated_value = predicted_value + self.alpha_coefficient * residual_error;
49        let estimated_velocity =
50            last_step.estimated_velocity + self.beta_coefficient * (residual_error / dt);
51
52        let step = ABStep {
53            time,
54            predicted_value,
55            estimated_velocity,
56            estimated_value,
57            residual_error,
58        };
59        self.last_step = Some(step);
60        self.last_step
61    }
62}
63
64#[derive(Debug, Copy, Clone)]
65pub struct ABGStep<T: Copy> {
66    pub time: Timestamp<T>,
67    pub predicted_value: f64,
68    pub predicted_velocity: f64,
69
70    pub estimated_value: f64,
71    pub estimated_velocity: f64,
72    pub estimated_acceleration: f64,
73
74    pub residual_error: f64,
75}
76
77pub struct AlphaBetaGammaFilter<T: Copy> {
78    alpha_coefficient: f64,
79    beta_coefficient: f64,
80    gamma_coefficient: f64,
81
82    last_step: Option<ABGStep<T>>,
83}
84
85impl<T: Copy> AlphaBetaGammaFilter<T> {
86    pub fn add_sample(&mut self, sample: Sample<T>) -> Option<ABGStep<T>> {
87        self.insert(sample.time, sample.value)
88    }
89    pub fn insert(&mut self, time: Timestamp<T>, value: f64) -> Option<ABGStep<T>> {
90        let Some(last_step) = &self.last_step else {
91            self.last_step = Some(ABGStep {
92                time,
93                predicted_value: value,
94                predicted_velocity: 0.0,
95                estimated_value: value,
96                estimated_velocity: 0.0,
97                estimated_acceleration: 0.0,
98                residual_error: 0.0,
99            });
100            return None;
101        };
102        let dt = (time - last_step.time).as_seconds_f64();
103        let dt2 = (dt * dt) / 2.;
104
105        // predict step, predict the current (measured) value and velocity
106        let predicted_velocity =
107            last_step.estimated_velocity + last_step.estimated_acceleration * dt;
108        let predicted_value = last_step.estimated_value
109            + last_step.estimated_velocity * dt
110            + last_step.estimated_acceleration * dt2;
111
112        // update step, calculate residual error of the prediction
113        let residual_error = value - predicted_value;
114
115        // update step, update estimates for next iteration
116        let estimated_value = predicted_value + self.alpha_coefficient * residual_error;
117        let estimated_velocity =
118            last_step.estimated_velocity + self.beta_coefficient * (residual_error / dt);
119        let estimated_acceleration =
120            last_step.estimated_acceleration + self.gamma_coefficient * (residual_error / dt2);
121
122        let step = ABGStep {
123            time,
124            predicted_value,
125            predicted_velocity,
126            estimated_velocity,
127            estimated_value,
128            residual_error,
129            estimated_acceleration,
130        };
131        self.last_step = Some(step);
132        self.last_step
133    }
134}
irox_stats/abg.rs

irox_stats/
abg.rs
