From b15e81409e82e1eadbd1b8d4c5def014c5788358 Mon Sep 17 00:00:00 2001 From: "J.A. de Jong - Redu-Sone B.V., ASCEE V.O.F" Date: Fri, 19 Apr 2024 14:09:32 +0200 Subject: [PATCH] Split up filter in module parts --- Cargo.toml | 8 +- src/config.rs | 33 ++- src/daq/mod.rs | 11 +- src/filter.rs | 495 ------------------------------------- src/filter/biquad.rs | 164 ++++++++++++ src/filter/biquadbank.rs | 175 +++++++++++++ src/filter/mod.rs | 45 ++++ src/filter/seriesbiquad.rs | 137 ++++++++++ src/lib.rs | 17 +- 9 files changed, 564 insertions(+), 521 deletions(-) delete mode 100644 src/filter.rs create mode 100644 src/filter/biquad.rs create mode 100644 src/filter/biquadbank.rs create mode 100644 src/filter/mod.rs create mode 100644 src/filter/seriesbiquad.rs diff --git a/Cargo.toml b/Cargo.toml index 710e13f..f44279d 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -29,8 +29,8 @@ num = "0.4.1" rayon = "1.8.0" # Python bindings -pyo3 = { version = "0.20", optional = true, features = ["extension-module", "anyhow"]} -numpy = { version = "0.20", optional = true} +pyo3 = { version = "0.21.2", optional = true, features = ["extension-module", "anyhow"]} +numpy = { version = "0.21.0", optional = true} # White noise etc rand = "0.8.5" @@ -40,8 +40,8 @@ rand_distr = "0.4.3" cpal = { version = "0.15.3", optional = true } # Nice enumerations -strum = "0.25.0" -strum_macros = "0.25.3" +strum = "0.26.2" +strum_macros = "0.26.2" # Conditional compilation enhancements cfg-if = "1.0.0" diff --git a/src/config.rs b/src/config.rs index 459b797..97b0fde 100644 --- a/src/config.rs +++ b/src/config.rs @@ -20,18 +20,45 @@ cfg_if::cfg_if! { } } +cfg_if::cfg_if! { +if #[cfg(feature = "python-bindings")] { + pub use numpy::ndarray::{ArrayD, ArrayViewD, ArrayViewMutD}; + pub use numpy::ndarray::prelude::*; + pub use numpy::{IntoPyArray, PyArray1, PyArrayDyn, PyArrayLike1, PyReadonlyArrayDyn}; + pub use pyo3::exceptions::PyValueError; + pub use pyo3::prelude::*; + pub use pyo3::{pymodule, types::PyModule, PyResult}; +} else { + + pub use ndarray::prelude::*; + pub use ndarray::{Array1, Array2, ArrayView1}; +} } + +// pub use num::complex::i; use num::complex::*; + + +/// View into 1D array of floats +pub type VdView<'a> = ArrayView1<'a, Flt>; + +/// View into 1D array of complex floats +pub type VcView<'a> = ArrayView1<'a, Cflt>; + /// Complex number floating point pub type Cflt = Complex; -use ndarray::{Array1, Array2}; -/// Vector of floating point values +/// Complex unit sqrt(-1) +pub const I: Cflt = Cflt::new(0., 1.); + +/// (Owning) Vector of floating point values pub type Vd = Vec; -/// Vector of complex floating point values + +/// (Owning) Vector of complex floating point values pub type Vc = Vec; /// 1D array of floats pub type Dcol = Array1; + /// 1D array of complex floats pub type Ccol = Array1; diff --git a/src/daq/mod.rs b/src/daq/mod.rs index c33d8c6..c7c1305 100644 --- a/src/daq/mod.rs +++ b/src/daq/mod.rs @@ -18,7 +18,7 @@ mod streamstatus; // Module re-exports pub use daqconfig::{DaqChannel, DaqConfig}; -pub use datatype::*; +pub use datatype::DataType; pub use deviceinfo::DeviceInfo; pub use qty::Qty; pub use streamhandler::StreamHandler; @@ -31,13 +31,8 @@ pub use record::*; use strum_macros::Display; -cfg_if::cfg_if! { -if #[cfg(feature = "python-bindings")] { - use pyo3::exceptions::PyValueError; - use pyo3::prelude::*; - use pyo3::{pymodule, pyclass, types::PyModule, PyResult}; -} else {} } - +use crate::config::*; +use super::*; /// Stream types that can be started /// #[cfg_attr(feature = "python-bindings", pyclass)] diff --git a/src/filter.rs b/src/filter.rs deleted file mode 100644 index ed1095f..0000000 --- a/src/filter.rs +++ /dev/null @@ -1,495 +0,0 @@ -//! # Filter implemententations for biquads, series of biquads and banks of series of biquads. -//! -//! Contains [Biquad], [SeriesBiquad], and [BiquadBank]. These are all constructs that work on -//! blocks of input data, and apply filters on it. Todo: implement FIR filter. -#![allow(non_snake_case)] -use super::config::*; -use anyhow::{bail, Result}; -use cfg_if::cfg_if; - -use rayon::prelude::*; - -cfg_if! { -if #[cfg(feature = "python-bindings")] { - use numpy::ndarray::{ArrayD, ArrayViewD, ArrayViewMutD}; - use numpy::{IntoPyArray, PyArray1, PyArrayDyn, PyArrayLike1, PyReadonlyArrayDyn}; - use pyo3::exceptions::PyValueError; - use pyo3::prelude::*; - use pyo3::{pymodule, types::PyModule, PyResult}; -} else {} } - -pub trait Filter: Send { - //! The filter trait is implemented by Biquad, SeriesBiquad, and BiquadBank - - /// Filter input to generate output. A vector of output floats is generated with the same - /// length as input. - fn filter(&mut self, input: &[Flt]) -> Vd; - /// Reset the filter state(s). In essence, this makes sure that all memory of the past is - /// forgotten. - fn reset(&mut self); - - /// Required method for cloning a BiquadBank, such that arbitrary filter types can be used as - /// their 'channels'. - fn clone_dyn(&self) -> Box; -} -impl Clone for Box { - fn clone(&self) -> Self { - self.clone_dyn() - } -} - -/// # A biquad is a second order recursive filter structure. -#[cfg_attr(feature = "python-bindings", pyclass)] -#[derive(Clone, Copy, Debug)] -pub struct Biquad { - // State parameters - w1: Flt, - w2: Flt, - // Filter coefficients - forward - b0: Flt, - b1: Flt, - b2: Flt, - // Filter coefficients - recursive - // a0: Flt, // a0 is assumed one, not used - a1: Flt, - a2: Flt, -} -#[cfg(feature = "python-bindings")] -#[cfg_attr(feature = "python-bindings", pymethods)] -impl Biquad { - #[new] - /// Create new biquad filter. See [Biquad::new()] - /// - pub fn new_py<'py>(coefs: PyReadonlyArrayDyn) -> PyResult { - Ok(Biquad::new(coefs.as_slice()?)?) - } - #[pyo3(name = "unit")] - #[staticmethod] - /// See: [Biquad::unit()] - pub fn unit_py() -> Biquad { - Biquad::unit() - } - #[pyo3(name = "firstOrderHighPass")] - #[staticmethod] - /// See: [Biquad::firstOrderHighPass()] - pub fn firstOrderHighPass_py(fs: Flt, cuton_Hz: Flt) -> PyResult { - Ok(Biquad::firstOrderHighPass(fs, cuton_Hz)?) - } - #[pyo3(name = "filter")] - /// See: [Biquad::filter()] - pub fn filter_py<'py>( - &mut self, - py: Python<'py>, - input: PyArrayLike1, - ) -> PyResult<&'py PyArray1> { - Ok(self.filter(input.as_slice()?).into_pyarray(py)) - } -} -impl Biquad { - /// Create new biquad filter from given filter coeficients - /// - /// # Args - /// - /// - coefs: Filter coefficients. - /// - pub fn new(coefs: &[Flt]) -> Result { - match coefs { - [b0, b1, b2, a0, a1, a2] => { - if *a0 != 1.0 { - bail!("Coefficient a0 should be equal to 1.0") - } - Ok(Biquad { w1: 0., w2: 0., b0: *b0, b1: *b1, b2: *b2, a1: *a1, a2: *a2}) - }, - _ => bail!("Could not initialize biquad. Please make sure that the coefficients contain 6 terms") - } - } - - /// Create unit impulse response biquad filter. Input = output - fn unit() -> Biquad { - let filter_coefs = &[1., 0., 0., 1., 0., 0.]; - Biquad::new(filter_coefs).unwrap() - } - - /// Initialize biquad as first order high pass filter. - /// - /// - /// * fs: Sampling frequency in \[Hz\] - /// * cuton_Hz: -3 dB cut-on frequency in \[Hz\] - /// - pub fn firstOrderHighPass(fs: Flt, cuton_Hz: Flt) -> Result { - if fs <= 0. { - bail!("Invalid sampling frequency: {} [Hz]", fs); - } - if cuton_Hz <= 0. { - bail!("Invalid cuton frequency: {} [Hz]", cuton_Hz); - } - if cuton_Hz >= 0.98 * fs / 2. { - bail!( - "Invalid cuton frequency. We limit this to 0.98* fs / 2. Given value {} [Hz]", - cuton_Hz - ); - } - - let tau: Flt = 1. / (2. * pi * cuton_Hz); - let facnum = 2. * fs * tau / (1. + 2. * fs * tau); - let facden = (1. - 2. * fs * tau) / (1. + 2. * fs * tau); - - let coefs = [ - facnum, // b0 - -facnum, // b1 - 0., // b2 - 1., // a0 - facden, // a1 - 0., // a2 - ]; - - Ok(Biquad::new(&coefs).unwrap()) - } - fn filter_inout(&mut self, inout: &mut [Flt]) { - for sample in inout.iter_mut() { - let w0 = *sample - self.a1 * self.w1 - self.a2 * self.w2; - let yn = self.b0 * w0 + self.b1 * self.w1 + self.b2 * self.w2; - self.w2 = self.w1; - self.w1 = w0; - *sample = yn; - } - // println!("{:?}", inout); - } -} -impl Filter for Biquad { - fn filter(&mut self, input: &[Flt]) -> Vec { - let mut out = input.to_vec(); - self.filter_inout(&mut out); - // println!("{:?}", out); - out - } - fn reset(&mut self) { - self.w1 = 0.; - self.w2 = 0.; - } - fn clone_dyn(&self) -> Box { - Box::new(*self) - } -} - -/// Series of biquads that filter sequentially on an input signal -/// -/// # Examples -/// -/// See (tests) -/// ``` -#[derive(Clone, Debug)] -#[cfg_attr(feature = "python-bindings", pyclass)] -pub struct SeriesBiquad { - biqs: Vec, -} - -#[cfg(feature = "python-bindings")] -#[cfg_attr(feature = "python-bindings", pymethods)] -impl SeriesBiquad { - #[new] - /// Create new series filter set. See [SeriesBiquad::new()] - /// - pub fn new_py<'py>(coefs: PyReadonlyArrayDyn) -> PyResult { - Ok(SeriesBiquad::new(coefs.as_slice()?)?) - } - #[pyo3(name = "unit")] - #[staticmethod] - /// See: [Biquad::unit()] - pub fn unit_py() -> SeriesBiquad { - SeriesBiquad::unit() - } - #[pyo3(name = "filter")] - /// See: [SeriesBiquad::filter()] - pub fn filter_py<'py>( - &mut self, - py: Python<'py>, - input: PyArrayLike1, - ) -> PyResult<&'py PyArray1> { - Ok(self.filter(input.as_slice()?).into_pyarray(py)) - } - #[pyo3(name = "reset")] - /// See: [SeriesBiquad::reset()] - pub fn reset_py(&mut self) { - self.reset(); - } -} -impl SeriesBiquad { - /// Create a new series biquad, having an arbitrary number of biquads. - /// - /// # Arguments - /// - /// * `filter_coefs` - Vector of biquad coefficients, stored in a single array. The first six - /// for the first biquad, and so on. - /// - /// - pub fn new(filter_coefs: &[Flt]) -> Result { - if filter_coefs.len() % 6 != 0 { - bail!( - "filter_coefs should be multiple of 6, given: {}.", - filter_coefs.len() - ); - } - let nfilters = filter_coefs.len() / 6; - - let mut biqs: Vec = Vec::with_capacity(nfilters); - for coefs in filter_coefs.chunks(6) { - let biq = Biquad::new(coefs)?; - biqs.push(biq); - } - - if biqs.is_empty() { - bail!("No filter coefficients given!"); - } - - Ok(SeriesBiquad { biqs }) - } - - /// Unit impulse response series biquad. Input = output - pub fn unit() -> SeriesBiquad { - let filter_coefs = &[1., 0., 0., 1., 0., 0.]; - SeriesBiquad::new(filter_coefs).unwrap() - } - fn clone_dyn(&self) -> Box { - Box::new(self.clone()) - } -} - -impl Filter for SeriesBiquad { - //! Filter input by applying all biquad filters in series on each input sample, to obtain the - //! output samples. - //! - fn filter(&mut self, input: &[Flt]) -> Vd { - let mut inout = input.to_vec(); - for biq in self.biqs.iter_mut() { - biq.filter_inout(&mut inout); - } - inout - } - fn reset(&mut self) { - self.biqs.iter_mut().for_each(|f| f.reset()); - } - fn clone_dyn(&self) -> Box { - Box::new(self.clone()) - } -} - -#[cfg_attr(feature = "python-bindings", pyclass)] -#[derive(Clone)] -/// Multiple biquad filter that operate in parallel on a signal, and can apply a gain value to each -/// of the returned values. The BiquadBank can be used to decompose a signal by running it through -/// parallel filters, or it can directly be used to eq a signal. For the latter process, also a -/// gain can be applied when the output is made as the sum of the filtered inputs for each biquad. -/// -/// # Detailed description -/// -/// Below is an example of the signal flow is for the case of three SeriesBiquad filters, `h1`, -/// `h2` and `h3`: -/// -/// ```markdown -/// -/// analysis() gain application sum() -/// -/// +------+ +-----+ +------------+ -/// +-----+ h1 |---+ g1 +-------------+ | | -/// | +------+ +-----+ ++ | +------ | -/// | +-----| ++ | -/// Input | +------+ +-----+ | ++ | Output of filter() -///--------|>--+-----+ h2 |---+ g2 |--------------------| +-+ +----------------|> -/// | ---+---+ +-----+ | +-+ | -/// | +-----| ++ | -/// | +------+ +-----+ ++ | ++ | -/// +-----+ h3 |---+ g3 |-------------+ | +------ | -/// +------+ +-----+ | | -/// | +------------+ -/// | -/// | Output of analysis() method (optional) -/// +-------------|> -/// ``` -pub struct BiquadBank { - biqs: Vec>, - gains: Vec, -} - -#[cfg(feature = "python-bindings")] -#[cfg_attr(feature = "python-bindings", pymethods)] -/// Methods to wrap it in Python -impl BiquadBank { - #[new] - /// Create new biquadbank filter set. See [BiquadBank::new()] - /// - pub fn new_py<'py>(coefs: PyReadonlyArrayDyn) -> PyResult { - let mut filts = vec![]; - for col in coefs.as_array().columns() { - match col.as_slice() { - Some(colslice) => { - let new_ser = SeriesBiquad::new(colslice)?; - filts.push(new_ser.clone_dyn()); - } - None => { - return Err(PyValueError::new_err("Error generating column")); - } - } - } - Ok(BiquadBank::new(filts)) - } - #[pyo3(name = "filter")] - /// See: [BiquadBank::filter()] - pub fn filter_py<'py>( - &mut self, - py: Python<'py>, - input: PyArrayLike1, - ) -> PyResult<&'py PyArray1> { - Ok(self.filter(input.as_slice()?).into_pyarray(py)) - } - #[pyo3(name = "reset")] - /// See: [BiquadBank::reset()] - pub fn reset_py(&mut self) { - self.reset(); - } - - #[pyo3(name = "set_gains")] - /// See: [BiquadBank::set_gains()] - pub fn set_gains_py<'py>(&mut self, gains: PyArrayLike1) -> PyResult<()> { - if gains.len() != self.len() { - return Err(PyValueError::new_err("Invalid number of provided gains")); - } - self.set_gains(gains.as_slice()?); - Ok(()) - } - #[pyo3(name = "set_gains_dB")] - /// See: [BiquadBank::set_gains_dB()] - pub fn set_gains_dB_py<'py>(&mut self, gains_dB: PyArrayLike1) -> PyResult<()> { - if gains_dB.len() != self.len() { - return Err(PyValueError::new_err("Invalid number of provided gains")); - } - self.set_gains_dB(gains_dB.as_slice()?); - Ok(()) - } - #[pyo3(name = "len")] - /// See: [BiquadBank::len()] - pub fn len_py(&self) -> usize { - self.len() - } -} - -impl BiquadBank { - /// Create new biquad bank. Initialized from given vector of series biquads. - pub fn new(biqs: Vec>) -> BiquadBank { - let gains = vec![1.0; biqs.len()]; - BiquadBank { biqs, gains } - } - /// Return the number of parallel filters installed. - pub fn len(&self) -> usize { - self.biqs.len() - } - - /// Set the gains for each of the biquad. The gains are not used in the analyisis phase, but in - /// the reconstruction phase, so when BiquadBank::filter() is run on an input signal. - /// - /// # Panics - /// - /// When gains_dB.len() != to the number of filters. - pub fn set_gains_dB(&mut self, gains_dB: &[Flt]) { - if gains_dB.len() != self.gains.len() { - panic!("Invalid gains size!"); - } - self.gains - .iter_mut() - .zip(gains_dB) - .for_each(|(g, gdB)| *g = Flt::powf(10., gdB / 20.)); - } - /// Set linear gain values for each biquad. Same comments hold as for - /// [BiquadBank::set_gains_dB()]. - pub fn set_gains(&mut self, gains: &[Flt]) { - if gains.len() != self.gains.len() { - panic!("Invalid gains size!"); - } - // This could be done more efficient, but it does not matter. How often would you change - // the gain values? - self.gains.clone_from(&gains.to_vec()); - } - /// Analysis step. Runs input signal through all filters. Outputs a vector of output results, - /// one for each filter in the bank. - - pub fn analysis(&mut self, input: &[Flt]) -> Vec { - // Filtered output for each filter in biquad bank - let filtered_out: Vec = self - .biqs - .par_iter_mut() - // .iter_mut() - .map(|biq| biq.filter(input)) - .collect(); - filtered_out - } -} - -impl Filter for BiquadBank { - fn filter(&mut self, input: &[Flt]) -> Vd { - // Sum of filter output times gains - let filtered_out = self.analysis(input); - - let mut out: Vd = vec![0.; input.len()]; - - for (f, g) in filtered_out.iter().zip(&self.gains) { - for (outi, fi) in out.iter_mut().zip(f) { - // Sum and multiply by gain value - *outi += g * fi; - } - } - out - } - fn reset(&mut self) { - self.biqs.iter_mut().for_each(|b| b.reset()); - } - fn clone_dyn(&self) -> Box { - Box::new(self.clone()) - } -} - -#[cfg(test)] -mod test { - use super::*; - - #[test] - fn test_biquad1() { - let mut ser = Biquad::unit(); - let inp = vec![1., 0., 0., 0., 0., 0.]; - let filtered = ser.filter(&inp); - assert_eq!(&filtered, &inp); - } - #[test] - #[should_panic] - fn test_biquad2() { - // A a0 coefficient not in the right place, meaning we panic on unwrap - let filter_coefs = vec![1., 0., 0., 0., 0., 0.]; - let mut ser = SeriesBiquad::new(&filter_coefs).unwrap(); - let inp = vec![1., 0., 0., 0., 0., 0.]; - let filtered = ser.filter(&inp); - assert_eq!(&filtered, &inp); - } - #[test] - fn test_biquad3() { - let filter_coefs = vec![0.5, 0.5, 0., 1., 0., 0.]; - let mut ser = SeriesBiquad::new(&filter_coefs).unwrap(); - - let mut inp = vec![1., 0., 0., 0., 0., 0.]; - let filtered = ser.filter(&inp); - - // Change input to see match what should come out of output - inp[0] = 0.5; - inp[1] = 0.5; - assert_eq!(&inp, &filtered); - } - #[test] - fn test_biquadbank1() { - //! Creates two unit filters with gains of 0.5. Runs the input signal through these filters - //! in parallel and check if input == output. - let ser = Biquad::unit(); - let mut biq = BiquadBank::new(vec![ser.clone_dyn(), ser.clone_dyn()]); - biq.set_gains(&[0.5, 0.5]); - let inp = vec![1., 0., 0., 0., 0., 0.]; - let out = biq.filter(&inp); - assert_eq!(&out, &inp); - } -} diff --git a/src/filter/biquad.rs b/src/filter/biquad.rs new file mode 100644 index 0000000..d7dc638 --- /dev/null +++ b/src/filter/biquad.rs @@ -0,0 +1,164 @@ +use super::*; +use ndarray::prelude::*; +use anyhow::{Result, bail}; +use num::Complex; + +#[cfg_attr(feature = "python-bindings", pyclass)] +#[derive(Clone, Copy, Debug)] +/// # A biquad is a second order recursive filter structure. +/// +/// +pub struct Biquad { + // State parameters + w1: Flt, + w2: Flt, + // Filter coefficients - forward + b0: Flt, + b1: Flt, + b2: Flt, + // Filter coefficients - recursive + // a0: Flt, // a0 is assumed one, not used + a1: Flt, + a2: Flt, +} +#[cfg(feature = "python-bindings")] +#[cfg_attr(feature = "python-bindings", pymethods)] +impl Biquad { + #[new] + /// Create new biquad filter. See [Biquad::new()] + /// + pub fn new_py<'py>(coefs: PyReadonlyArrayDyn) -> PyResult { + Ok(Biquad::new(coefs.as_slice()?)?) + } + #[pyo3(name = "unit")] + #[staticmethod] + /// See: [Biquad::unit()] + pub fn unit_py() -> Biquad { + Biquad::unit() + } + #[pyo3(name = "firstOrderHighPass")] + #[staticmethod] + /// See: [Biquad::firstOrderHighPass()] + pub fn firstOrderHighPass_py(fs: Flt, cuton_Hz: Flt) -> PyResult { + Ok(Biquad::firstOrderHighPass(fs, cuton_Hz)?) + } + #[pyo3(name = "filter")] + /// See: [Biquad::filter()] + pub fn filter_py<'py>( + &mut self, + py: Python<'py>, + input: PyArrayLike1, + ) -> PyResult<&'py PyArray1> { + Ok(self.filter(input.as_slice()?).into_pyarray(py)) + } +} +impl Biquad { + /// Create new biquad filter from given filter coeficients + /// + /// # Args + /// + /// - coefs: Filter coefficients. + /// + pub fn new(coefs: &[Flt]) -> Result { + match coefs { + [b0, b1, b2, a0, a1, a2] => { + if *a0 != 1.0 { + bail!("Coefficient a0 should be equal to 1.0") + } + Ok(Biquad { w1: 0., w2: 0., b0: *b0, b1: *b1, b2: *b2, a1: *a1, a2: *a2}) + }, + _ => bail!("Could not initialize biquad. Please make sure that the coefficients contain 6 terms") + } + } + + /// Create unit impulse response biquad filter. Input = output + fn unit() -> Biquad { + let filter_coefs = &[1., 0., 0., 1., 0., 0.]; + Biquad::new(filter_coefs).unwrap() + } + + /// Initialize biquad as first order high pass filter. + /// + /// + /// * fs: Sampling frequency in \[Hz\] + /// * cuton_Hz: -3 dB cut-on frequency in \[Hz\] + /// + pub fn firstOrderHighPass(fs: Flt, cuton_Hz: Flt) -> Result { + if fs <= 0. { + bail!("Invalid sampling frequency: {} [Hz]", fs); + } + if cuton_Hz <= 0. { + bail!("Invalid cuton frequency: {} [Hz]", cuton_Hz); + } + if cuton_Hz >= 0.98 * fs / 2. { + bail!( + "Invalid cuton frequency. We limit this to 0.98* fs / 2. Given value {} [Hz]", + cuton_Hz + ); + } + + let tau: Flt = 1. / (2. * pi * cuton_Hz); + let facnum = 2. * fs * tau / (1. + 2. * fs * tau); + let facden = (1. - 2. * fs * tau) / (1. + 2. * fs * tau); + + let coefs = [ + facnum, // b0 + -facnum, // b1 + 0., // b2 + 1., // a0 + facden, // a1 + 0., // a2 + ]; + + Ok(Biquad::new(&coefs).unwrap()) + } + pub fn filter_inout(&mut self, inout: &mut [Flt]) { + for sample in inout.iter_mut() { + let w0 = *sample - self.a1 * self.w1 - self.a2 * self.w2; + let yn = self.b0 * w0 + self.b1 * self.w1 + self.b2 * self.w2; + self.w2 = self.w1; + self.w1 = w0; + *sample = yn; + } + // println!("{:?}", inout); + } +} +impl Filter for Biquad { + fn filter(&mut self, input: &[Flt]) -> Vec { + let mut out = input.to_vec(); + self.filter_inout(&mut out); + // println!("{:?}", out); + out + } + fn reset(&mut self) { + self.w1 = 0.; + self.w2 = 0.; + } + fn clone_dyn(&self) -> Box { + Box::new(*self) + } +} +impl TransferFunction for Biquad { + fn tf(&self, fs: Flt, freq: VdView) -> Ccol { + let res = freq.mapv(|f| { + let z = Complex::exp(I * 2. * pi * f / fs); + let num = self.b0 + self.b1 / z + self.b2 / z / z; + let den = 1. + self.a1 / z + self.a2 / z / z; + num / den + }); + res + // re + } +} +#[cfg(test)] +mod test { + use super::*; + + #[test] + fn test_biquad1() { + let mut ser = Biquad::unit(); + let inp = vec![1., 0., 0., 0., 0., 0.]; + let filtered = ser.filter(&inp); + assert_eq!(&filtered, &inp); + } +} diff --git a/src/filter/biquadbank.rs b/src/filter/biquadbank.rs new file mode 100644 index 0000000..8b77914 --- /dev/null +++ b/src/filter/biquadbank.rs @@ -0,0 +1,175 @@ +use super::*; +use super::seriesbiquad::*; +use rayon::prelude::*; +#[cfg_attr(feature = "python-bindings", pyclass)] +#[derive(Clone)] +/// Multiple biquad filter that operate in parallel on a signal, and can apply a gain value to each +/// of the returned values. The BiquadBank can be used to decompose a signal by running it through +/// parallel filters, or it can directly be used to eq a signal. For the latter process, also a +/// gain can be applied when the output is made as the sum of the filtered inputs for each biquad. +/// +/// # Detailed description +/// +/// Below is an example of the signal flow is for the case of three SeriesBiquad filters, `h1`, +/// `h2` and `h3`: +/// +/// ```markdown +/// +/// analysis() gain application sum() +/// +/// +------+ +-----+ +------------+ +/// +-----+ h1 |---+ g1 +-------------+ | | +/// | +------+ +-----+ ++ | +------ | +/// | +-----| ++ | +/// Input | +------+ +-----+ | ++ | Output of filter() +///--------|>--+-----+ h2 |---+ g2 |--------------------| +-+ +----------------|> +/// | ---+---+ +-----+ | +-+ | +/// | +-----| ++ | +/// | +------+ +-----+ ++ | ++ | +/// +-----+ h3 |---+ g3 |-------------+ | +------ | +/// +------+ +-----+ | | +/// | +------------+ +/// | +/// | Output of analysis() method (optional) +/// +-------------|> +/// ``` +pub struct BiquadBank { + biqs: Vec>, + gains: Vec, +} + +#[cfg(feature = "python-bindings")] +#[cfg_attr(feature = "python-bindings", pymethods)] +/// Methods to wrap it in Python +impl BiquadBank { + #[new] + /// Create new biquadbank filter set. See [BiquadBank::new()] + /// + pub fn new_py<'py>(coefs: PyReadonlyArrayDyn) -> PyResult { + let mut filts = vec![]; + for col in coefs.as_array().columns() { + match col.as_slice() { + Some(colslice) => { + let new_ser = SeriesBiquad::new(colslice)?; + filts.push(new_ser.clone_dyn()); + } + None => { + return Err(PyValueError::new_err("Error generating column")); + } + } + } + Ok(BiquadBank::new(filts)) + } + #[pyo3(name = "filter")] + /// See: [BiquadBank::filter()] + pub fn filter_py<'py>( + &mut self, + py: Python<'py>, + input: PyArrayLike1, + ) -> PyResult<&'py PyArray1> { + Ok(self.filter(input.as_slice()?).into_pyarray(py)) + } + #[pyo3(name = "reset")] + /// See: [BiquadBank::reset()] + pub fn reset_py(&mut self) { + self.reset(); + } + + #[pyo3(name = "set_gains")] + /// See: [BiquadBank::set_gains()] + pub fn set_gains_py<'py>(&mut self, gains: PyArrayLike1) -> PyResult<()> { + if gains.len()? != self.len() { + return Err(PyValueError::new_err("Invalid number of provided gains")); + } + self.set_gains(gains.as_slice()?); + Ok(()) + } + #[pyo3(name = "set_gains_dB")] + /// See: [BiquadBank::set_gains_dB()] + pub fn set_gains_dB_py<'py>(&mut self, gains_dB: PyArrayLike1) -> PyResult<()> { + if gains_dB.len()? != self.len() { + return Err(PyValueError::new_err("Invalid number of provided gains")); + } + self.set_gains_dB(gains_dB.as_slice()?); + Ok(()) + } + #[pyo3(name = "len")] + /// See: [BiquadBank::len()] + pub fn len_py(&self) -> usize { + self.len() + } +} + +impl BiquadBank { + /// Create new biquad bank. Initialized from given vector of series biquads. + pub fn new(biqs: Vec>) -> BiquadBank { + let gains = vec![1.0; biqs.len()]; + BiquadBank { biqs, gains } + } + /// Return the number of parallel filters installed. + pub fn len(&self) -> usize { + self.biqs.len() + } + + /// Set the gains for each of the biquad. The gains are not used in the analyisis phase, but in + /// the reconstruction phase, so when BiquadBank::filter() is run on an input signal. + /// + /// # Panics + /// + /// When gains_dB.len() != to the number of filters. + pub fn set_gains_dB(&mut self, gains_dB: &[Flt]) { + if gains_dB.len() != self.gains.len() { + panic!("Invalid gains size!"); + } + self.gains + .iter_mut() + .zip(gains_dB) + .for_each(|(g, gdB)| *g = Flt::powf(10., gdB / 20.)); + } + /// Set linear gain values for each biquad. Same comments hold as for + /// [BiquadBank::set_gains_dB()]. + pub fn set_gains(&mut self, gains: &[Flt]) { + if gains.len() != self.gains.len() { + panic!("Invalid gains size!"); + } + // This could be done more efficient, but it does not matter. How often would you change + // the gain values? + self.gains.clone_from(&gains.to_vec()); + } + /// Analysis step. Runs input signal through all filters. Outputs a vector of output results, + /// one for each filter in the bank. + + pub fn analysis(&mut self, input: &[Flt]) -> Vec { + // Filtered output for each filter in biquad bank + let filtered_out: Vec = self + .biqs + .par_iter_mut() + // .iter_mut() + .map(|biq| biq.filter(input)) + .collect(); + filtered_out + } +} + +impl Filter for BiquadBank { + fn filter(&mut self, input: &[Flt]) -> Vd { + // Sum of filter output times gains + let filtered_out = self.analysis(input); + + let mut out: Vd = vec![0.; input.len()]; + + for (f, g) in filtered_out.iter().zip(&self.gains) { + for (outi, fi) in out.iter_mut().zip(f) { + // Sum and multiply by gain value + *outi += g * fi; + } + } + out + } + fn reset(&mut self) { + self.biqs.iter_mut().for_each(|b| b.reset()); + } + fn clone_dyn(&self) -> Box { + Box::new(self.clone()) + } +} \ No newline at end of file diff --git a/src/filter/mod.rs b/src/filter/mod.rs new file mode 100644 index 0000000..f59ebc2 --- /dev/null +++ b/src/filter/mod.rs @@ -0,0 +1,45 @@ +//! # Filter implemententations for biquads, series of biquads and banks of series of biquads. +//! +//! Contains [Biquad], [SeriesBiquad], and [BiquadBank]. These are all constructs that work on +//! blocks of input data, and apply filters on it. Todo: implement FIR filter. +#![allow(non_snake_case)] +pub use super::config::*; + +mod biquad; +mod biquadbank; +mod seriesbiquad; + +pub use biquad::Biquad; +pub use biquadbank::BiquadBank; +pub use seriesbiquad::SeriesBiquad; + +/// Implementations of this trait are able to DSP-filter input data. +pub trait Filter: Send { + //! The filter trait is implemented by Biquad, SeriesBiquad, and BiquadBank + + /// Filter input to generate output. A vector of output floats is generated with the same + /// length as input. + fn filter(&mut self, input: &[Flt]) -> Vd; + /// Reset the filter state(s). In essence, this makes sure that all memory of the past is + /// forgotten. + fn reset(&mut self); + + /// Required method for cloning a BiquadBank, such that arbitrary filter types can be used as + /// their 'channels'. + fn clone_dyn(&self) -> Box; +} + +/// Implementations are able to generate transfer functions of itself + +pub trait TransferFunction: Send { + /// Compute frequency response (i.e. transfer function from input to output) + /// + /// Args + fn tf(&self, fs: Flt, freq: VdView) -> Ccol; +} + +impl Clone for Box { + fn clone(&self) -> Self { + self.clone_dyn() + } +} diff --git a/src/filter/seriesbiquad.rs b/src/filter/seriesbiquad.rs new file mode 100644 index 0000000..e6ab7ff --- /dev/null +++ b/src/filter/seriesbiquad.rs @@ -0,0 +1,137 @@ + +/// Series of biquads that filter sequentially on an input signal +/// +/// # Examples +/// +/// See (tests) +/// +use super::*; +use super::biquad::Biquad; +use anyhow::{bail, Result}; + +#[derive(Clone, Debug)] +#[cfg_attr(feature = "python-bindings", pyclass)] + +pub struct SeriesBiquad { + biqs: Vec, +} + +#[cfg(feature = "python-bindings")] +#[cfg_attr(feature = "python-bindings", pymethods)] +impl SeriesBiquad { + #[new] + /// Create new series filter set. See [SeriesBiquad::new()] + /// + pub fn new_py<'py>(coefs: PyReadonlyArrayDyn) -> PyResult { + Ok(SeriesBiquad::new(coefs.as_slice()?)?) + } + #[pyo3(name = "unit")] + #[staticmethod] + /// See: [Biquad::unit()] + pub fn unit_py() -> SeriesBiquad { + SeriesBiquad::unit() + } + #[pyo3(name = "filter")] + /// See: [SeriesBiquad::filter()] + pub fn filter_py<'py>( + &mut self, + py: Python<'py>, + input: PyArrayLike1, + ) -> PyResult<&'py PyArray1> { + Ok(self.filter(input.as_slice()?).into_pyarray(py)) + } + #[pyo3(name = "reset")] + /// See: [SeriesBiquad::reset()] + pub fn reset_py(&mut self) { + self.reset(); + } +} +impl SeriesBiquad { + /// Create a new series biquad, having an arbitrary number of biquads. + /// + /// # Arguments + /// + /// * `filter_coefs` - Vector of biquad coefficients, stored in a single array. The first six + /// for the first biquad, and so on. + /// + /// + pub fn new(filter_coefs: &[Flt]) -> Result { + if filter_coefs.len() % 6 != 0 { + bail!( + "filter_coefs should be multiple of 6, given: {}.", + filter_coefs.len() + ); + } + let nfilters = filter_coefs.len() / 6; + + let mut biqs: Vec = Vec::with_capacity(nfilters); + for coefs in filter_coefs.chunks(6) { + let biq = Biquad::new(coefs)?; + biqs.push(biq); + } + + if biqs.is_empty() { + bail!("No filter coefficients given!"); + } + + Ok(SeriesBiquad { biqs }) + } + + /// Unit impulse response series biquad. Input = output + pub fn unit() -> SeriesBiquad { + let filter_coefs = &[1., 0., 0., 1., 0., 0.]; + SeriesBiquad::new(filter_coefs).unwrap() + } + fn clone_dyn(&self) -> Box { + Box::new(self.clone()) + } +} + +impl Filter for SeriesBiquad { + //! Filter input by applying all biquad filters in series on each input sample, to obtain the + //! output samples. + //! + fn filter(&mut self, input: &[Flt]) -> Vd { + let mut inout = input.to_vec(); + for biq in self.biqs.iter_mut() { + biq.filter_inout(&mut inout); + } + inout + } + fn reset(&mut self) { + self.biqs.iter_mut().for_each(|f| f.reset()); + } + fn clone_dyn(&self) -> Box { + Box::new(self.clone()) + } +} + + +#[cfg(test)] +mod test { + use super::*; + + #[test] + #[should_panic] + fn test_biquad2() { + // A a0 coefficient not in the right place, meaning we panic on unwrap + let filter_coefs = vec![1., 0., 0., 0., 0., 0.]; + let mut ser = SeriesBiquad::new(&filter_coefs).unwrap(); + let inp = vec![1., 0., 0., 0., 0., 0.]; + let filtered = ser.filter(&inp); + assert_eq!(&filtered, &inp); + } + #[test] + fn test_biquad3() { + let filter_coefs = vec![0.5, 0.5, 0., 1., 0., 0.]; + let mut ser = SeriesBiquad::new(&filter_coefs).unwrap(); + + let mut inp = vec![1., 0., 0., 0., 0., 0.]; + let filtered = ser.filter(&inp); + + // Change input to see match what should come out of output + inp[0] = 0.5; + inp[1] = 0.5; + assert_eq!(&inp, &filtered); + } +} \ No newline at end of file diff --git a/src/lib.rs b/src/lib.rs index 68f9335..0e9ab29 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -2,29 +2,24 @@ //! //! This crate contains structures and functions to perform acoustic measurements, interact with //! data acquisition devices and apply common acoustic analysis operations on them. - #![warn(missing_docs)] #![allow(non_snake_case)] #![allow(non_upper_case_globals)] #![allow(unused_imports)] -mod config; -pub mod filter; +mod config; +use config::*; + +pub use config::Flt; // pub mod window; // pub mod ps; +pub mod filter; pub mod daq; pub mod siggen; -pub use config::*; -pub use daq::*; - -cfg_if::cfg_if! { -if #[cfg(feature = "python-bindings")] { - use pyo3::prelude::*; - use pyo3::{pymodule, PyResult}; -} else {} } +use filter::*; /// A Python module implemented in Rust.