microdsp 0.1.3

DSP algorithms and utilities. Performant, embedded friendly and no_std compatible.
Documentation 
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
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110

use alloc::{vec, vec::Vec};

/// An adaptive [normalized least mean squares filter](https://en.wikipedia.org/wiki/Least_mean_squares_filter#Normalized_least_mean_squares_filter_(NLMS)).
/// Using the same notation as in the linked description.
pub struct NlmsFilter {
    /// FIR filter coefficients
    h: Vec<f32>,
    /// Most recent input values. Newest sample is at index 0.
    x: Vec<f32>,
    /// Step size scale
    μ: f32,
    /// Running sum of current input signal power.
    x_power: f32,
    /// Constant added to the update step denominator to avoid division by zero.
    ε: f32,
    buffer_pos: usize,
}

impl NlmsFilter {
    pub fn new(order: usize, mu: f32, eps: f32) -> Self {
        let h = vec![0.0; order];
        NlmsFilter {
            h,
            // Trade memory usage for update speed:
            // double size for x to avoid index wrapping in the update method.
            x: vec![0.0; 2 * order],
            μ: mu,
            ε: eps,
            buffer_pos: 0,
            x_power: 0.0,
        }
    }

    pub fn h(&mut self) -> &[f32] {
        &self.h
    }

    pub fn order(&mut self) -> usize {
        self.h.len()
    }

    pub fn update(&mut self, x: f32, d: f32) -> f32 {
        assert!(self.buffer_pos < self.order());
        let order = self.order();
        self.x[self.buffer_pos] = x;
        self.x[self.buffer_pos + order] = x;

        // Add new input sample to signal power
        self.x_power += x * x;

        // Compute filter output y = h applied to x.
        let mut y = 0.0;
        for (h, x) in self.h.iter().zip(self.x.iter().skip(self.buffer_pos)) {
            y += h * *x
        }

        let e = d - y;
        let delta_scale = self.μ * e / (self.x_power + self.ε);
        for (h, x) in self.h.iter_mut().zip(self.x.iter().skip(self.buffer_pos)) {
            *h += delta_scale * *x;
        }

        // Subtract oldest input sample from signal power and advance buffer position
        let next_buffer_pos = if self.buffer_pos == 0 {
            self.order() - 1
        } else {
            self.buffer_pos - 1
        };
        let x_oldest = self.x[next_buffer_pos];
        self.x_power -= x_oldest * x_oldest;
        self.buffer_pos = next_buffer_pos;

        e
    }

    pub fn reset(&mut self) {
        for i in 0..self.order() {
            self.h[i] = 0.0;
            self.x[i] = 0.0;
        }
        self.buffer_pos = 0;
        self.x_power = 0.0;
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_nlms_iterations() {
        let h_expected: [[f32; 3]; 6] = [
            [0.0, 0.0, 0.0],
            [1.4985014, 0.0, 0.0],
            [1.6990607, 0.10027966, 0.0],
            [1.9885677, 0.29328436, 0.09650234],
            [1.9866968, 0.29188123, 0.09556692],
            [1.7673157, 0.116376325, -0.036061756],
        ];
        let mut filter = NlmsFilter::new(3, 0.5, 0.001);
        let x: [f32; 6] = [0.0, 1.0, 2.0, 3.0, 4.0, 5.0];
        let d: [f32; 6] = [1.0, 3.0, 4.0, 8.0, 9.0, 7.0];
        for (i, (x, d)) in x.iter().zip(d.iter()).enumerate() {
            filter.update(*x, *d);
            for (h, h_expected) in filter.h().iter().zip(h_expected[i].iter()) {
                assert_eq!(*h, *h_expected);
            }
        }
    }
}