sac_base/operations/math/
iir.rs

1use crate::network::node::Node;
2use alloc::boxed::Box;
3use core::any::Any;
4use alloc::vec::Vec;
5use alloc::collections::VecDeque;
6use core::ops;
7use core::default::Default;
8
9/// The IIR struct contains the data required to calculate the iir data.
10/// It contains a buffer for the old samples, as well as the description of the fir and iir systems.
11/// Since each node in the network needs to be of the same size, the IIR struct will be boxed, and
12/// passed in as a pointer.
13pub struct IIR<T> {
14    /// FIR system description
15    fir: Vec<T>,
16    /// IIR system description
17    iir: Vec<T>,
18    /// FIR sample buffer
19    fir_samples: VecDeque<T>,
20    /// IIR sample buffer
21    iir_samples: VecDeque<T>,
22}
23
24impl<T> IIR<T>
25    where T: Copy + Clone + 'static + ops::Div<Output=T> + ops::Mul<Output=T> + ops::Add<Output=T> + ops::Sub<Output=T> + Default
26{
27    /// Allows to calculate samples of a iir system.
28    /// https://en.wikipedia.org/wiki/Infinite_impulse_response
29    /// This can be used in both signal processing as well as control engineering use cases.
30    /// This specific implementation calculates samples any given length of both the FIR as well
31    /// as the IIR part.
32    /// A implementation designed for a known length will certainly be faster. But this allows for
33    /// some quick experimentation.
34    ///
35    /// ## Example
36    /// ```rust
37    /// use sac_base::operations::math::iir::IIR;
38    ///
39    /// let mut test_vector = Vec::new();
40    /// test_vector.push(&[0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0][..]);
41    ///
42    /// let fir = [1.0].iter().map(|v| v.clone()).collect();
43    /// let iir = [1.0, 1.0].iter().map(|v| v.clone()).collect();
44    ///
45    /// let mut iir = IIR::new(fir, iir);
46    ///
47    /// iir.process((test_vector, 9));
48    /// let result = Vec::from(iir.poll());
49    ///
50    /// let control_vector: Vec<f64> = [0, 1, 0, 1, 0, 1, 0, 1, 0].iter().map(|v| v.clone() as f64).collect();
51    ///
52    /// assert_eq!(result, control_vector);
53    /// ```
54    pub fn new(fir: Vec<T>, iir: Vec<T>) -> Node<T>
55    {
56        let fir_len = fir.len();
57        let iir_len = iir.len() - 1;
58        let mut internal: IIR<T> = IIR {
59            fir: fir,
60            iir: iir,
61            fir_samples: VecDeque::with_capacity(fir_len),
62            iir_samples: VecDeque::with_capacity(iir_len),
63        };
64
65        // Initialize the buffer properly to avoid checks during runtime
66        for _i in 0..fir_len {
67            internal.fir_samples.push_back(T::default());
68        }
69        // Initialize the buffer properly to avoid checks during runtime
70        for _i in 0..iir_len {
71            internal.iir_samples.push_back(T::default());
72        }
73
74        let storage = Box::new(internal) as Box<dyn Any>;
75        Node::new(storage, | data_input, data_container, output| {
76            let iir: &mut IIR<T> = data_container.downcast_mut::<IIR<T>>().unwrap();
77            let (inputs, max_len) = data_input;
78            output.clear();
79            inputs.into_iter().take(1).into_iter().for_each(|data| {
80                data.into_iter().take(max_len).into_iter().for_each(|v| {
81                    let y_0 = IIR::calculate(*v, iir);
82                    output.push(y_0);
83                })
84            })
85        })
86    }
87
88    #[inline(always)]
89    pub fn calculate(sample: T, data: &mut IIR<T>) -> T {
90        // Discard oldest sample
91        data.fir_samples.pop_back();
92        // Push new sample
93        data.fir_samples.push_front(sample);
94
95        let mut y_0 = T::default();
96
97        // Calculate X
98        for i in 0..data.fir.len() {
99            let x = *data.fir_samples.get(i).unwrap();
100            let b = *data.fir.get(i).unwrap();
101            y_0 = y_0 + b * x;
102        }
103
104        // Calculate Y
105        for i in 1..data.iir.len() {
106            let y = *data.iir_samples.get(i - 1).unwrap();
107            let a = *data.iir.get(i).unwrap();
108            y_0 = y_0 - a * y;
109        }
110
111        // Discard oldest sample
112        data.iir_samples.pop_back();
113        // Push y value
114        data.iir_samples.push_front(y_0);
115
116        return y_0;
117    }
118}
119
120#[cfg(test)]
121mod tests {
122
123    use super::*;
124    use alloc::vec::Vec;
125    use assert_approx_eq::assert_approx_eq;
126
127    #[test]
128    fn test_iir_actual() {
129
130        let fir = [-0.06, 0.4].iter().map(|v| v.clone() as f64).collect();
131        let iir = [1.0, -1.6, 0.78].iter().map(|v| v.clone() as f64).collect();
132
133        let mut iir = IIR::new(fir, iir);
134
135        let mut result = Vec::new();
136
137        let mut input = Vec::new();
138        let slice_ref = &[0.0][..];
139        input.push(slice_ref);
140        iir.process((input, 1));
141        result.push(iir.data.get(0).unwrap().clone());
142
143        for _i in 1..100 {
144            let mut input = Vec::new();
145            let slice_ref = &[1.0][..];
146            input.push(slice_ref);
147            iir.process((input, 1));
148            result.push(iir.data.get(0).unwrap().clone());
149        }
150
151
152        let test_vector: Vec<f64> = [ 0.        , -0.06      ,  0.244     ,  0.7772    ,  1.3932    ,
153            1.962904  ,  2.3939504 ,  2.63925552,  2.69552752,  2.59422473,
154            2.3882481 ,  2.13770167,  1.89748915,  1.70857534,  1.59367901,
155            1.55719765,  1.58844661,  1.66690041,  1.7680523 ,  1.86870136,
156            1.95084138,  2.00375915,  2.02435836,  2.01604124,  1.98666647,
157            1.94615418,  1.90424684,  1.86879468,  1.84475896,  1.83395448,
158            1.83541518,  1.8461798 ,  1.86226383,  1.87960189,  1.89479723,
159            1.9055861 ,  1.91099592,  1.91123631,  1.90740128,  1.90107773,
160            1.89395136,  1.88748156,  1.88268842,  1.88006587,  1.87960841,
161            1.88092209,  1.88338078,  1.88629001,  1.88902702,  1.89113702,
162            1.89237815,  1.89271817,  1.89229412,  1.89135041,  1.89017125,
163            1.88902068,  1.88809951,  1.88752309,  1.88731932,  1.88744291,
164            1.88779958,  1.88827386,  1.88875451,  1.8891536 ,  1.88941724,
165            1.88952778,  1.889499  ,  1.88936673,  1.88917755,  1.88897803,
166            1.88880636,  1.88868731,  1.88863074,  1.88863308,  1.88868095,
167            1.88875572,  1.88883801,  1.88891135,  1.88896452,  1.88899237,
168            1.88899547,  1.88897871,  1.88894946,  1.88891575,  1.88888461,
169            1.8888611 ,  1.88884776,  1.88884476,  1.88885036,  1.88886167,
170            1.88887538,  1.88888851,  1.88889882,  1.88890508,  1.88890704,
171            1.8889053 ,  1.888901  ,  1.88889546,  1.88888995,  1.88888547].iter().map(|v| v.clone() as f64).collect();
172
173
174        for i in 0..100 {
175            assert_approx_eq!(result[i], test_vector[i]);
176        }
177    }
178
179
180    #[test]
181    fn test_iir_simple() {
182
183        let mut test_vector = Vec::new();
184        test_vector.push(&[0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0][..]);
185
186        let fir = [1.0].iter().map(|v| v.clone()).collect();
187        let iir = [1.0, 1.0].iter().map(|v| v.clone()).collect();
188
189        let mut iir = IIR::new(fir, iir);
190
191        iir.process((test_vector, 9));
192        let result = Vec::from(iir.poll());
193
194        let control_vector: Vec<f64> = [0, 1, 0, 1, 0, 1, 0, 1, 0].iter().map(|v| v.clone() as f64).collect();
195
196        assert_eq!(result, control_vector);
197    }
198}
199
sac_base/operations/math/iir.rs

sac_base/operations/math/
iir.rs
