irox_stats/
filter.rs

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
// SPDX-License-Identifier: MIT
// Copyright 2023 IROX Contributors

//!
//! This module provides Discrete Convolution Filters for Digital Signals Processing.
//!
//! Many algorithms sourced from the wonderful [DSPGuide](https://www.dspguide.com)
//!

extern crate alloc;

use alloc::collections::vec_deque::Drain;
use alloc::collections::VecDeque;
use alloc::vec;
use alloc::vec::Vec;

///
/// Represents a convolving operation with streaming input data against the provided kernel.
#[derive(Clone)]
pub struct StreamingFilter {
    kernel: Vec<f64>,
    buffer: VecDeque<f64>,
}

impl StreamingFilter {
    ///
    /// Creates a new streaming filter with the provided input kernel
    pub fn new(kernel: Vec<f64>) -> StreamingFilter {
        let len = kernel.len() - 2;
        let buffer = VecDeque::from(vec![0.0; len]);
        StreamingFilter { kernel, buffer }
    }

    ///
    /// Adds the sample to the internal data buffer.  If there are enough samples in the buffer,
    /// the convolve operation is run, and the output returned.  Otherwise returns None.
    pub fn add_and_convolve(&mut self, sample: f64) -> Option<f64> {
        self.buffer.push_back(0.0_f64);

        self.kernel.iter().enumerate().for_each(|(idx, k)| {
            if let Some(val) = self.buffer.get_mut(idx) {
                *val += sample * *k;
            }
        });

        self.buffer.pop_front()
    }

    ///
    /// Drains the residual data in the buffer to the caller.
    pub fn drain(&mut self) -> Drain<f64> {
        self.buffer.drain(..)
    }
}

///
/// Performs the [discrete convolution](https://en.wikipedia.org/wiki/Convolution#Discrete_convolution)
/// function.  Convolves the provided kernel against the provided data.
pub fn convolve<K: AsRef<[f64]>, D: AsRef<[f64]>>(kernel: K, data: D) -> Vec<f64> {
    let kernel = kernel.as_ref();
    let data = data.as_ref();
    let length = kernel.len() + data.len();
    let mut out: Vec<f64> = Vec::with_capacity(length);
    out.resize(length, 0.0_f64);

    data.iter().enumerate().for_each(|(d_idx, d)| {
        kernel.iter().enumerate().for_each(|(k_idx, k)| {
            let idx = d_idx + k_idx;
            if let Some(val) = out.get_mut(idx) {
                *val += *d * *k;
            }
        });
    });

    out
}