1use crate::fixed::{Fixed, PromoteAddSub};
6use crate::fixed_format::Value;
7use crate::num_traits;
8
9pub struct ExponentialAverage<Val, Alpha, Beta> {
24 average: Val,
25 variance: Val,
26 alpha: Alpha,
27 beta: Beta,
28}
29
30impl<V, const V_FRAC: usize, A, const A_FRAC: usize, B, const B_FRAC: usize>
31 ExponentialAverage<Fixed<V, V_FRAC>, Fixed<A, A_FRAC>, Fixed<B, B_FRAC>>
32where
33 V: num_traits::Bounded + num_traits::NumCast + Copy + PromoteAddSub<V> + num_traits::Signed,
34 A: num_traits::Bounded + num_traits::NumCast + Copy + PromoteAddSub<A>,
35 B: num_traits::Bounded + num_traits::NumCast + Copy + PromoteAddSub<B>,
36 V::Output: num_traits::Bounded + num_traits::NumCast + Copy,
37 A::Output: num_traits::Bounded + num_traits::NumCast + Copy,
38 B::Output: num_traits::Bounded + num_traits::NumCast + Copy,
39 Fixed<V, V_FRAC>: Copy
40 + Default
41 + PartialOrd
42 + core::ops::AddAssign
43 + core::ops::Sub<Output = Fixed<V::Output, V_FRAC>>
44 + core::ops::Add<Output = Fixed<V::Output, V_FRAC>>,
45 Fixed<V::Output, V_FRAC>: Copy + PartialOrd,
46 Fixed<A, A_FRAC>: Copy + PartialOrd + core::ops::Sub<Output = Fixed<A::Output, A_FRAC>>,
47 Fixed<B, B_FRAC>: Copy + PartialOrd + core::ops::Sub<Output = Fixed<B::Output, B_FRAC>>,
48 Value<V, 0>: crate::fixed_format::ConvertValue<V, V_FRAC>,
49 Value<V::Output, 0>: crate::fixed_format::ConvertValue<V::Output, V_FRAC>,
50 Value<A, 0>: crate::fixed_format::ConvertValue<A, A_FRAC>,
51 Value<B, 0>: crate::fixed_format::ConvertValue<B, B_FRAC>,
52{
53 pub fn new(value: Fixed<V, V_FRAC>, alpha: Fixed<A, A_FRAC>, beta: Fixed<B, B_FRAC>) -> Self {
63 assert!(alpha >= Fixed::from_integer(0));
64 assert!(alpha <= Fixed::from_integer(1));
65 assert!(beta >= Fixed::from_integer(0));
66 assert!(beta <= Fixed::from_integer(1));
67
68 Self { average: value, variance: Fixed::from_integer(0), alpha, beta }
69 }
70
71 pub fn add_sample(&mut self, sample: Fixed<V, V_FRAC>) {
73 let delta = sample - self.average;
74 let one_a = Fixed::<A, A_FRAC>::from_integer(1);
75 let one_b = Fixed::<B, B_FRAC>::from_integer(1);
76
77 if delta >= Fixed::from_integer(0) {
78 let term1: Fixed<V, V_FRAC> = self.alpha.mul_to(delta);
79 self.average += term1;
80 let term2: Fixed<V, V_FRAC> = term1.mul_to(delta);
81 let sum = self.variance + term2;
82 self.variance = (one_a - self.alpha).mul_to(sum);
83 } else {
84 let term1: Fixed<V, V_FRAC> = self.beta.mul_to(delta);
85 self.average += term1;
86 let term2: Fixed<V, V_FRAC> = term1.mul_to(delta);
87 let sum = self.variance + term2;
88 self.variance = (one_b - self.beta).mul_to(sum);
89 }
90 }
91
92 pub fn value(&self) -> Fixed<V, V_FRAC> {
94 self.average
95 }
96
97 pub fn variance(&self) -> Fixed<V, V_FRAC> {
99 self.variance
100 }
101}
102
103impl<V, const V_FRAC: usize, A, const A_FRAC: usize>
104 ExponentialAverage<Fixed<V, V_FRAC>, Fixed<A, A_FRAC>, Fixed<A, A_FRAC>>
105where
106 V: num_traits::Bounded + num_traits::NumCast + Copy + PromoteAddSub<V> + num_traits::Signed,
107 A: num_traits::Bounded + num_traits::NumCast + Copy + PromoteAddSub<A>,
108 V::Output: num_traits::Bounded + num_traits::NumCast + Copy,
109 A::Output: num_traits::Bounded + num_traits::NumCast + Copy,
110 Fixed<V, V_FRAC>: Copy
111 + Default
112 + PartialOrd
113 + core::ops::AddAssign
114 + core::ops::Sub<Output = Fixed<V::Output, V_FRAC>>
115 + core::ops::Add<Output = Fixed<V::Output, V_FRAC>>,
116 Fixed<V::Output, V_FRAC>: Copy + PartialOrd,
117 Fixed<A, A_FRAC>: Copy + PartialOrd + core::ops::Sub<Output = Fixed<A::Output, A_FRAC>>,
118 Value<V, 0>: crate::fixed_format::ConvertValue<V, V_FRAC>,
119 Value<V::Output, 0>: crate::fixed_format::ConvertValue<V::Output, V_FRAC>,
120 Value<A, 0>: crate::fixed_format::ConvertValue<A, A_FRAC>,
121{
122 pub fn new_single_rate(value: Fixed<V, V_FRAC>, alpha: Fixed<A, A_FRAC>) -> Self {
125 Self::new(value, alpha, alpha)
126 }
127}