From 31ab7e6fbf0ac86669b79ab4c6b389d7d382836d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 2 Jan 2024 23:50:36 +0100 Subject: [PATCH 01/26] rework iir filter --- CHANGELOG.md | 10 + benches/micro.rs | 12 +- src/complex.rs | 4 +- src/iir.rs | 609 ++++++++++++++++++++++++++++++++++++++--------- 4 files changed, 521 insertions(+), 114 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4f4915d..db93ad2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,16 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added * `hbf` FIRs, symmetric FIRs, half band filters, HBF decimators and interpolators +* `iir::PidBuilder` a builder for PID coefficients +* `iir::Biquad::{hold, proportional, identity}` + +### Removed + +* `iir::Vec5` type alias has been removed. + +### Changed + +* `iir`: The biquad IIR filter API has been reworked. `IIR -> Biquad` renamed. ## [0.10.0](https://github.com/quartiq/idsp/compare/v0.9.2..v0.10.0) - 2023-07-20 diff --git a/benches/micro.rs b/benches/micro.rs index 9fb552d..7292332 100644 --- a/benches/micro.rs +++ b/benches/micro.rs @@ -62,20 +62,20 @@ fn iir_int_bench() { } fn iir_f32_bench() { - let dut = iir::IIR::::default(); - let mut xy = iir::Vec5::default(); + let dut = iir::Biquad::::default(); + let mut xy = [0.0; 5]; println!( "int::IIR::::update(s, x): {}", - bench_env(0.32241, |x| dut.update(&mut xy, *x, true)) + bench_env(0.32241, |x| dut.update(&mut xy, *x)) ); } fn iir_f64_bench() { - let dut = iir::IIR::::default(); - let mut xy = iir::Vec5::default(); + let dut = iir::Biquad::::default(); + let mut xy = [0.0; 5]; println!( "int::IIR::::update(s, x): {}", - bench_env(0.32241, |x| dut.update(&mut xy, *x, true)) + bench_env(0.32241, |x| dut.update(&mut xy, *x)) ); } diff --git a/src/complex.rs b/src/complex.rs index 87c6c58..91ba4f4 100644 --- a/src/complex.rs +++ b/src/complex.rs @@ -20,8 +20,8 @@ impl ComplexExt for Complex { /// ``` /// use idsp::{Complex, ComplexExt}; /// Complex::::from_angle(0); - /// Complex::::from_angle(1 << 30); // pi/2 - /// Complex::::from_angle(-1 << 30); // -pi/2 + /// Complex::::from_angle(1 << 30); // pi/2 + /// Complex::::from_angle(-1 << 30); // -pi/2 /// ``` fn from_angle(angle: i32) -> Self { let (c, s) = cossin(angle); diff --git a/src/iir.rs b/src/iir.rs index 3217bc9..7e501f1 100644 --- a/src/iir.rs +++ b/src/iir.rs @@ -1,25 +1,28 @@ use serde::{Deserialize, Serialize}; -use super::{abs, copysign, macc}; -use core::iter::Sum; -use num_traits::{clamp, Float, One, Zero}; +use num_traits::{clamp, Float}; -/// IIR state and coefficients type. +/// # Coefficients and state /// -/// To represent the IIR state (input and output memory) during the filter update -/// this contains the three inputs (x0, x1, x2) and the two outputs (y1, y2) +/// `[T; 5]` is both the IIR state and coefficients type. +/// +/// To represent the IIR state (input and output memory) during [`Biquad::update()`] +/// this contains the three inputs `[x0, x1, x2]` and the two outputs `[y1, y2]` /// concatenated. Lower indices correspond to more recent samples. /// To represent the IIR coefficients, this contains the feed-forward -/// coefficients (b0, b1, b2) followd by the negated feed-back coefficients -/// (-a1, -a2), all five normalized such that a0 = 1. -pub type Vec5 = [T; 5]; - -/// IIR configuration. +/// coefficients `[b0, b1, b2]` followd by the negated feed-back coefficients +/// `[a1, a2]`, all five normalized such that `a0 = 1`. +/// Note that between filter [`Biquad::update()`] the `xy` state contains +/// `[x0, x1, y0, y1, y2]`. +/// +/// The IIR coefficients can be mapped to other transfer function +/// representations, for example as described in +/// +/// # IIR filter as PID controller /// -/// Contains the coeeficients `ba`, the output offset `y_offset`, and the -/// output limits `y_min` and `y_max`. Data is represented in variable precision -/// floating-point. The dataformat is the same for all internal signals, input -/// and output. +/// Contains the coeeficients `ba`, the summing junction offset `u`, and the +/// output limits `min` and `max`. Data is represented in floating-point +/// for all internal signals, input and output. /// /// This implementation achieves several important properties: /// @@ -34,125 +37,519 @@ pub type Vec5 = [T; 5]; /// * It has universal derivative-kick (undesired, unlimited, and un-physical /// amplification of set-point changes by the derivative term) avoidance. /// * An offset at the input of an IIR filter (a.k.a. "set-point") is -/// equivalent to an offset at the output. They are related by the -/// overall (DC feed-forward) gain of the filter. +/// equivalent to an offset at the summing junction (in output units). +/// They are related by the overall (DC feed-forward) gain of the filter. /// * It stores only previous outputs and inputs. These have direct and -/// invariant interpretation (independent of gains and offsets). -/// Therefore it can trivially implement bump-less transfer. +/// invariant interpretation (independent of coefficients and offset). +/// Therefore it can trivially implement bump-less transfer between any +/// coefficients/offset sets. /// * Cascading multiple IIR filters allows stable and robust /// implementation of transfer functions beyond bequadratic terms. /// -/// # Serialization/Deserialization/Miniconf -/// -/// `{"y_offset": y_offset, "y_min": y_min, "y_max": y_max, "ba": [b0, b1, b2, a1, a2]}` -/// -/// * `y0` is the output offset code -/// * `ym` is the lower saturation limit -/// * `yM` is the upper saturation limit -/// -/// IIR filter tap gains (`ba`) are an array `[b0, b1, b2, a1, a2]` such that the -/// new output is computed as `y0 = a1*y1 + a2*y2 + b0*x0 + b1*x1 + b2*x2`. -/// The IIR coefficients can be mapped to other transfer function -/// representations, for example as described in -#[derive(Copy, Clone, Debug, Default, Deserialize, Serialize)] -pub struct IIR { - pub ba: Vec5, - pub y_offset: T, - pub y_min: T, - pub y_max: T, +/// See also . +#[derive(Copy, Clone, Debug, Deserialize, Serialize, PartialEq, Eq, PartialOrd, Ord)] +pub struct Biquad { + ba: [T; 5], + u: T, + min: T, + max: T, +} + +impl Default for Biquad { + fn default() -> Self { + Self { + ba: [T::zero(); 5], + u: T::zero(), + min: T::neg_infinity(), + max: T::infinity(), + } + } } -impl> IIR { - pub fn new(gain: T, y_min: T, y_max: T) -> Self { - let mut ba = [T::zero(); 5]; - ba[0] = gain; +impl From<[T; 5]> for Biquad { + fn from(ba: [T; 5]) -> Self { Self { ba, - y_offset: T::zero(), - y_min, - y_max, + ..Default::default() } } +} - /// Configures IIR filter coefficients for proportional-integral behavior - /// with gain limit. +impl Biquad { + /// Filter coefficients /// - /// # Arguments + /// IIR filter tap gains (`ba`) are an array `[b0, b1, b2, a1, a2]` such that + /// `y0 = clamp(b0*x0 + b1*x1 + b2*x2 + a1*y1 + a2*y2 + u, min, max)`. + pub fn ba(&self) -> &[T; 5] { + &self.ba + } + + /// Mutable reference to the filter coefficients. /// - /// * `kp` - Proportional gain. Also defines gain sign. - /// * `ki` - Integral gain at Nyquist. Sign taken from `kp`. - /// * `g` - Gain limit. - pub fn set_pi(&mut self, kp: T, ki: T, g: T) -> Result<(), &str> { - let ki = copysign(ki, kp); - let g = copysign(g, kp); - let (a1, b0, b1) = if abs(ki) < T::epsilon() { - (T::zero(), kp, T::zero()) - } else { - let c = if abs(g) < T::epsilon() { - T::one() - } else { - T::one() / (T::one() + ki / g) - }; - let a1 = (T::one() + T::one()) * c - T::one(); - let b0 = ki * c + kp; - let b1 = ki * c - a1 * kp; - if abs(b0 + b1) < T::epsilon() { - return Err("low integrator gain and/or gain limit"); - } - (a1, b0, b1) - }; - self.ba.copy_from_slice(&[b0, b1, T::zero(), a1, T::zero()]); - Ok(()) + /// See [`Biquad::ba()`]. + pub fn ba_mut(&mut self) -> &mut [T; 5] { + &mut self.ba } - /// Compute the overall (DC feed-forward) gain. - pub fn get_k(&self) -> T { - self.ba[..3].iter().copied().sum() + /// Summing junction offset + /// + /// This offset is applied to the output `y0` summing junction + /// on top of the feed-forward (`b`) and feed-back (`a`) terms. + /// The feedback samples are taken at the summing junction and + /// thus also include (and feed back) this offset. + pub fn u(&self) -> T { + self.u } - // /// Compute input-referred (`x`) offset from output (`y`) offset. - pub fn get_x_offset(&self) -> Result { - let k = self.get_k(); - if abs(k) < T::epsilon() { - Err("k is zero") - } else { - Ok(self.y_offset / k) - } + /// Set the summing junction offset + /// + /// See [`Biquad::u()`]. + pub fn set_u(&mut self, u: T) { + self.u = u; } - /// Convert input (`x`) offset to equivalent output (`y`) offset and apply. + + /// Lower output limit + /// + /// Guaranteed minimum output value. + /// The value is inclusive. + /// The clamping also cleanly affects the feedback terms. + pub fn min(&self) -> T { + self.min + } + + /// Set the lower output limit + /// + /// See [`Biquad::min()`]. + pub fn set_min(&mut self, min: T) { + self.max = min; + } + + /// Upper output limit + /// + /// Guaranteed maximum output value. + /// The value is inclusive. + /// The clamping also cleanly affects the feedback terms. + pub fn max(&self) -> T { + self.max + } + + /// Set the upper output limit + /// + /// See [`Biquad::max()`]. + pub fn set_max(&mut self, max: T) { + self.max = max; + } + + /// A unit gain filter + /// + /// ``` + /// # use idsp::iir::*; + /// let x0 = 3.0; + /// let y0 = Biquad::identity().update(&mut [0.0; 5], x0); + /// assert_eq!(y0, x0); + /// ``` + pub fn identity() -> Self { + Self::proportional(T::one()) + } + + /// A filter with the given proportional gain at all frequencies + /// + /// ``` + /// # use idsp::iir::*; + /// let x0 = 2.0; + /// let k = 5.0; + /// let y0 = Biquad::proportional(k).update(&mut [0.0; 5], x0); + /// assert_eq!(y0, x0 * k); + /// ``` + pub fn proportional(k: T) -> Self { + let mut s = Self::default(); + s.ba[0] = k; + s + } + + /// A "hold" filter that ingests input and maintains output + /// + /// ``` + /// # use idsp::iir::*; + /// let mut xy = core::array::from_fn(|i| i as _); + /// let x0 = 7.0; + /// let y0 = Biquad::hold().update(&mut xy, x0); + /// assert_eq!(y0, 2.0); + /// assert_eq!(xy, [x0, 0.0, y0, y0, 3.0]); + /// ``` + pub fn hold() -> Self { + let mut s = Self::default(); + s.ba[3] = T::one(); + s + } + + // TODO + // lowpass1 + // highpass1 + // butterworth + // elliptic + // chebychev1/2 + // bessel + // invert // high-to-low/low-to-high + // notch + // PI-notch + // SOS cascades thereoff + + /// Return a builder for a "PID" controller + /// + /// ``` + /// # use idsp::iir::*; + /// let i = Biquad::::pid().build().unwrap(); + /// assert_eq!(i, Biquad::default()); + /// ``` + pub fn pid() -> PidBuilder { + PidBuilder::default() + } + + /// Compute the overall (DC/proportional feed-forward) gain. + /// + /// ``` + /// # use idsp::iir::*; + /// assert_eq!(Biquad::proportional(3.0).forward_gain(), 3.0); + /// ``` + /// + /// # Returns + /// The sum of the `b` feed-forward coefficients. + pub fn forward_gain(&self) -> T { + self.ba[0] + self.ba[1] + self.ba[2] + } + + /// Compute input-referred (`x`) offset. + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::proportional(3.0); + /// i.set_input_offset(2.0); + /// assert_eq!(i.input_offset(), 2.0); + /// ``` + pub fn input_offset(&self) -> T { + self.u / self.forward_gain() + } + /// Convert input (`x`) offset to equivalent summing junction offset (`u`) and apply. + /// + /// In the case of a "PID" controller the response behavior of the controller + /// to the offset is "stabilizing", and not "tracking": its frequency response + /// is exclusively according to the lowest non-zero [`PidAction`] gain. + /// There is no high order ("faster") response as would be the case for a "tracking" + /// controller. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::proportional(3.0); + /// i.set_input_offset(2.0); + /// assert_eq!(i.input_offset(), 2.0); + /// let x0 = 0.5; + /// let y0 = i.update(&mut [0.0; 5], x0); + /// assert_eq!(y0, (x0 + i.input_offset()) * i.forward_gain()); + /// ``` /// /// # Arguments - /// * `xo`: Input (`x`) offset. - pub fn set_x_offset(&mut self, xo: T) { - self.y_offset = xo * self.get_k(); + /// * `offset`: Input (`x`) offset. + pub fn set_input_offset(&mut self, offset: T) { + self.u = offset * self.forward_gain(); } - /// Feed a new input value into the filter, update the filter state, and + /// Ingest a new input value into the filter, update the filter state, and /// return the new output. Only the state `xy` is modified. /// + /// ``` + /// # use idsp::iir::*; + /// let mut xy = core::array::from_fn(|i| i as _); + /// let x0 = 3.0; + /// let y0 = Biquad::identity().update(&mut xy, x0); + /// assert_eq!(y0, x0); + /// assert_eq!(xy, [x0, 0.0, y0, 2.0, 3.0]); + /// ``` + /// /// # Arguments /// * `xy` - Current filter state. /// * `x0` - New input. - pub fn update(&self, xy: &mut Vec5, x0: T, hold: bool) -> T { - let n = self.ba.len(); - debug_assert!(xy.len() == n); - // `xy` contains x0 x1 y0 y1 y2 - // Increment time x1 x2 y1 y2 y3 - // Shift x1 x1 x2 y1 y2 - // This unrolls better than xy.rotate_right(1) - xy.copy_within(0..n - 1, 1); - // Store x0 x0 x1 x2 y1 y2 + /// + /// # Returns + /// The new output `y0`. + /// + /// # Panics + /// Panics in debug mode if `!(self.min <= self.max)`. + pub fn update(&self, xy: &mut [T; 5], x0: T) -> T { + // `xy` contains x0 x1 y0 y1 y2 + // Increment time x1 x2 y1 y2 y3 + // Shift x1 x1 x2 y1 y2 + xy.copy_within(0..4, 1); + // Store x0 x0 x1 x2 y1 y2 xy[0] = x0; - // Compute y0 by multiply-accumulate - let y0 = if hold { - xy[n / 2 + 1] - } else { - macc(self.y_offset, xy, &self.ba) - }; - // Limit y0 - let y0 = clamp(y0, self.y_min, self.y_max); - // Store y0 x0 x1 y0 y1 y2 - xy[n / 2] = y0; + let y0 = xy + .iter() + .zip(self.ba.iter()) + .fold(self.u, |y, (x, a)| y + *x * *a); + let y0 = clamp(y0, self.min, self.max); + // Store y0 x0 x1 y0 y1 y2 + xy[2] = y0; y0 } } + +/// PID controller builder +/// +/// Builds `Biquad` from action gains, gain limits, input offset and output limits. +/// +/// ``` +/// # use idsp::iir::*; +/// let b = Biquad::pid() +/// .period(1e-3) +/// .gain(PidAction::Ki, 1e-3) +/// .gain(PidAction::Kp, 1.0) +/// .gain(PidAction::Kd, 1e2) +/// .limit(PidAction::Ki, 1e3) +/// .limit(PidAction::Kd, 1e1) +/// .build() +/// .unwrap(); +/// ``` +#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize)] +pub struct PidBuilder { + period: T, + gains: [T; 5], + limits: [T; 5], +} + +impl Default for PidBuilder { + fn default() -> Self { + Self { + period: T::one(), + gains: [T::zero(); 5], + limits: [T::infinity(); 5], + } + } +} + +/// [`PidBuilder::build()`] errors +#[derive(Copy, Clone, Debug, PartialEq, Eq, Ord, PartialOrd, Serialize, Deserialize)] +#[non_exhaustive] +pub enum PidError { + /// The action gains cover more than three successive orders + OrderRange, +} + +/// PID action +/// +/// This enumerates the five possible PID style actions of a [`Biquad`] +#[derive(Copy, Clone, Debug, PartialEq, Eq, Ord, PartialOrd, Serialize, Deserialize)] +pub enum PidAction { + /// Double integrating, -40 dB per decade + Kii = 0, + /// Integrating, -20 dB per decade + Ki = 1, + /// Proportional + Kp = 2, + /// Derivative=, 20 dB per decade + Kd = 3, + /// Double derivative, 40 dB per decade + Kdd = 4, +} + +impl PidBuilder { + /// Sample period + /// + /// # Arguments + /// * `period`: Sample period in some units, e.g. SI seconds + pub fn period(mut self, period: T) -> Self { + self.period = period; + self + } + + /// Gain for a given action + /// + /// Gain units are `output/input * time.powi(order)` where + /// * `output` are output (`y`) units + /// * `input` are input (`x`) units + /// * `time` are sample period units, e.g. SI seconds + /// * `order` is the action order: the frequency exponent + /// (`-1` for integrating, `0` for proportional, etc.) + /// + /// Note that inverse time units correspond to angular frequency units. + /// Gains are accurate in the low frequency limit. Towards Nyquist, the + /// frequency response is wrapped. + /// + /// ``` + /// # use idsp::iir::*; + /// let tau = 1e-3; + /// let ki = 1e-4; + /// let i = Biquad::pid() + /// .period(tau) + /// .gain(PidAction::Ki, ki) + /// .build() + /// .unwrap(); + /// let x0 = 5.0; + /// let y0 = i.update(&mut [0.0; 5], x0); + /// assert_eq!(y0, x0 * ki / tau); + /// ``` + /// + /// # Arguments + /// * `action`: Action to control + /// * `gain`: Gain value + pub fn gain(mut self, action: PidAction, gain: T) -> Self { + self.gains[action as usize] = gain; + self + } + + /// Gain limit for a given action + /// + /// Gain limit units are `output/input`. See also [`PidBuilder::gain()`]. + /// Multiple gains and limits may interact and lead to peaking. + /// + /// ``` + /// # use idsp::iir::*; + /// let ki_limit = 1e3; + /// let i = Biquad::pid() + /// .gain(PidAction::Ki, 8.0) + /// .limit(PidAction::Ki, ki_limit) + /// .build() + /// .unwrap(); + /// let mut xy = [0.0; 5]; + /// let x0 = 5.0; + /// for _ in 0..1000 { + /// i.update(&mut xy, x0); + /// } + /// let y0 = i.update(&mut xy, x0); + /// assert!((y0 / (x0 * ki_limit) - 1.0f32).abs() < 1e-3); + /// ``` + /// + /// # Arguments + /// * `action`: Action to limit in gain + /// * `limit`: Gain limit + pub fn limit(mut self, action: PidAction, limit: T) -> Self { + self.limits[action as usize] = limit; + self + } + + /// Perform checks, compute coefficients and return `Biquad`. + /// + /// No attempt is made to detect NaNs, non-finite gains, non-positive period, + /// zero gain limits, or gain/limit sign mismatches. + /// These will consequently result in NaNs/infinities, peaking, or notches in + /// the Biquad coefficients. + /// + /// Gain limits for zero gain actions or for proportional action are ignored. + /// + /// ``` + /// # use idsp::iir::*; + /// let i = Biquad::::pid() + /// .gain(PidAction::Kp, 3.0) + /// .build() + /// .unwrap(); + /// assert_eq!(i, Biquad::proportional(3.0)); + /// ``` + pub fn build(self) -> Result, PidError> { + const KP: usize = PidAction::Kp as usize; + + // Determine highest denominator (feedback, `a`) order + let low = self + .gains + .iter() + .take(KP) + .position(|g| !g.is_zero()) + .unwrap_or(KP); + + if self.gains.iter().skip(low + 3).any(|g| !g.is_zero()) { + return Err(PidError::OrderRange); + } + + // Derivative/integration kernels + let kernels = [ + [T::one(), T::zero(), T::zero()], + [T::one(), -T::one(), T::zero()], + [T::one(), -T::one() - T::one(), T::one()], + ]; + + // Coefficients + let mut b = [T::zero(); 3]; + let mut a = [T::zero(); 3]; + + let mut zi = self.period.powi(low as i32 - KP as i32); + + for ((i, (gi, li)), ki) in self + .gains + .iter() + .zip(self.limits.iter()) + .enumerate() + .skip(low) + .zip(kernels.iter()) + { + // Scale gains and compute limits in place + let gi = *gi * zi; + zi = zi * self.period; + let li = if i == KP { T::one() } else { gi / *li }; + + for (j, (bj, aj)) in b.iter_mut().zip(a.iter_mut()).enumerate() { + *bj = *bj + gi * ki[j]; + *aj = *aj + li * ki[j]; + } + } + + // Normalize + let a0 = T::one() / a[0]; + for baj in b.iter_mut().chain(a.iter_mut().skip(1)) { + *baj = *baj * a0; + } + + Ok(Biquad { + ba: [b[0], b[1], b[2], -a[1], -a[2]], + ..Default::default() + }) + } +} + +#[cfg(test)] +mod test { + use super::*; + + #[test] + fn pid() { + let b = Biquad::pid() + .period(1.0f32) + .gain(PidAction::Ki, 1e-3) + .gain(PidAction::Kp, 1.0) + .gain(PidAction::Kd, 1e2) + .limit(PidAction::Ki, 1e3) + .limit(PidAction::Kd, 1e1) + .build() + .unwrap(); + let want = [ + 9.18190826, + -18.27272561, + 9.09090826, + 1.90909074, + -0.90909083, + ]; + for (ba_have, ba_want) in b.ba.iter().zip(want.iter()) { + assert!( + (ba_have / ba_want - 1.0).abs() < 2.0 * f32::EPSILON, + "have {:?} != want {want:?}", + &b.ba, + ); + } + } + + #[test] + fn units() { + let ki = 5e-2; + let tau = 3e-3; + let b = Biquad::pid() + .period(tau) + .gain(PidAction::Ki, ki) + .build() + .unwrap(); + let mut xy = [0.0; 5]; + for i in 1..10 { + let y_have = b.update(&mut xy, 1.0); + let y_want = (i as f32) * (ki / tau); + assert!( + (y_have / y_want - 1.0).abs() < 3.0 * f32::EPSILON, + "{i}: have {y_have} != {y_want}" + ); + } + } +} From b6c487c7e04cf0419306f649ebf87435a64c42d6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 4 Jan 2024 22:00:17 +0100 Subject: [PATCH 02/26] iir: fix set_min() --- src/iir.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/iir.rs b/src/iir.rs index 7e501f1..6d8784a 100644 --- a/src/iir.rs +++ b/src/iir.rs @@ -121,7 +121,7 @@ impl Biquad { /// /// See [`Biquad::min()`]. pub fn set_min(&mut self, min: T) { - self.max = min; + self.min = min; } /// Upper output limit From c8d3adb97852a3cd9c6d58837faead5e8d131281 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 5 Jan 2024 23:06:32 +0100 Subject: [PATCH 03/26] iir: coefficient builders independent of Biquad --- src/iir.rs | 176 +++++++++++++++++++++++++---------------------------- 1 file changed, 84 insertions(+), 92 deletions(-) diff --git a/src/iir.rs b/src/iir.rs index 6d8784a..d02c9a9 100644 --- a/src/iir.rs +++ b/src/iir.rs @@ -47,7 +47,7 @@ use num_traits::{clamp, Float}; /// implementation of transfer functions beyond bequadratic terms. /// /// See also . -#[derive(Copy, Clone, Debug, Deserialize, Serialize, PartialEq, Eq, PartialOrd, Ord)] +#[derive(Copy, Clone, Debug, Deserialize, Serialize, PartialEq, Eq, PartialOrd)] pub struct Biquad { ba: [T; 5], u: T, @@ -75,6 +75,61 @@ impl From<[T; 5]> for Biquad { } } +/// A unit gain filter +/// +/// ``` +/// # use idsp::iir::*; +/// let x0 = 3.0; +/// let y0 = Biquad::from(identity()).update(&mut [0.0; 5], x0); +/// assert_eq!(y0, x0); +/// ``` +pub fn identity() -> [T; 5] { + proportional(T::one()) +} + +/// A filter with the given proportional gain at all frequencies +/// +/// ``` +/// # use idsp::iir::*; +/// let x0 = 2.0; +/// let k = 5.0; +/// let y0 = Biquad::from(proportional(k)).update(&mut [0.0; 5], x0); +/// assert_eq!(y0, x0 * k); +/// ``` +pub fn proportional(k: T) -> [T; 5] { + let mut ba = [T::zero(); 5]; + ba[0] = k; + ba +} + +/// A "hold" filter that ingests input and maintains output +/// +/// ``` +/// # use idsp::iir::*; +/// let mut xy = core::array::from_fn(|i| i as _); +/// let x0 = 7.0; +/// let y0 = Biquad::from(hold()).update(&mut xy, x0); +/// assert_eq!(y0, 2.0); +/// assert_eq!(xy, [x0, 0.0, y0, y0, 3.0]); +/// ``` +pub fn hold() -> [T; 5] { + let mut ba = [T::zero(); 5]; + ba[3] = T::one(); + ba +} + +// TODO +// lowpass1 +// highpass1 +// butterworth +// elliptic +// chebychev1/2 +// bessel +// invert // high-to-low/low-to-high +// notch +// PI-notch +// SOS cascades thereoff + impl Biquad { /// Filter coefficients /// @@ -140,77 +195,11 @@ impl Biquad { self.max = max; } - /// A unit gain filter - /// - /// ``` - /// # use idsp::iir::*; - /// let x0 = 3.0; - /// let y0 = Biquad::identity().update(&mut [0.0; 5], x0); - /// assert_eq!(y0, x0); - /// ``` - pub fn identity() -> Self { - Self::proportional(T::one()) - } - - /// A filter with the given proportional gain at all frequencies - /// - /// ``` - /// # use idsp::iir::*; - /// let x0 = 2.0; - /// let k = 5.0; - /// let y0 = Biquad::proportional(k).update(&mut [0.0; 5], x0); - /// assert_eq!(y0, x0 * k); - /// ``` - pub fn proportional(k: T) -> Self { - let mut s = Self::default(); - s.ba[0] = k; - s - } - - /// A "hold" filter that ingests input and maintains output - /// - /// ``` - /// # use idsp::iir::*; - /// let mut xy = core::array::from_fn(|i| i as _); - /// let x0 = 7.0; - /// let y0 = Biquad::hold().update(&mut xy, x0); - /// assert_eq!(y0, 2.0); - /// assert_eq!(xy, [x0, 0.0, y0, y0, 3.0]); - /// ``` - pub fn hold() -> Self { - let mut s = Self::default(); - s.ba[3] = T::one(); - s - } - - // TODO - // lowpass1 - // highpass1 - // butterworth - // elliptic - // chebychev1/2 - // bessel - // invert // high-to-low/low-to-high - // notch - // PI-notch - // SOS cascades thereoff - - /// Return a builder for a "PID" controller - /// - /// ``` - /// # use idsp::iir::*; - /// let i = Biquad::::pid().build().unwrap(); - /// assert_eq!(i, Biquad::default()); - /// ``` - pub fn pid() -> PidBuilder { - PidBuilder::default() - } - /// Compute the overall (DC/proportional feed-forward) gain. /// /// ``` /// # use idsp::iir::*; - /// assert_eq!(Biquad::proportional(3.0).forward_gain(), 3.0); + /// assert_eq!(Biquad::from(proportional(3.0)).forward_gain(), 3.0); /// ``` /// /// # Returns @@ -222,13 +211,14 @@ impl Biquad { /// Compute input-referred (`x`) offset. /// ``` /// # use idsp::iir::*; - /// let mut i = Biquad::proportional(3.0); + /// let mut i = Biquad::from(proportional(3.0)); /// i.set_input_offset(2.0); /// assert_eq!(i.input_offset(), 2.0); /// ``` pub fn input_offset(&self) -> T { self.u / self.forward_gain() } + /// Convert input (`x`) offset to equivalent summing junction offset (`u`) and apply. /// /// In the case of a "PID" controller the response behavior of the controller @@ -239,9 +229,8 @@ impl Biquad { /// /// ``` /// # use idsp::iir::*; - /// let mut i = Biquad::proportional(3.0); + /// let mut i = Biquad::from(proportional(3.0)); /// i.set_input_offset(2.0); - /// assert_eq!(i.input_offset(), 2.0); /// let x0 = 0.5; /// let y0 = i.update(&mut [0.0; 5], x0); /// assert_eq!(y0, (x0 + i.input_offset()) * i.forward_gain()); @@ -260,7 +249,7 @@ impl Biquad { /// # use idsp::iir::*; /// let mut xy = core::array::from_fn(|i| i as _); /// let x0 = 3.0; - /// let y0 = Biquad::identity().update(&mut xy, x0); + /// let y0 = Biquad::from(identity()).update(&mut xy, x0); /// assert_eq!(y0, x0); /// assert_eq!(xy, [x0, 0.0, y0, 2.0, 3.0]); /// ``` @@ -298,7 +287,7 @@ impl Biquad { /// /// ``` /// # use idsp::iir::*; -/// let b = Biquad::pid() +/// let b: Biquad = PidBuilder::default() /// .period(1e-3) /// .gain(PidAction::Ki, 1e-3) /// .gain(PidAction::Kp, 1.0) @@ -306,7 +295,8 @@ impl Biquad { /// .limit(PidAction::Ki, 1e3) /// .limit(PidAction::Kd, 1e1) /// .build() -/// .unwrap(); +/// .unwrap() +/// .into(); /// ``` #[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize)] pub struct PidBuilder { @@ -377,14 +367,15 @@ impl PidBuilder { /// # use idsp::iir::*; /// let tau = 1e-3; /// let ki = 1e-4; - /// let i = Biquad::pid() + /// let i: Biquad = PidBuilder::default() /// .period(tau) /// .gain(PidAction::Ki, ki) /// .build() - /// .unwrap(); + /// .unwrap() + /// .into(); /// let x0 = 5.0; /// let y0 = i.update(&mut [0.0; 5], x0); - /// assert_eq!(y0, x0 * ki / tau); + /// assert!((y0 / (x0 * ki / tau) - 1.0).abs() < 2.0 * f32::EPSILON); /// ``` /// /// # Arguments @@ -403,11 +394,12 @@ impl PidBuilder { /// ``` /// # use idsp::iir::*; /// let ki_limit = 1e3; - /// let i = Biquad::pid() + /// let i: Biquad = PidBuilder::default() /// .gain(PidAction::Ki, 8.0) /// .limit(PidAction::Ki, ki_limit) /// .build() - /// .unwrap(); + /// .unwrap() + /// .into(); /// let mut xy = [0.0; 5]; /// let x0 = 5.0; /// for _ in 0..1000 { @@ -436,13 +428,14 @@ impl PidBuilder { /// /// ``` /// # use idsp::iir::*; - /// let i = Biquad::::pid() + /// let i: Biquad = PidBuilder::default() /// .gain(PidAction::Kp, 3.0) /// .build() - /// .unwrap(); - /// assert_eq!(i, Biquad::proportional(3.0)); + /// .unwrap() + /// .into(); + /// assert_eq!(i, Biquad::from(proportional(3.0))); /// ``` - pub fn build(self) -> Result, PidError> { + pub fn build(self) -> Result<[T; 5], PidError> { const KP: usize = PidAction::Kp as usize; // Determine highest denominator (feedback, `a`) order @@ -495,10 +488,7 @@ impl PidBuilder { *baj = *baj * a0; } - Ok(Biquad { - ba: [b[0], b[1], b[2], -a[1], -a[2]], - ..Default::default() - }) + Ok([b[0], b[1], b[2], -a[1], -a[2]]) } } @@ -508,15 +498,16 @@ mod test { #[test] fn pid() { - let b = Biquad::pid() - .period(1.0f32) + let b: Biquad = PidBuilder::default() + .period(1.0) .gain(PidAction::Ki, 1e-3) .gain(PidAction::Kp, 1.0) .gain(PidAction::Kd, 1e2) .limit(PidAction::Ki, 1e3) .limit(PidAction::Kd, 1e1) .build() - .unwrap(); + .unwrap() + .into(); let want = [ 9.18190826, -18.27272561, @@ -537,11 +528,12 @@ mod test { fn units() { let ki = 5e-2; let tau = 3e-3; - let b = Biquad::pid() + let b: Biquad = PidBuilder::default() .period(tau) .gain(PidAction::Ki, ki) .build() - .unwrap(); + .unwrap() + .into(); let mut xy = [0.0; 5]; for i in 1..10 { let y_have = b.update(&mut xy, 1.0); From f9262f43f0727b1560e9c36dc5f57c930a2101cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 5 Jan 2024 23:06:55 +0100 Subject: [PATCH 04/26] iir_int: rework --- benches/micro.rs | 4 +- src/iir_int.rs | 351 +++++++++++++++++++++++++++++++++++++++-------- 2 files changed, 295 insertions(+), 60 deletions(-) diff --git a/benches/micro.rs b/benches/micro.rs index 7292332..620c555 100644 --- a/benches/micro.rs +++ b/benches/micro.rs @@ -53,8 +53,8 @@ fn pll_bench() { } fn iir_int_bench() { - let dut = iir_int::IIR::default(); - let mut xy = iir_int::Vec5::default(); + let dut = iir_int::Biquad::default(); + let mut xy = [0; 5]; println!( "int_iir::IIR::update(s, x): {}", bench_env(0x2832, |x| dut.update(&mut xy, *x)) diff --git a/src/iir_int.rs b/src/iir_int.rs index 0dfd476..b4b183b 100644 --- a/src/iir_int.rs +++ b/src/iir_int.rs @@ -1,96 +1,331 @@ -use super::tools::macc_i32; -use core::f64::consts::PI; use serde::{Deserialize, Serialize}; /// Generic vector for integer IIR filter. /// This struct is used to hold the x/y input/output data vector or the b/a coefficient /// vector. -pub type Vec5 = [i32; 5]; +/// +/// Integer biquad IIR +/// +/// See [`crate::iir::Biquad`] for general implementation details. +/// Offset and limiting disabled to suit lowpass applications. +/// Coefficient scaling fixed and optimized such that -2 is representable. +/// Tailored to low-passes, PID, II etc, where the integration rule is [1, -2, 1]. +/// Since the relevant coefficients `a1` and `a2` are negated, we also negate the +/// stored `y1` and `y2` in the state. +/// Note that `xy` contains the negative `y1` and `y2`, such that `-a1` +#[derive(Copy, Clone, Debug, Serialize, Deserialize, PartialEq, Eq, PartialOrd)] +pub struct Biquad { + ba: [i32; 5], + u: i32, + min: i32, + max: i32, +} -trait Coeff { - /// Lowpass biquad filter using cutoff and sampling frequencies. Taken from: - /// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html - /// - /// # Args - /// * `f` - Corner frequency, or 3dB cutoff frequency (in units of sample rate). - /// This is only accurate for low corner frequencies less than ~0.01. - /// * `q` - Quality factor (1/sqrt(2) for critical). - /// * `k` - DC gain. - /// - /// # Returns - /// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 is set to -1. - fn lowpass(f: f64, q: f64, k: f64) -> Self; +impl Default for Biquad { + fn default() -> Self { + Self { + ba: [0; 5], + u: 0, + // Due to the negation of the stored `y1`, `y2` + // values, we need to avoid `i32::MIN`. + min: -i32::MAX, + max: i32::MAX, + } + } } -impl Coeff for Vec5 { - fn lowpass(f: f64, q: f64, k: f64) -> Self { - // 3rd order Taylor approximation of sin and cos. - let f = f * 2. * PI; - let f2 = f * f * 0.5; - let fcos = 1. - f2; - let fsin = f * (1. - f2 / 3.); - let alpha = fsin / (2. * q); - // IIR uses Q2.30 fixed point - let a0 = (1. + alpha) / (1 << IIR::SHIFT) as f64; - let b0 = (k / 2. * (1. - fcos) / a0 + 0.5) as _; - let a1 = (2. * fcos / a0 + 0.5) as _; - let a2 = ((alpha - 1.) / a0 + 0.5) as _; - - [b0, 2 * b0, b0, a1, a2] +impl From<[i32; 5]> for Biquad { + fn from(value: [i32; 5]) -> Self { + Self { + ba: value, + ..Default::default() + } } } -/// Integer biquad IIR +/// A filter with the given proportional gain at all frequencies /// -/// See `dsp::iir::IIR` for general implementation details. -/// Offset and limiting disabled to suit lowpass applications. -/// Coefficient scaling fixed and optimized. -#[derive(Copy, Clone, Default, Debug, Serialize, Deserialize)] -pub struct IIR { - pub ba: Vec5, - pub y_offset: i32, - pub y_min: i32, - pub y_max: i32, +/// ``` +/// # use idsp::iir_int::*; +/// let x0 = 3; +/// let k = 5; +/// let y0 = Biquad::from(proportional(k << 20)).update(&mut [0; 5], x0 << 20); +/// assert_eq!(y0, (x0 * k) << (20 + 20 - Biquad::SHIFT)); +/// ``` +pub fn proportional(k: i32) -> [i32; 5] { + [k, 0, 0, 0, 0] +} + +/// A unit gain filter +/// +/// ``` +/// # use idsp::iir_int::*; +/// let x0 = 3; +/// let y0 = Biquad::from(identity()).update(&mut [0; 5], x0); +/// assert_eq!(y0, x0); +/// ``` +pub fn identity() -> [i32; 5] { + proportional(Biquad::ONE) +} + +/// A "hold" filter that ingests input and maintains output +/// +/// ``` +/// # use idsp::iir_int::*; +/// let mut xy = [0, 1, 2, 3, 4]; +/// let x0 = 7; +/// let y0 = Biquad::from(hold()).update(&mut xy, x0); +/// assert_eq!(y0, -2); +/// assert_eq!(xy, [x0, 0, -y0, -y0, 3]); +/// ``` +pub fn hold() -> [i32; 5] { + [0, 0, 0, -Biquad::ONE, 0] +} + +/// Lowpass biquad filter using cutoff and sampling frequencies. Taken from: +/// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html +/// +/// # Args +/// * `f` - Corner frequency, or 3dB cutoff frequency (in units of sample rate). +/// This is only accurate for low corner frequencies less than ~0.01. +/// * `q` - Quality factor (1/sqrt(2) for critical). +/// * `k` - DC gain. +/// +/// # Returns +/// Biquad IIR filter. +pub fn lowpass(f: f64, q: f64, k: f64) -> [i32; 5] { + // 3rd order Taylor approximation of sin and cos. + let f = f * core::f64::consts::TAU; + let f2 = f * f * 0.5; + let fcos = 1. - f2; + let fsin = f * (1. - f2 / 3.); + let alpha = fsin / (2. * q); + let a0 = Biquad::ONE as f64 / (1. + alpha); + let b = (k / 2. * (1. - fcos) * a0 + 0.5) as i32; + let a1 = (2. * fcos * a0 + 0.5) as i32; + let a2 = ((alpha - 1.) * a0 + 0.5) as i32; + + [b, 2 * b, b, -a1, -a2] } -impl IIR { - /// Coefficient fixed point format: signed Q2.30. - /// Tailored to low-passes, PI, II etc. +impl Biquad { + /// Filter coefficients + /// + /// IIR filter tap gains (`ba`) are an array `[b0, b1, b2, -a1, -a2]` such that + /// [`Biquad::update(&mut xy, x0)`] with `xy = [x1, x2, -y1, -y2, -y3]` returns + /// `y0 = clamp(b0*x0 + b1*x1 + b2*x2 + a1*y1 + a2*y2 + u, min, max)`. + /// + /// ``` + /// # use idsp::iir_int::*; + /// assert_eq!(Biquad::from(identity()).ba()[0], Biquad::ONE); + /// assert_eq!(Biquad::from(hold()).ba()[3], -Biquad::ONE); + /// ``` + pub fn ba(&self) -> &[i32; 5] { + &self.ba + } + + /// Mutable reference to the filter coefficients. + /// + /// See [`Biquad::ba()`]. + /// + /// ``` + /// # use idsp::iir_int::*; + /// let mut i = Biquad::default(); + /// i.ba_mut()[0] = Biquad::ONE; + /// assert_eq!(i, Biquad::from(identity())); + /// ``` + pub fn ba_mut(&mut self) -> &mut [i32; 5] { + &mut self.ba + } + + /// Summing junction offset + /// + /// This offset is applied to the output `y0` summing junction + /// on top of the feed-forward (`b`) and feed-back (`a`) terms. + /// The feedback samples are taken at the summing junction and + /// thus also include (and feed back) this offset. + pub fn u(&self) -> i32 { + self.u + } + + /// Set the summing junction offset + /// + /// See [`Biquad::u()`]. + /// + /// ``` + /// # use idsp::iir_int::*; + /// let mut i = Biquad::default(); + /// i.set_u(5); + /// assert_eq!(i.update(&mut [0; 5], 0), 5); + /// ``` + pub fn set_u(&mut self, u: i32) { + self.u = u; + } + + /// Lower output limit + /// + /// Guaranteed minimum output value. + /// The value is inclusive. + /// The clamping also cleanly affects the feedback terms. + /// + /// Note: `i32::MIN` should not be passed as the `y` samples stored in + /// the filter state are negated. Instead use `-i32::MAX` as the lowest + /// possible limit. + /// + /// ``` + /// # use idsp::iir_int::*; + /// assert_eq!(Biquad::default().min(), -i32::MAX); + /// ``` + pub fn min(&self) -> i32 { + self.min + } + + /// Set the lower output limit + /// + /// See [`Biquad::min()`]. + /// + /// ``` + /// # use idsp::iir_int::*; + /// let mut i = Biquad::default(); + /// i.set_min(5); + /// assert_eq!(i.update(&mut [0; 5], 0), 5); + /// ``` + pub fn set_min(&mut self, min: i32) { + self.min = min; + } + + /// Upper output limit + /// + /// Guaranteed maximum output value. + /// The value is inclusive. + /// The clamping also cleanly affects the feedback terms. + /// + /// ``` + /// # use idsp::iir_int::*; + /// assert_eq!(Biquad::default().max(), i32::MAX); + /// ``` + pub fn max(&self) -> i32 { + self.max + } + + /// Set the upper output limit + /// + /// See [`Biquad::max()`]. + /// + /// ``` + /// # use idsp::iir_int::*; + /// let mut i = Biquad::default(); + /// i.set_max(-5); + /// assert_eq!(i.update(&mut [0; 5], 0), -5); + /// ``` + pub fn set_max(&mut self, max: i32) { + self.max = max; + } + + /// Compute the overall (DC/proportional feed-forward) gain. + /// + /// ``` + /// # use idsp::iir_int::*; + /// assert_eq!(Biquad::from(proportional(3)).forward_gain(), 3); + /// ``` + /// + /// # Returns + /// The sum of the `b` feed-forward coefficients. + pub fn forward_gain(&self) -> i32 { + self.ba.iter().take(3).sum() + } + + /// Compute input-referred (`x`) offset. + /// + /// ``` + /// # use idsp::iir_int::*; + /// let mut i = Biquad::from(proportional(3)); + /// i.set_u(3); + /// assert_eq!(i.input_offset(), Biquad::ONE); + /// ``` + pub fn input_offset(&self) -> i32 { + (((self.u as i64) << Self::SHIFT) / self.forward_gain() as i64) as i32 + } + + /// Convert input (`x`) offset to equivalent summing junction offset (`u`) and apply. + /// + /// In the case of a "PID" controller the response behavior of the controller + /// to the offset is "stabilizing", and not "tracking": its frequency response + /// is exclusively according to the lowest non-zero [`PidAction`] gain. + /// There is no high order ("faster") response as would be the case for a "tracking" + /// controller. + /// + /// ``` + /// # use idsp::iir_int::*; + /// let mut i = Biquad::from(proportional(5 << 20)); + /// i.set_input_offset(3 << 20); + /// assert_eq!(i.u(), 15 << (20 + 20 - Biquad::SHIFT)); + /// ``` + /// + /// # Arguments + /// * `offset`: Input (`x`) offset. + pub fn set_input_offset(&mut self, offset: i32) { + self.u = (((1 << (Self::SHIFT - 1)) + offset as i64 * self.forward_gain() as i64) + >> Self::SHIFT) as i32; + } + + /// Coefficient fixed point format: signed Q2.30 pub const SHIFT: u32 = 30; + pub const ONE: i32 = 1 << Self::SHIFT; /// Feed a new input value into the filter, update the filter state, and /// return the new output. Only the state `xy` is modified. /// + /// ``` + /// # use idsp::iir_int::*; + /// let i = Biquad::from(identity()); + /// let mut xy = [0, 1, 2, 3, 4]; + /// let x0 = 5; + /// let y0 = i.update(&mut xy, x0); + /// assert_eq!(y0, x0); + /// assert_eq!(xy, [x0, 0, -y0, 2, 3]); + /// ``` + /// /// # Arguments /// * `xy` - Current filter state. + /// On entry: `[x1, x2, -y1, -y2, -y3]` + /// On exit: `[x0, x1, -y0, -y1, -y2]` /// * `x0` - New input. - pub fn update(&self, xy: &mut Vec5, x0: i32) -> i32 { - let n = self.ba.len(); - debug_assert!(xy.len() == n); - // `xy` contains x0 x1 y0 y1 y2 - // Increment time x1 x2 y1 y2 y3 - // Shift x1 x1 x2 y1 y2 + /// + /// # Returns + /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 + a1*y1 + a2*y2 + u, min, max)` + pub fn update(&self, xy: &mut [i32; 5], x0: i32) -> i32 { + // `xy` contains x0 x1 -y0 -y1 -y2 + // Increment time x1 x2 -y1 -y2 -y3 + // Shift x1 x1 x2 -y1 -y2 // This unrolls better than xy.rotate_right(1) - xy.copy_within(0..n - 1, 1); - // Store x0 x0 x1 x2 y1 y2 + xy.copy_within(0..4, 1); + // Store x0 x0 x1 x2 -y1 -y2 xy[0] = x0; // Compute y0 by multiply-accumulate - let y0 = macc_i32(self.y_offset, xy, &self.ba, Self::SHIFT); + let y0 = (xy + .iter() + .zip(self.ba.iter()) + .fold(1 << (Self::SHIFT - 1), |y0, (xy, ba)| { + y0 + *xy as i64 * *ba as i64 + }) + >> Self::SHIFT) as i32 + + self.u; // Limit y0 - let y0 = y0.max(self.y_min).min(self.y_max); - // Store y0 x0 x1 y0 y1 y2 - xy[n / 2] = y0; + let y0 = y0.clamp(self.min, self.max); + // Store y0 x0 x1 -y0 -y1 -y2 + xy[2] = -y0; y0 } } #[cfg(test)] mod test { - use super::{Coeff, Vec5}; + use super::*; #[test] fn lowpass_gen() { - let ba = Vec5::lowpass(1e-5, 1. / 2f64.sqrt(), 2.); + let ba = Biquad::from(lowpass(5e-6, 0.5f64.sqrt(), 2.)); println!("{:?}", ba); } } From e2781d07f9b656b56f8bb724a392800c19c2cfe0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Mon, 8 Jan 2024 22:29:16 +0100 Subject: [PATCH 05/26] iir: support fixedpoint --- src/iir.rs | 835 +++++++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 683 insertions(+), 152 deletions(-) diff --git a/src/iir.rs b/src/iir.rs index d02c9a9..847b9f7 100644 --- a/src/iir.rs +++ b/src/iir.rs @@ -1,7 +1,122 @@ +use core::{ + iter::Sum, + ops::{Add, Neg, Sub}, +}; +use num_traits::{AsPrimitive, Float, FloatConst}; use serde::{Deserialize, Serialize}; -use num_traits::{clamp, Float}; +pub trait FilterNum: + Copy + + Sum + + PartialEq + + Neg + + Sub + + Add +where + Self: 'static, +{ + const ONE: Self; + const NEG_ONE: Self; + const ZERO: Self; + const MIN: Self; + const MAX: Self; + fn macc(self, xa: impl Iterator) -> Self; + fn mul(self, other: Self) -> Self; + fn div(self, other: Self) -> Self; + fn clamp(self, min: Self, max: Self) -> Self; + fn quantize(value: C) -> Self + where + Self: AsPrimitive, + C: Float + AsPrimitive; +} +macro_rules! impl_float { + ($T:ty) => { + impl FilterNum for $T { + const ONE: Self = 1.0; + const NEG_ONE: Self = -Self::ONE; + const ZERO: Self = 0.0; + const MIN: Self = Self::NEG_INFINITY; + const MAX: Self = Self::INFINITY; + fn macc(self, xa: impl Iterator) -> Self { + xa.fold(self, |y, (a, x)| a.mul_add(x, y)) + // xa.fold(self, |y, (a, x)| y + a * x) + } + fn clamp(self, min: Self, max: Self) -> Self { + <$T>::clamp(self, min, max) + } + fn div(self, other: Self) -> Self { + self / other + } + fn mul(self, other: Self) -> Self { + self * other + } + fn quantize>(value: C) -> Self { + value.as_() + } + } + }; +} +impl_float!(f32); +impl_float!(f64); + +macro_rules! impl_int { + ($T:ty, $A:ty, $Q:literal) => { + impl FilterNum for $T { + const ONE: Self = 1 << $Q; + const NEG_ONE: Self = -Self::ONE; + const ZERO: Self = 0; + // Need to avoid `$T::MIN*$T::MIN` overflow. + const MIN: Self = -Self::MAX; + const MAX: Self = Self::MAX; + fn macc(self, xa: impl Iterator) -> Self { + self + (xa.fold(1 << ($Q - 1), |y, (a, x)| y + a as $A * x as $A) >> $Q) as Self + } + fn clamp(self, min: Self, max: Self) -> Self { + Ord::clamp(self, min, max) + } + fn div(self, other: Self) -> Self { + (((self as $A) << $Q) / other as $A) as Self + } + fn mul(self, other: Self) -> Self { + (((1 << ($Q - 1)) + self as $A * other as $A) >> $Q) as Self + } + fn quantize(value: C) -> Self + where + Self: AsPrimitive, + C: Float + AsPrimitive, + { + (value * Self::ONE.as_()).round().as_() + } + } + }; +} +// Q2.X chosen to be able to exactly and inclusively represent -2 as `-1 << X + 1` +impl_int!(i8, i16, 6); +impl_int!(i16, i32, 14); +impl_int!(i32, i64, 30); +impl_int!(i64, i128, 62); + +/// Filter architecture +/// +/// Direct Form 1 (DF1) and Direct Form 2 transposed (DF2T) are the only IIR filter +/// structures with an (effective bin the case of TDF2) single summing junction +/// this allows clamping of the output before feedback. +/// +/// DF1 allows atomic coefficient change because only x/y are pipelined. +/// The summing junctuion pipelining of TDF2 would require incremental +/// coefficient changes and is thus less amenable to online tuning. +/// +/// DF2T needs less state storage (2 instead of 4). This is in addition to the coefficient storage +/// (5 plus 2 limits plus 1 offset) +/// This implementation already saves storage by decoupling coefficients/limits and offset from state +/// and thus supports both (a) sharing a single filter between multiple states ("channels") and (b) +/// rapid switching of filters (tuning, transfer) for a given state without copying. +/// +/// DF2T is less efficient and accurate for fixed-point architectures as quantization +/// happens at each intermediate summing junction in addition to the output quantization. This is +/// especially true for common `i64 + i32 * i32 -> i64` MACC architectures. +/// /// # Coefficients and state /// /// `[T; 5]` is both the IIR state and coefficients type. @@ -47,7 +162,15 @@ use num_traits::{clamp, Float}; /// implementation of transfer functions beyond bequadratic terms. /// /// See also . -#[derive(Copy, Clone, Debug, Deserialize, Serialize, PartialEq, Eq, PartialOrd)] +/// +/// +/// Offset and limiting disabled to suit lowpass applications. +/// Coefficient scaling fixed and optimized such that -2 is representable. +/// Tailored to low-passes, PID, II etc, where the integration rule is [1, -2, 1]. +/// Since the relevant coefficients `a1` and `a2` are negated, we also negate the +/// stored `y1` and `y2` in the state. +/// Note that `xy` contains the negative `y1` and `y2`, such that `-a1` +#[derive(Clone, Debug, Deserialize, Serialize, PartialEq, PartialOrd)] pub struct Biquad { ba: [T; 5], u: T, @@ -55,18 +178,18 @@ pub struct Biquad { max: T, } -impl Default for Biquad { +impl Default for Biquad { fn default() -> Self { Self { - ba: [T::zero(); 5], - u: T::zero(), - min: T::neg_infinity(), - max: T::infinity(), + ba: [T::ZERO; 5], + u: T::ZERO, + min: T::MIN, + max: T::MAX, } } } -impl From<[T; 5]> for Biquad { +impl From<[T; 5]> for Biquad { fn from(ba: [T; 5]) -> Self { Self { ba, @@ -75,66 +198,81 @@ impl From<[T; 5]> for Biquad { } } -/// A unit gain filter -/// -/// ``` -/// # use idsp::iir::*; -/// let x0 = 3.0; -/// let y0 = Biquad::from(identity()).update(&mut [0.0; 5], x0); -/// assert_eq!(y0, x0); -/// ``` -pub fn identity() -> [T; 5] { - proportional(T::one()) +impl From<[C; 6]> for Biquad +where + T: FilterNum + AsPrimitive, + C: Float + AsPrimitive, +{ + fn from(ba: [C; 6]) -> Self { + let ia0 = C::one() / ba[3]; + Self::from([ + T::quantize(ba[0] * ia0), + T::quantize(ba[1] * ia0), + T::quantize(ba[2] * ia0), + // b[3]: a0*ia0 + T::quantize(ba[4] * ia0), + T::quantize(ba[5] * ia0), + ]) + } } -/// A filter with the given proportional gain at all frequencies -/// -/// ``` -/// # use idsp::iir::*; -/// let x0 = 2.0; -/// let k = 5.0; -/// let y0 = Biquad::from(proportional(k)).update(&mut [0.0; 5], x0); -/// assert_eq!(y0, x0 * k); -/// ``` -pub fn proportional(k: T) -> [T; 5] { - let mut ba = [T::zero(); 5]; - ba[0] = k; - ba -} +impl Biquad { + /// A "hold" filter that ingests input and maintains output + /// + /// ``` + /// # use idsp::iir::*; + /// let mut xy = core::array::from_fn(|i| i as _); + /// let x0 = 7.0; + /// let y0 = Biquad::HOLD.update(&mut xy, x0); + /// assert_eq!(y0, -2.0); + /// assert_eq!(xy, [x0, 0.0, -y0, -y0, 3.0]); + /// ``` + pub const HOLD: Self = Self { + ba: [T::ZERO, T::ZERO, T::ZERO, T::NEG_ONE, T::ZERO], + u: T::ZERO, + min: T::MIN, + max: T::MAX, + }; -/// A "hold" filter that ingests input and maintains output -/// -/// ``` -/// # use idsp::iir::*; -/// let mut xy = core::array::from_fn(|i| i as _); -/// let x0 = 7.0; -/// let y0 = Biquad::from(hold()).update(&mut xy, x0); -/// assert_eq!(y0, 2.0); -/// assert_eq!(xy, [x0, 0.0, y0, y0, 3.0]); -/// ``` -pub fn hold() -> [T; 5] { - let mut ba = [T::zero(); 5]; - ba[3] = T::one(); - ba -} + /// A unity gain filter + /// + /// ``` + /// # use idsp::iir::*; + /// let x0 = 3.0; + /// let y0 = Biquad::IDENTITY.update(&mut [0.0; 5], x0); + /// assert_eq!(y0, x0); + /// ``` + pub const IDENTITY: Self = Self::proportional(T::ONE); + + /// A filter with the given proportional gain at all frequencies + /// + /// ``` + /// # use idsp::iir::*; + /// let x0 = 2.0; + /// let k = 5.0; + /// let y0 = Biquad::proportional(k).update(&mut [0.0; 5], x0); + /// assert_eq!(y0, x0 * k); + /// ``` + pub const fn proportional(k: T) -> Self { + Self { + ba: [k, T::ZERO, T::ZERO, T::ZERO, T::ZERO], + u: T::ZERO, + min: T::MIN, + max: T::MAX, + } + } -// TODO -// lowpass1 -// highpass1 -// butterworth -// elliptic -// chebychev1/2 -// bessel -// invert // high-to-low/low-to-high -// notch -// PI-notch -// SOS cascades thereoff - -impl Biquad { /// Filter coefficients /// /// IIR filter tap gains (`ba`) are an array `[b0, b1, b2, a1, a2]` such that - /// `y0 = clamp(b0*x0 + b1*x1 + b2*x2 + a1*y1 + a2*y2 + u, min, max)`. + /// [`Biquad::update(&mut xy, x0)`] returns + /// `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)`. + /// + /// ``` + /// # use idsp::iir::*; + /// assert_eq!(Biquad::::IDENTITY.ba()[0], i32::ONE); + /// assert_eq!(Biquad::::HOLD.ba()[3], -i32::ONE); + /// ``` pub fn ba(&self) -> &[T; 5] { &self.ba } @@ -142,6 +280,13 @@ impl Biquad { /// Mutable reference to the filter coefficients. /// /// See [`Biquad::ba()`]. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::default(); + /// i.ba_mut()[0] = i32::ONE; + /// assert_eq!(i, Biquad::IDENTITY); + /// ``` pub fn ba_mut(&mut self) -> &mut [T; 5] { &mut self.ba } @@ -159,6 +304,13 @@ impl Biquad { /// Set the summing junction offset /// /// See [`Biquad::u()`]. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::default(); + /// i.set_u(5); + /// assert_eq!(i.update(&mut [0; 5], 0), 5); + /// ``` pub fn set_u(&mut self, u: T) { self.u = u; } @@ -168,6 +320,16 @@ impl Biquad { /// Guaranteed minimum output value. /// The value is inclusive. /// The clamping also cleanly affects the feedback terms. + /// + /// Note: For fixed point filters `Biquad`, `T::MIN` should not be passed + /// to `min()` since the `y` samples stored in + /// the filter state are negated. Instead use `-T::MAX` as the lowest + /// possible limit. + /// + /// ``` + /// # use idsp::iir::*; + /// assert_eq!(Biquad::::default().min(), -i32::MAX); + /// ``` pub fn min(&self) -> T { self.min } @@ -175,6 +337,13 @@ impl Biquad { /// Set the lower output limit /// /// See [`Biquad::min()`]. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::default(); + /// i.set_min(7); + /// assert_eq!(i.update(&mut [0; 5], 0), 7); + /// ``` pub fn set_min(&mut self, min: T) { self.min = min; } @@ -184,6 +353,11 @@ impl Biquad { /// Guaranteed maximum output value. /// The value is inclusive. /// The clamping also cleanly affects the feedback terms. + /// + /// ``` + /// # use idsp::iir::*; + /// assert_eq!(Biquad::::default().max(), i32::MAX); + /// ``` pub fn max(&self) -> T { self.max } @@ -191,6 +365,13 @@ impl Biquad { /// Set the upper output limit /// /// See [`Biquad::max()`]. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::default(); + /// i.set_max(-7); + /// assert_eq!(i.update(&mut [0; 5], 0), -7); + /// ``` pub fn set_max(&mut self, max: T) { self.max = max; } @@ -199,49 +380,58 @@ impl Biquad { /// /// ``` /// # use idsp::iir::*; - /// assert_eq!(Biquad::from(proportional(3.0)).forward_gain(), 3.0); + /// assert_eq!(Biquad::proportional(3.0).forward_gain(), 3.0); /// ``` /// /// # Returns /// The sum of the `b` feed-forward coefficients. pub fn forward_gain(&self) -> T { - self.ba[0] + self.ba[1] + self.ba[2] + self.ba.iter().take(3).copied().sum() } /// Compute input-referred (`x`) offset. /// ``` /// # use idsp::iir::*; - /// let mut i = Biquad::from(proportional(3.0)); - /// i.set_input_offset(2.0); - /// assert_eq!(i.input_offset(), 2.0); + /// let mut i = Biquad::proportional(3); + /// i.set_u(3); + /// assert_eq!(i.input_offset(), i32::ONE); /// ``` pub fn input_offset(&self) -> T { - self.u / self.forward_gain() + self.u.div(self.forward_gain()) } /// Convert input (`x`) offset to equivalent summing junction offset (`u`) and apply. /// /// In the case of a "PID" controller the response behavior of the controller /// to the offset is "stabilizing", and not "tracking": its frequency response - /// is exclusively according to the lowest non-zero [`PidAction`] gain. + /// is exclusively according to the lowest non-zero [`Action`] gain. /// There is no high order ("faster") response as would be the case for a "tracking" /// controller. /// /// ``` /// # use idsp::iir::*; - /// let mut i = Biquad::from(proportional(3.0)); + /// let mut i = Biquad::proportional(3.0); /// i.set_input_offset(2.0); /// let x0 = 0.5; /// let y0 = i.update(&mut [0.0; 5], x0); /// assert_eq!(y0, (x0 + i.input_offset()) * i.forward_gain()); /// ``` /// + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::proportional(-i32::ONE); + /// i.set_input_offset(1); + /// assert_eq!(i.u(), -1); + /// ``` + /// /// # Arguments /// * `offset`: Input (`x`) offset. pub fn set_input_offset(&mut self, offset: T) { - self.u = offset * self.forward_gain(); + self.u = offset.mul(self.forward_gain()); } + /// Direct Form 1 Update + /// /// Ingest a new input value into the filter, update the filter state, and /// return the new output. Only the state `xy` is modified. /// @@ -249,36 +439,358 @@ impl Biquad { /// # use idsp::iir::*; /// let mut xy = core::array::from_fn(|i| i as _); /// let x0 = 3.0; - /// let y0 = Biquad::from(identity()).update(&mut xy, x0); + /// let y0 = Biquad::IDENTITY.update(&mut xy, x0); /// assert_eq!(y0, x0); - /// assert_eq!(xy, [x0, 0.0, y0, 2.0, 3.0]); + /// assert_eq!(xy, [x0, 0.0, -y0, 2.0, 3.0]); /// ``` /// /// # Arguments /// * `xy` - Current filter state. + /// On entry: `[x1, x2, -y1, -y2, -y3]` + /// On exit: `[x0, x1, -y0, -y1, -y2]` /// * `x0` - New input. /// /// # Returns - /// The new output `y0`. + /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` /// /// # Panics /// Panics in debug mode if `!(self.min <= self.max)`. pub fn update(&self, xy: &mut [T; 5], x0: T) -> T { - // `xy` contains x0 x1 y0 y1 y2 - // Increment time x1 x2 y1 y2 y3 - // Shift x1 x1 x2 y1 y2 + // `xy` contains x0 x1 -y0 -y1 -y2 + // Increment time x1 x2 -y1 -y2 -y3 + // Shift x1 x1 x2 -y1 -y2 xy.copy_within(0..4, 1); - // Store x0 x0 x1 x2 y1 y2 + // Store x0 x0 x1 x2 -y1 -y2 xy[0] = x0; - let y0 = xy - .iter() - .zip(self.ba.iter()) - .fold(self.u, |y, (x, a)| y + *x * *a); - let y0 = clamp(y0, self.min, self.max); - // Store y0 x0 x1 y0 y1 y2 - xy[2] = y0; + // Compute y0 + let y0 = self + .u + .macc(xy.iter().copied().zip(self.ba.iter().copied())) + .clamp(self.min, self.max); + // Store -y0 x0 x1 -y0 -y1 -y2 + xy[2] = -y0; + y0 + } + + /// Direct Form 1 Update + /// + /// Ingest a new input value into the filter, update the filter state, and + /// return the new output. Only the state `xy` is modified. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut xy = core::array::from_fn(|i| i as _); + /// let x0 = 3.0; + /// let y0 = Biquad::IDENTITY.update_df1(&mut xy, x0); + /// assert_eq!(y0, x0); + /// assert_eq!(xy, [x0, 0.0, -y0, 2.0]); + /// ``` + /// + /// # Arguments + /// * `xy` - Current filter state. + /// On entry: `[x1, x2, -y1, -y2]` + /// On exit: `[x0, x1, -y0, -y1]` + /// * `x0` - New input. + /// + /// # Returns + /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` + /// + /// # Panics + /// Panics in debug mode if `!(self.min <= self.max)`. + pub fn update_df1(&self, xy: &mut [T; 4], x0: T) -> T { + // `xy` contains x0 x1 -y0 -y1 + // Increment time x1 x2 -y1 -y2 + // Compute y0 + let y0 = self + .u + .macc( + core::iter::once(x0) + .chain(xy.iter().copied()) + .zip(self.ba.iter().copied()), + ) + .clamp(self.min, self.max); + // Shift x1 x1 -y1 -y2 + xy[1] = xy[0]; + // Store x0 x0 x1 -y1 -y2 + xy[0] = x0; + // Shift x0 x1 -y0 -y1 + xy[3] = xy[2]; + // Store -y0 x0 x1 -y0 -y1 + xy[2] = -y0; y0 } + + /// Ingest new input and perform a Direct Form 2 Transposed update. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut xy = core::array::from_fn(|i| i as _); + /// let x0 = 3.0; + /// let y0 = Biquad::IDENTITY.update_df2t(&mut xy, x0); + /// assert_eq!(y0, x0); + /// assert_eq!(xy, [1.0, 0.0]); + /// ``` + /// + /// # Arguments + /// * `s` - Current filter state. + /// On entry: `[b1*x1 + b2*x2 - a1*y1 - a2*y2, b2*x1 - a2*y1]` + /// On exit: `[b1*x0 + b2*x1 - a1*y0 - a2*y1, b2*x0 - a2*y0]` + /// * `x0` - New input. + /// + /// # Returns + /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` + /// + /// # Panics + /// Panics in debug mode if `!(self.min <= self.max)`. + pub fn update_df2t(&self, u: &mut [T; 2], x0: T) -> T { + let y0 = (u[0] + self.ba[0].mul(x0)).clamp(self.min, self.max); + u[0] = u[1] + self.ba[1].mul(x0) - self.ba[3].mul(y0); + u[1] = self.u + self.ba[2].mul(x0) - self.ba[4].mul(y0); + y0 + } +} + +#[derive(Copy, Clone, Debug, PartialEq, PartialOrd, Serialize, Deserialize)] +enum Shape { + InverseQ(T), + Bandwidth(T), + Slope(T), +} + +impl Default for Shape { + fn default() -> Self { + Self::InverseQ(T::SQRT_2()) + } +} + +/// Standard audio biquad filter builder +/// +/// +#[derive(Clone, Debug, PartialEq, PartialOrd, Serialize, Deserialize)] +pub struct Filter { + /// Angular critical frequency (in units of sampling frequency) + /// Corner frequency, or 3dB cutoff frequency, + w0: T, + /// Passband gain + gain: T, + /// Shelf gain (only for peaking, lowshelf, highshelf) + /// Relative to passband gain + shelf: T, + /// Inverse Q + shape: Shape, +} + +impl Default for Filter { + fn default() -> Self { + Self { + w0: T::zero(), + gain: T::one(), + shape: Shape::default(), + shelf: T::one(), + } + } +} + +impl Filter +where + T: 'static + Float + FloatConst, + f32: AsPrimitive, +{ + pub fn frequency(self, critical_frequency: T, sample_frequency: T) -> Self { + self.critical_frequency(critical_frequency / sample_frequency) + } + + pub fn critical_frequency(self, critical_frequency: T) -> Self { + self.angular_critical_frequency(T::TAU() * critical_frequency) + } + + pub fn angular_critical_frequency(mut self, w0: T) -> Self { + self.w0 = w0; + self + } + + pub fn gain(mut self, k: T) -> Self { + self.gain = k; + self + } + + pub fn gain_db(self, k_db: T) -> Self { + self.gain(10.0.as_().powf(k_db / 20.0.as_())) + } + + pub fn shelf(mut self, a: T) -> Self { + self.shelf = a; + self + } + + pub fn shelf_db(self, k_db: T) -> Self { + self.gain(10.0.as_().powf(k_db / 20.0.as_())) + } + + pub fn inverse_q(mut self, qi: T) -> Self { + self.shape = Shape::InverseQ(qi); + self + } + + pub fn q(self, q: T) -> Self { + self.inverse_q(T::one() / q) + } + + /// Set [`FilterBuilder::frequency()`] first. + /// In octaves. + pub fn bandwidth(mut self, bw: T) -> Self { + self.shape = Shape::Bandwidth(bw); + self + } + + /// Set [`FilterBuilder::gain()`] first. + pub fn shelf_slope(mut self, s: T) -> Self { + self.shape = Shape::Slope(s); + self + } + + fn qi(&self) -> T { + match self.shape { + Shape::InverseQ(qi) => qi, + Shape::Bandwidth(bw) => { + 2.0.as_() * (T::LN_2() / 2.0.as_() * bw * self.w0 / self.w0.sin()).sinh() + } + Shape::Slope(s) => { + ((self.gain + T::one() / self.gain) * (T::one() / s - T::one()) + 2.0.as_()).sqrt() + } + } + } + + fn alpha(&self) -> T { + 0.5.as_() * self.w0.sin() * self.qi() + } + + /// Lowpass biquad filter. + /// + /// ``` + /// use idsp::iir::*; + /// let ba = Filter::default().critical_frequency(0.1).lowpass(); + /// println!("{ba:?}"); + /// ``` + pub fn lowpass(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + let b = self.gain * 0.5.as_() * (T::one() - fcos); + [ + b, + b + b, + b, + T::one() + alpha, + -(fcos + fcos), + T::one() - alpha, + ] + } + + pub fn highpass(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + let b = self.gain * 0.5.as_() * (T::one() + fcos); + [ + b, + -(b + b), + b, + T::one() + alpha, + -(fcos + fcos), + T::one() - alpha, + ] + } + + pub fn bandpass(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + let b = self.gain * alpha; + [ + b, + T::zero(), + b, + T::one() + alpha, + -(fcos + fcos), + T::one() - alpha, + ] + } + + pub fn notch(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + [ + self.gain, + -(fcos + fcos) * self.gain, + self.gain, + T::one() + alpha, + -(fcos + fcos), + T::one() - alpha, + ] + } + + pub fn allpass(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + [ + (T::one() - alpha) * self.gain, + -(fcos + fcos) * self.gain, + (T::one() + alpha) * self.gain, + T::one() + alpha, + -(fcos + fcos), + T::one() - alpha, + ] + } + + pub fn peaking(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + [ + (T::one() + alpha * self.shelf) * self.gain, + -(fcos + fcos) * self.gain, + (T::one() - alpha * self.shelf) * self.gain, + T::one() + alpha / self.shelf, + -(fcos + fcos), + T::one() - alpha / self.shelf, + ] + } + + pub fn lowshelf(self) -> [T; 6] { + let fcos = self.w0.cos(); + let sp1 = self.shelf + T::one(); + let sm1 = self.shelf - T::one(); + let tsa = 2.0.as_() * self.shelf.sqrt() * self.alpha(); + [ + self.shelf * self.gain * (sp1 - sm1 * fcos + tsa), + 2.0.as_() * self.shelf * self.gain * (sm1 - sp1 * fcos), + self.shelf * self.gain * (sp1 - sm1 * fcos - tsa), + sp1 + sm1 * fcos + tsa, + (-2.0).as_() * (sm1 + sp1 * fcos), + sp1 + sm1 * fcos - tsa, + ] + } + + pub fn highshelf(self) -> [T; 6] { + let fcos = self.w0.cos(); + let sp1 = self.shelf + T::one(); + let sm1 = self.shelf - T::one(); + let tsa = 2.0.as_() * self.shelf.sqrt() * self.alpha(); + [ + self.shelf * self.gain * (sp1 + sm1 * fcos + tsa), + (-2.0).as_() * self.shelf * self.gain * (sm1 + sp1 * fcos), + self.shelf * self.gain * (sp1 + sm1 * fcos - tsa), + sp1 - sm1 * fcos + tsa, + 2.0.as_() * (sm1 - sp1 * fcos), + sp1 - sm1 * fcos - tsa, + ] + } + + // TODO + // PI-notch + // + // SOS cascades: + // butterworth + // elliptic + // chebychev1/2 + // bessel } /// PID controller builder @@ -287,25 +799,25 @@ impl Biquad { /// /// ``` /// # use idsp::iir::*; -/// let b: Biquad = PidBuilder::default() +/// let b: Biquad = Pid::default() /// .period(1e-3) -/// .gain(PidAction::Ki, 1e-3) -/// .gain(PidAction::Kp, 1.0) -/// .gain(PidAction::Kd, 1e2) -/// .limit(PidAction::Ki, 1e3) -/// .limit(PidAction::Kd, 1e1) +/// .gain(Action::Ki, 1e-3) +/// .gain(Action::Kp, 1.0) +/// .gain(Action::Kd, 1e2) +/// .limit(Action::Ki, 1e3) +/// .limit(Action::Kd, 1e1) /// .build() /// .unwrap() /// .into(); /// ``` -#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize)] -pub struct PidBuilder { +#[derive(Debug, Clone, PartialEq, PartialOrd, Serialize, Deserialize)] +pub struct Pid { period: T, gains: [T; 5], limits: [T; 5], } -impl Default for PidBuilder { +impl Default for Pid { fn default() -> Self { Self { period: T::one(), @@ -315,7 +827,7 @@ impl Default for PidBuilder { } } -/// [`PidBuilder::build()`] errors +/// [`Pid::build()`] errors #[derive(Copy, Clone, Debug, PartialEq, Eq, Ord, PartialOrd, Serialize, Deserialize)] #[non_exhaustive] pub enum PidError { @@ -327,7 +839,7 @@ pub enum PidError { /// /// This enumerates the five possible PID style actions of a [`Biquad`] #[derive(Copy, Clone, Debug, PartialEq, Eq, Ord, PartialOrd, Serialize, Deserialize)] -pub enum PidAction { +pub enum Action { /// Double integrating, -40 dB per decade Kii = 0, /// Integrating, -20 dB per decade @@ -340,7 +852,7 @@ pub enum PidAction { Kdd = 4, } -impl PidBuilder { +impl> Pid { /// Sample period /// /// # Arguments @@ -361,15 +873,15 @@ impl PidBuilder { /// /// Note that inverse time units correspond to angular frequency units. /// Gains are accurate in the low frequency limit. Towards Nyquist, the - /// frequency response is wrapped. + /// frequency response is warped. /// /// ``` /// # use idsp::iir::*; /// let tau = 1e-3; /// let ki = 1e-4; - /// let i: Biquad = PidBuilder::default() + /// let i: Biquad = Pid::default() /// .period(tau) - /// .gain(PidAction::Ki, ki) + /// .gain(Action::Ki, ki) /// .build() /// .unwrap() /// .into(); @@ -381,22 +893,22 @@ impl PidBuilder { /// # Arguments /// * `action`: Action to control /// * `gain`: Gain value - pub fn gain(mut self, action: PidAction, gain: T) -> Self { + pub fn gain(mut self, action: Action, gain: T) -> Self { self.gains[action as usize] = gain; self } /// Gain limit for a given action /// - /// Gain limit units are `output/input`. See also [`PidBuilder::gain()`]. + /// Gain limit units are `output/input`. See also [`Pid::gain()`]. /// Multiple gains and limits may interact and lead to peaking. /// /// ``` /// # use idsp::iir::*; /// let ki_limit = 1e3; - /// let i: Biquad = PidBuilder::default() - /// .gain(PidAction::Ki, 8.0) - /// .limit(PidAction::Ki, ki_limit) + /// let i: Biquad = Pid::default() + /// .gain(Action::Ki, 8.0) + /// .limit(Action::Ki, ki_limit) /// .build() /// .unwrap() /// .into(); @@ -412,7 +924,7 @@ impl PidBuilder { /// # Arguments /// * `action`: Action to limit in gain /// * `limit`: Gain limit - pub fn limit(mut self, action: PidAction, limit: T) -> Self { + pub fn limit(mut self, action: Action, limit: T) -> Self { self.limits[action as usize] = limit; self } @@ -428,15 +940,14 @@ impl PidBuilder { /// /// ``` /// # use idsp::iir::*; - /// let i: Biquad = PidBuilder::default() - /// .gain(PidAction::Kp, 3.0) - /// .build() - /// .unwrap() - /// .into(); - /// assert_eq!(i, Biquad::from(proportional(3.0))); + /// let i: Biquad = Pid::default().gain(Action::Kp, 3.0).build().unwrap().into(); + /// assert_eq!(i, Biquad::proportional(3.0)); /// ``` - pub fn build(self) -> Result<[T; 5], PidError> { - const KP: usize = PidAction::Kp as usize; + pub fn build>(self) -> Result<[C; 5], PidError> + where + T: AsPrimitive, + { + const KP: usize = Action::Kp as usize; // Determine highest denominator (feedback, `a`) order let low = self @@ -452,43 +963,37 @@ impl PidBuilder { // Derivative/integration kernels let kernels = [ - [T::one(), T::zero(), T::zero()], - [T::one(), -T::one(), T::zero()], - [T::one(), -T::one() - T::one(), T::one()], + [C::ONE, C::ZERO, C::ZERO], + [C::ONE, C::NEG_ONE, C::ZERO], + [C::ONE, C::NEG_ONE + C::NEG_ONE, C::ONE], ]; - // Coefficients - let mut b = [T::zero(); 3]; - let mut a = [T::zero(); 3]; - + // Scale gains, compute limits, quantize let mut zi = self.period.powi(low as i32 - KP as i32); - - for ((i, (gi, li)), ki) in self - .gains - .iter() - .zip(self.limits.iter()) - .enumerate() - .skip(low) - .zip(kernels.iter()) - { - // Scale gains and compute limits in place - let gi = *gi * zi; + let mut gl = [[T::zero(); 2]; 3]; + for (gli, (i, (ggi, lli))) in gl.iter_mut().zip( + self.gains + .iter() + .zip(self.limits.iter()) + .enumerate() + .skip(low), + ) { + gli[0] = *ggi * zi; + gli[1] = if i == KP { T::one() } else { gli[0] / *lli }; zi = zi * self.period; - let li = if i == KP { T::one() } else { gi / *li }; - - for (j, (bj, aj)) in b.iter_mut().zip(a.iter_mut()).enumerate() { - *bj = *bj + gi * ki[j]; - *aj = *aj + li * ki[j]; - } } + let a0i = T::one() / gl.iter().map(|gli| gli[1]).sum(); - // Normalize - let a0 = T::one() / a[0]; - for baj in b.iter_mut().chain(a.iter_mut().skip(1)) { - *baj = *baj * a0; + // Coefficients + let mut ba = [[C::ZERO; 2]; 3]; + for (gli, ki) in gl.iter().zip(kernels.iter()) { + let (g, l) = (C::quantize(gli[0] * a0i), C::quantize(gli[1] * a0i)); + for (j, baj) in ba.iter_mut().enumerate() { + *baj = [baj[0] + ki[j].mul(g), baj[1] + ki[j].mul(l)]; + } } - Ok([b[0], b[1], b[2], -a[1], -a[2]]) + Ok([ba[0][0], ba[1][0], ba[2][0], ba[1][1], ba[2][1]]) } } @@ -498,13 +1003,13 @@ mod test { #[test] fn pid() { - let b: Biquad = PidBuilder::default() + let b: Biquad = Pid::default() .period(1.0) - .gain(PidAction::Ki, 1e-3) - .gain(PidAction::Kp, 1.0) - .gain(PidAction::Kd, 1e2) - .limit(PidAction::Ki, 1e3) - .limit(PidAction::Kd, 1e1) + .gain(Action::Ki, 1e-3) + .gain(Action::Kp, 1.0) + .gain(Action::Kd, 1e2) + .limit(Action::Ki, 1e3) + .limit(Action::Kd, 1e1) .build() .unwrap() .into(); @@ -512,8 +1017,8 @@ mod test { 9.18190826, -18.27272561, 9.09090826, - 1.90909074, - -0.90909083, + -1.90909074, + 0.90909083, ]; for (ba_have, ba_want) in b.ba.iter().zip(want.iter()) { assert!( @@ -524,13 +1029,28 @@ mod test { } } + #[test] + fn pid_i32() { + let b: Biquad = Pid::default() + .period(1.0) + .gain(Action::Ki, 1e-5) + .gain(Action::Kp, 1e-2) + .gain(Action::Kd, 1e0) + .limit(Action::Ki, 1e1) + .limit(Action::Kd, 1e-1) + .build() + .unwrap() + .into(); + println!("{b:?}"); + } + #[test] fn units() { let ki = 5e-2; let tau = 3e-3; - let b: Biquad = PidBuilder::default() + let b: Biquad = Pid::default() .period(tau) - .gain(PidAction::Ki, ki) + .gain(Action::Ki, ki) .build() .unwrap() .into(); @@ -544,4 +1064,15 @@ mod test { ); } } + + #[test] + fn lowpass_gen() { + let ba = Biquad::::from( + Filter::default() + .critical_frequency(2e-9f64) + .gain(2e7) + .lowpass(), + ); + println!("{:?}", ba); + } } From 55e19736dbdcee76edb8afb654dc292ebaedf9f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Mon, 8 Jan 2024 23:04:04 +0100 Subject: [PATCH 06/26] remove iir_int --- CHANGELOG.md | 7 +- README.md | 7 +- benches/micro.rs | 4 +- src/iir_int.rs | 331 ----------------------------------------------- src/lib.rs | 1 - 5 files changed, 12 insertions(+), 338 deletions(-) delete mode 100644 src/iir_int.rs diff --git a/CHANGELOG.md b/CHANGELOG.md index db93ad2..0f0d90f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,12 +10,15 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added * `hbf` FIRs, symmetric FIRs, half band filters, HBF decimators and interpolators -* `iir::PidBuilder` a builder for PID coefficients -* `iir::Biquad::{hold, proportional, identity}` +* `iir::Pid`, `iir:Filter` a builder for PID coefficients and the collection of standard Biquad filters +* `iir::Biquad::{HOLD, proportional, identity}` +* `iir`: support for other integers (i8, i16, i128) +* `iir::Biquad`: support for reduced DF1 state and DF2T state ### Removed * `iir::Vec5` type alias has been removed. +* `iir_int`: integrated into `iir`. ### Changed diff --git a/README.md b/README.md index e6eadac..7663b9e 100644 --- a/README.md +++ b/README.md @@ -36,9 +36,12 @@ High accuracy, zero-assumption, fully robust, forward and reciprocal PLLs with d Tools to handle, track, and unwrap phase signals or generate them. -## iir_int, iir +## iir -`i32` and `f32` biquad IIR filters with robust and clean clipping and offset (anti-windup, no derivative kick, dynamically adjustable gains). +Fixed point (`i8`, `i16`, `i32`, `i64`) and floating point (`f32`, `f64`) biquad IIR filters. +Robust and clean clipping and offset (anti-windup, no derivative kick, dynamically adjustable gains) suitable for PID controller applications. +Direct Form 1 and Direct Form 2 Transposed supported. +Coefficient sharing for multiple channels. ## Lowpass, Lockin diff --git a/benches/micro.rs b/benches/micro.rs index 620c555..2869ba0 100644 --- a/benches/micro.rs +++ b/benches/micro.rs @@ -2,7 +2,7 @@ use core::f32::consts::PI; use easybench::bench_env; -use idsp::{atan2, cossin, iir, iir_int, Filter, Lowpass, PLL, RPLL}; +use idsp::{atan2, cossin, iir, Filter, Lowpass, PLL, RPLL}; fn atan2_bench() { let xi = (10 << 16) as i32; @@ -53,7 +53,7 @@ fn pll_bench() { } fn iir_int_bench() { - let dut = iir_int::Biquad::default(); + let dut = iir::Biquad::default(); let mut xy = [0; 5]; println!( "int_iir::IIR::update(s, x): {}", diff --git a/src/iir_int.rs b/src/iir_int.rs deleted file mode 100644 index b4b183b..0000000 --- a/src/iir_int.rs +++ /dev/null @@ -1,331 +0,0 @@ -use serde::{Deserialize, Serialize}; - -/// Generic vector for integer IIR filter. -/// This struct is used to hold the x/y input/output data vector or the b/a coefficient -/// vector. -/// -/// Integer biquad IIR -/// -/// See [`crate::iir::Biquad`] for general implementation details. -/// Offset and limiting disabled to suit lowpass applications. -/// Coefficient scaling fixed and optimized such that -2 is representable. -/// Tailored to low-passes, PID, II etc, where the integration rule is [1, -2, 1]. -/// Since the relevant coefficients `a1` and `a2` are negated, we also negate the -/// stored `y1` and `y2` in the state. -/// Note that `xy` contains the negative `y1` and `y2`, such that `-a1` -#[derive(Copy, Clone, Debug, Serialize, Deserialize, PartialEq, Eq, PartialOrd)] -pub struct Biquad { - ba: [i32; 5], - u: i32, - min: i32, - max: i32, -} - -impl Default for Biquad { - fn default() -> Self { - Self { - ba: [0; 5], - u: 0, - // Due to the negation of the stored `y1`, `y2` - // values, we need to avoid `i32::MIN`. - min: -i32::MAX, - max: i32::MAX, - } - } -} - -impl From<[i32; 5]> for Biquad { - fn from(value: [i32; 5]) -> Self { - Self { - ba: value, - ..Default::default() - } - } -} - -/// A filter with the given proportional gain at all frequencies -/// -/// ``` -/// # use idsp::iir_int::*; -/// let x0 = 3; -/// let k = 5; -/// let y0 = Biquad::from(proportional(k << 20)).update(&mut [0; 5], x0 << 20); -/// assert_eq!(y0, (x0 * k) << (20 + 20 - Biquad::SHIFT)); -/// ``` -pub fn proportional(k: i32) -> [i32; 5] { - [k, 0, 0, 0, 0] -} - -/// A unit gain filter -/// -/// ``` -/// # use idsp::iir_int::*; -/// let x0 = 3; -/// let y0 = Biquad::from(identity()).update(&mut [0; 5], x0); -/// assert_eq!(y0, x0); -/// ``` -pub fn identity() -> [i32; 5] { - proportional(Biquad::ONE) -} - -/// A "hold" filter that ingests input and maintains output -/// -/// ``` -/// # use idsp::iir_int::*; -/// let mut xy = [0, 1, 2, 3, 4]; -/// let x0 = 7; -/// let y0 = Biquad::from(hold()).update(&mut xy, x0); -/// assert_eq!(y0, -2); -/// assert_eq!(xy, [x0, 0, -y0, -y0, 3]); -/// ``` -pub fn hold() -> [i32; 5] { - [0, 0, 0, -Biquad::ONE, 0] -} - -/// Lowpass biquad filter using cutoff and sampling frequencies. Taken from: -/// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html -/// -/// # Args -/// * `f` - Corner frequency, or 3dB cutoff frequency (in units of sample rate). -/// This is only accurate for low corner frequencies less than ~0.01. -/// * `q` - Quality factor (1/sqrt(2) for critical). -/// * `k` - DC gain. -/// -/// # Returns -/// Biquad IIR filter. -pub fn lowpass(f: f64, q: f64, k: f64) -> [i32; 5] { - // 3rd order Taylor approximation of sin and cos. - let f = f * core::f64::consts::TAU; - let f2 = f * f * 0.5; - let fcos = 1. - f2; - let fsin = f * (1. - f2 / 3.); - let alpha = fsin / (2. * q); - let a0 = Biquad::ONE as f64 / (1. + alpha); - let b = (k / 2. * (1. - fcos) * a0 + 0.5) as i32; - let a1 = (2. * fcos * a0 + 0.5) as i32; - let a2 = ((alpha - 1.) * a0 + 0.5) as i32; - - [b, 2 * b, b, -a1, -a2] -} - -impl Biquad { - /// Filter coefficients - /// - /// IIR filter tap gains (`ba`) are an array `[b0, b1, b2, -a1, -a2]` such that - /// [`Biquad::update(&mut xy, x0)`] with `xy = [x1, x2, -y1, -y2, -y3]` returns - /// `y0 = clamp(b0*x0 + b1*x1 + b2*x2 + a1*y1 + a2*y2 + u, min, max)`. - /// - /// ``` - /// # use idsp::iir_int::*; - /// assert_eq!(Biquad::from(identity()).ba()[0], Biquad::ONE); - /// assert_eq!(Biquad::from(hold()).ba()[3], -Biquad::ONE); - /// ``` - pub fn ba(&self) -> &[i32; 5] { - &self.ba - } - - /// Mutable reference to the filter coefficients. - /// - /// See [`Biquad::ba()`]. - /// - /// ``` - /// # use idsp::iir_int::*; - /// let mut i = Biquad::default(); - /// i.ba_mut()[0] = Biquad::ONE; - /// assert_eq!(i, Biquad::from(identity())); - /// ``` - pub fn ba_mut(&mut self) -> &mut [i32; 5] { - &mut self.ba - } - - /// Summing junction offset - /// - /// This offset is applied to the output `y0` summing junction - /// on top of the feed-forward (`b`) and feed-back (`a`) terms. - /// The feedback samples are taken at the summing junction and - /// thus also include (and feed back) this offset. - pub fn u(&self) -> i32 { - self.u - } - - /// Set the summing junction offset - /// - /// See [`Biquad::u()`]. - /// - /// ``` - /// # use idsp::iir_int::*; - /// let mut i = Biquad::default(); - /// i.set_u(5); - /// assert_eq!(i.update(&mut [0; 5], 0), 5); - /// ``` - pub fn set_u(&mut self, u: i32) { - self.u = u; - } - - /// Lower output limit - /// - /// Guaranteed minimum output value. - /// The value is inclusive. - /// The clamping also cleanly affects the feedback terms. - /// - /// Note: `i32::MIN` should not be passed as the `y` samples stored in - /// the filter state are negated. Instead use `-i32::MAX` as the lowest - /// possible limit. - /// - /// ``` - /// # use idsp::iir_int::*; - /// assert_eq!(Biquad::default().min(), -i32::MAX); - /// ``` - pub fn min(&self) -> i32 { - self.min - } - - /// Set the lower output limit - /// - /// See [`Biquad::min()`]. - /// - /// ``` - /// # use idsp::iir_int::*; - /// let mut i = Biquad::default(); - /// i.set_min(5); - /// assert_eq!(i.update(&mut [0; 5], 0), 5); - /// ``` - pub fn set_min(&mut self, min: i32) { - self.min = min; - } - - /// Upper output limit - /// - /// Guaranteed maximum output value. - /// The value is inclusive. - /// The clamping also cleanly affects the feedback terms. - /// - /// ``` - /// # use idsp::iir_int::*; - /// assert_eq!(Biquad::default().max(), i32::MAX); - /// ``` - pub fn max(&self) -> i32 { - self.max - } - - /// Set the upper output limit - /// - /// See [`Biquad::max()`]. - /// - /// ``` - /// # use idsp::iir_int::*; - /// let mut i = Biquad::default(); - /// i.set_max(-5); - /// assert_eq!(i.update(&mut [0; 5], 0), -5); - /// ``` - pub fn set_max(&mut self, max: i32) { - self.max = max; - } - - /// Compute the overall (DC/proportional feed-forward) gain. - /// - /// ``` - /// # use idsp::iir_int::*; - /// assert_eq!(Biquad::from(proportional(3)).forward_gain(), 3); - /// ``` - /// - /// # Returns - /// The sum of the `b` feed-forward coefficients. - pub fn forward_gain(&self) -> i32 { - self.ba.iter().take(3).sum() - } - - /// Compute input-referred (`x`) offset. - /// - /// ``` - /// # use idsp::iir_int::*; - /// let mut i = Biquad::from(proportional(3)); - /// i.set_u(3); - /// assert_eq!(i.input_offset(), Biquad::ONE); - /// ``` - pub fn input_offset(&self) -> i32 { - (((self.u as i64) << Self::SHIFT) / self.forward_gain() as i64) as i32 - } - - /// Convert input (`x`) offset to equivalent summing junction offset (`u`) and apply. - /// - /// In the case of a "PID" controller the response behavior of the controller - /// to the offset is "stabilizing", and not "tracking": its frequency response - /// is exclusively according to the lowest non-zero [`PidAction`] gain. - /// There is no high order ("faster") response as would be the case for a "tracking" - /// controller. - /// - /// ``` - /// # use idsp::iir_int::*; - /// let mut i = Biquad::from(proportional(5 << 20)); - /// i.set_input_offset(3 << 20); - /// assert_eq!(i.u(), 15 << (20 + 20 - Biquad::SHIFT)); - /// ``` - /// - /// # Arguments - /// * `offset`: Input (`x`) offset. - pub fn set_input_offset(&mut self, offset: i32) { - self.u = (((1 << (Self::SHIFT - 1)) + offset as i64 * self.forward_gain() as i64) - >> Self::SHIFT) as i32; - } - - /// Coefficient fixed point format: signed Q2.30 - pub const SHIFT: u32 = 30; - pub const ONE: i32 = 1 << Self::SHIFT; - - /// Feed a new input value into the filter, update the filter state, and - /// return the new output. Only the state `xy` is modified. - /// - /// ``` - /// # use idsp::iir_int::*; - /// let i = Biquad::from(identity()); - /// let mut xy = [0, 1, 2, 3, 4]; - /// let x0 = 5; - /// let y0 = i.update(&mut xy, x0); - /// assert_eq!(y0, x0); - /// assert_eq!(xy, [x0, 0, -y0, 2, 3]); - /// ``` - /// - /// # Arguments - /// * `xy` - Current filter state. - /// On entry: `[x1, x2, -y1, -y2, -y3]` - /// On exit: `[x0, x1, -y0, -y1, -y2]` - /// * `x0` - New input. - /// - /// # Returns - /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 + a1*y1 + a2*y2 + u, min, max)` - pub fn update(&self, xy: &mut [i32; 5], x0: i32) -> i32 { - // `xy` contains x0 x1 -y0 -y1 -y2 - // Increment time x1 x2 -y1 -y2 -y3 - // Shift x1 x1 x2 -y1 -y2 - // This unrolls better than xy.rotate_right(1) - xy.copy_within(0..4, 1); - // Store x0 x0 x1 x2 -y1 -y2 - xy[0] = x0; - // Compute y0 by multiply-accumulate - let y0 = (xy - .iter() - .zip(self.ba.iter()) - .fold(1 << (Self::SHIFT - 1), |y0, (xy, ba)| { - y0 + *xy as i64 * *ba as i64 - }) - >> Self::SHIFT) as i32 - + self.u; - // Limit y0 - let y0 = y0.clamp(self.min, self.max); - // Store y0 x0 x1 -y0 -y1 -y2 - xy[2] = -y0; - y0 - } -} - -#[cfg(test)] -mod test { - use super::*; - - #[test] - fn lowpass_gen() { - let ba = Biquad::from(lowpass(5e-6, 0.5f64.sqrt(), 2.)); - println!("{:?}", ba); - } -} diff --git a/src/lib.rs b/src/lib.rs index 34affbf..aaf6a51 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -13,7 +13,6 @@ pub use complex::*; mod cossin; pub use cossin::*; pub mod iir; -pub mod iir_int; mod lockin; pub use lockin::*; mod lowpass; From 04909eede7d2b5b5e4656c5ec93006d3d7570c55 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 9 Jan 2024 14:52:16 +0100 Subject: [PATCH 07/26] iir: split --- rustfmt.toml | 1 + src/iir.rs | 1078 --------------------------------------- src/iir/biquad.rs | 480 +++++++++++++++++ src/iir/coefficients.rs | 348 +++++++++++++ src/iir/mod.rs | 6 + src/iir/pid.rs | 279 ++++++++++ src/lib.rs | 2 + src/num.rs | 115 +++++ 8 files changed, 1231 insertions(+), 1078 deletions(-) create mode 100644 rustfmt.toml delete mode 100644 src/iir.rs create mode 100644 src/iir/biquad.rs create mode 100644 src/iir/coefficients.rs create mode 100644 src/iir/mod.rs create mode 100644 src/iir/pid.rs create mode 100644 src/num.rs diff --git a/rustfmt.toml b/rustfmt.toml new file mode 100644 index 0000000..16bdde9 --- /dev/null +++ b/rustfmt.toml @@ -0,0 +1 @@ +format_code_in_doc_comments = true diff --git a/src/iir.rs b/src/iir.rs deleted file mode 100644 index 847b9f7..0000000 --- a/src/iir.rs +++ /dev/null @@ -1,1078 +0,0 @@ -use core::{ - iter::Sum, - ops::{Add, Neg, Sub}, -}; -use num_traits::{AsPrimitive, Float, FloatConst}; -use serde::{Deserialize, Serialize}; - -pub trait FilterNum: - Copy - + Sum - + PartialEq - + Neg - + Sub - + Add -where - Self: 'static, -{ - const ONE: Self; - const NEG_ONE: Self; - const ZERO: Self; - const MIN: Self; - const MAX: Self; - fn macc(self, xa: impl Iterator) -> Self; - fn mul(self, other: Self) -> Self; - fn div(self, other: Self) -> Self; - fn clamp(self, min: Self, max: Self) -> Self; - fn quantize(value: C) -> Self - where - Self: AsPrimitive, - C: Float + AsPrimitive; -} - -macro_rules! impl_float { - ($T:ty) => { - impl FilterNum for $T { - const ONE: Self = 1.0; - const NEG_ONE: Self = -Self::ONE; - const ZERO: Self = 0.0; - const MIN: Self = Self::NEG_INFINITY; - const MAX: Self = Self::INFINITY; - fn macc(self, xa: impl Iterator) -> Self { - xa.fold(self, |y, (a, x)| a.mul_add(x, y)) - // xa.fold(self, |y, (a, x)| y + a * x) - } - fn clamp(self, min: Self, max: Self) -> Self { - <$T>::clamp(self, min, max) - } - fn div(self, other: Self) -> Self { - self / other - } - fn mul(self, other: Self) -> Self { - self * other - } - fn quantize>(value: C) -> Self { - value.as_() - } - } - }; -} -impl_float!(f32); -impl_float!(f64); - -macro_rules! impl_int { - ($T:ty, $A:ty, $Q:literal) => { - impl FilterNum for $T { - const ONE: Self = 1 << $Q; - const NEG_ONE: Self = -Self::ONE; - const ZERO: Self = 0; - // Need to avoid `$T::MIN*$T::MIN` overflow. - const MIN: Self = -Self::MAX; - const MAX: Self = Self::MAX; - fn macc(self, xa: impl Iterator) -> Self { - self + (xa.fold(1 << ($Q - 1), |y, (a, x)| y + a as $A * x as $A) >> $Q) as Self - } - fn clamp(self, min: Self, max: Self) -> Self { - Ord::clamp(self, min, max) - } - fn div(self, other: Self) -> Self { - (((self as $A) << $Q) / other as $A) as Self - } - fn mul(self, other: Self) -> Self { - (((1 << ($Q - 1)) + self as $A * other as $A) >> $Q) as Self - } - fn quantize(value: C) -> Self - where - Self: AsPrimitive, - C: Float + AsPrimitive, - { - (value * Self::ONE.as_()).round().as_() - } - } - }; -} -// Q2.X chosen to be able to exactly and inclusively represent -2 as `-1 << X + 1` -impl_int!(i8, i16, 6); -impl_int!(i16, i32, 14); -impl_int!(i32, i64, 30); -impl_int!(i64, i128, 62); - -/// Filter architecture -/// -/// Direct Form 1 (DF1) and Direct Form 2 transposed (DF2T) are the only IIR filter -/// structures with an (effective bin the case of TDF2) single summing junction -/// this allows clamping of the output before feedback. -/// -/// DF1 allows atomic coefficient change because only x/y are pipelined. -/// The summing junctuion pipelining of TDF2 would require incremental -/// coefficient changes and is thus less amenable to online tuning. -/// -/// DF2T needs less state storage (2 instead of 4). This is in addition to the coefficient storage -/// (5 plus 2 limits plus 1 offset) -/// This implementation already saves storage by decoupling coefficients/limits and offset from state -/// and thus supports both (a) sharing a single filter between multiple states ("channels") and (b) -/// rapid switching of filters (tuning, transfer) for a given state without copying. -/// -/// DF2T is less efficient and accurate for fixed-point architectures as quantization -/// happens at each intermediate summing junction in addition to the output quantization. This is -/// especially true for common `i64 + i32 * i32 -> i64` MACC architectures. -/// -/// # Coefficients and state -/// -/// `[T; 5]` is both the IIR state and coefficients type. -/// -/// To represent the IIR state (input and output memory) during [`Biquad::update()`] -/// this contains the three inputs `[x0, x1, x2]` and the two outputs `[y1, y2]` -/// concatenated. Lower indices correspond to more recent samples. -/// To represent the IIR coefficients, this contains the feed-forward -/// coefficients `[b0, b1, b2]` followd by the negated feed-back coefficients -/// `[a1, a2]`, all five normalized such that `a0 = 1`. -/// Note that between filter [`Biquad::update()`] the `xy` state contains -/// `[x0, x1, y0, y1, y2]`. -/// -/// The IIR coefficients can be mapped to other transfer function -/// representations, for example as described in -/// -/// # IIR filter as PID controller -/// -/// Contains the coeeficients `ba`, the summing junction offset `u`, and the -/// output limits `min` and `max`. Data is represented in floating-point -/// for all internal signals, input and output. -/// -/// This implementation achieves several important properties: -/// -/// * Its transfer function is universal in the sense that any biquadratic -/// transfer function can be implemented (high-passes, gain limits, second -/// order integrators with inherent anti-windup, notches etc) without code -/// changes preserving all features. -/// * It inherits a universal implementation of "integrator anti-windup", also -/// and especially in the presence of set-point changes and in the presence -/// of proportional or derivative gain without any back-off that would reduce -/// steady-state output range. -/// * It has universal derivative-kick (undesired, unlimited, and un-physical -/// amplification of set-point changes by the derivative term) avoidance. -/// * An offset at the input of an IIR filter (a.k.a. "set-point") is -/// equivalent to an offset at the summing junction (in output units). -/// They are related by the overall (DC feed-forward) gain of the filter. -/// * It stores only previous outputs and inputs. These have direct and -/// invariant interpretation (independent of coefficients and offset). -/// Therefore it can trivially implement bump-less transfer between any -/// coefficients/offset sets. -/// * Cascading multiple IIR filters allows stable and robust -/// implementation of transfer functions beyond bequadratic terms. -/// -/// See also . -/// -/// -/// Offset and limiting disabled to suit lowpass applications. -/// Coefficient scaling fixed and optimized such that -2 is representable. -/// Tailored to low-passes, PID, II etc, where the integration rule is [1, -2, 1]. -/// Since the relevant coefficients `a1` and `a2` are negated, we also negate the -/// stored `y1` and `y2` in the state. -/// Note that `xy` contains the negative `y1` and `y2`, such that `-a1` -#[derive(Clone, Debug, Deserialize, Serialize, PartialEq, PartialOrd)] -pub struct Biquad { - ba: [T; 5], - u: T, - min: T, - max: T, -} - -impl Default for Biquad { - fn default() -> Self { - Self { - ba: [T::ZERO; 5], - u: T::ZERO, - min: T::MIN, - max: T::MAX, - } - } -} - -impl From<[T; 5]> for Biquad { - fn from(ba: [T; 5]) -> Self { - Self { - ba, - ..Default::default() - } - } -} - -impl From<[C; 6]> for Biquad -where - T: FilterNum + AsPrimitive, - C: Float + AsPrimitive, -{ - fn from(ba: [C; 6]) -> Self { - let ia0 = C::one() / ba[3]; - Self::from([ - T::quantize(ba[0] * ia0), - T::quantize(ba[1] * ia0), - T::quantize(ba[2] * ia0), - // b[3]: a0*ia0 - T::quantize(ba[4] * ia0), - T::quantize(ba[5] * ia0), - ]) - } -} - -impl Biquad { - /// A "hold" filter that ingests input and maintains output - /// - /// ``` - /// # use idsp::iir::*; - /// let mut xy = core::array::from_fn(|i| i as _); - /// let x0 = 7.0; - /// let y0 = Biquad::HOLD.update(&mut xy, x0); - /// assert_eq!(y0, -2.0); - /// assert_eq!(xy, [x0, 0.0, -y0, -y0, 3.0]); - /// ``` - pub const HOLD: Self = Self { - ba: [T::ZERO, T::ZERO, T::ZERO, T::NEG_ONE, T::ZERO], - u: T::ZERO, - min: T::MIN, - max: T::MAX, - }; - - /// A unity gain filter - /// - /// ``` - /// # use idsp::iir::*; - /// let x0 = 3.0; - /// let y0 = Biquad::IDENTITY.update(&mut [0.0; 5], x0); - /// assert_eq!(y0, x0); - /// ``` - pub const IDENTITY: Self = Self::proportional(T::ONE); - - /// A filter with the given proportional gain at all frequencies - /// - /// ``` - /// # use idsp::iir::*; - /// let x0 = 2.0; - /// let k = 5.0; - /// let y0 = Biquad::proportional(k).update(&mut [0.0; 5], x0); - /// assert_eq!(y0, x0 * k); - /// ``` - pub const fn proportional(k: T) -> Self { - Self { - ba: [k, T::ZERO, T::ZERO, T::ZERO, T::ZERO], - u: T::ZERO, - min: T::MIN, - max: T::MAX, - } - } - - /// Filter coefficients - /// - /// IIR filter tap gains (`ba`) are an array `[b0, b1, b2, a1, a2]` such that - /// [`Biquad::update(&mut xy, x0)`] returns - /// `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)`. - /// - /// ``` - /// # use idsp::iir::*; - /// assert_eq!(Biquad::::IDENTITY.ba()[0], i32::ONE); - /// assert_eq!(Biquad::::HOLD.ba()[3], -i32::ONE); - /// ``` - pub fn ba(&self) -> &[T; 5] { - &self.ba - } - - /// Mutable reference to the filter coefficients. - /// - /// See [`Biquad::ba()`]. - /// - /// ``` - /// # use idsp::iir::*; - /// let mut i = Biquad::default(); - /// i.ba_mut()[0] = i32::ONE; - /// assert_eq!(i, Biquad::IDENTITY); - /// ``` - pub fn ba_mut(&mut self) -> &mut [T; 5] { - &mut self.ba - } - - /// Summing junction offset - /// - /// This offset is applied to the output `y0` summing junction - /// on top of the feed-forward (`b`) and feed-back (`a`) terms. - /// The feedback samples are taken at the summing junction and - /// thus also include (and feed back) this offset. - pub fn u(&self) -> T { - self.u - } - - /// Set the summing junction offset - /// - /// See [`Biquad::u()`]. - /// - /// ``` - /// # use idsp::iir::*; - /// let mut i = Biquad::default(); - /// i.set_u(5); - /// assert_eq!(i.update(&mut [0; 5], 0), 5); - /// ``` - pub fn set_u(&mut self, u: T) { - self.u = u; - } - - /// Lower output limit - /// - /// Guaranteed minimum output value. - /// The value is inclusive. - /// The clamping also cleanly affects the feedback terms. - /// - /// Note: For fixed point filters `Biquad`, `T::MIN` should not be passed - /// to `min()` since the `y` samples stored in - /// the filter state are negated. Instead use `-T::MAX` as the lowest - /// possible limit. - /// - /// ``` - /// # use idsp::iir::*; - /// assert_eq!(Biquad::::default().min(), -i32::MAX); - /// ``` - pub fn min(&self) -> T { - self.min - } - - /// Set the lower output limit - /// - /// See [`Biquad::min()`]. - /// - /// ``` - /// # use idsp::iir::*; - /// let mut i = Biquad::default(); - /// i.set_min(7); - /// assert_eq!(i.update(&mut [0; 5], 0), 7); - /// ``` - pub fn set_min(&mut self, min: T) { - self.min = min; - } - - /// Upper output limit - /// - /// Guaranteed maximum output value. - /// The value is inclusive. - /// The clamping also cleanly affects the feedback terms. - /// - /// ``` - /// # use idsp::iir::*; - /// assert_eq!(Biquad::::default().max(), i32::MAX); - /// ``` - pub fn max(&self) -> T { - self.max - } - - /// Set the upper output limit - /// - /// See [`Biquad::max()`]. - /// - /// ``` - /// # use idsp::iir::*; - /// let mut i = Biquad::default(); - /// i.set_max(-7); - /// assert_eq!(i.update(&mut [0; 5], 0), -7); - /// ``` - pub fn set_max(&mut self, max: T) { - self.max = max; - } - - /// Compute the overall (DC/proportional feed-forward) gain. - /// - /// ``` - /// # use idsp::iir::*; - /// assert_eq!(Biquad::proportional(3.0).forward_gain(), 3.0); - /// ``` - /// - /// # Returns - /// The sum of the `b` feed-forward coefficients. - pub fn forward_gain(&self) -> T { - self.ba.iter().take(3).copied().sum() - } - - /// Compute input-referred (`x`) offset. - /// ``` - /// # use idsp::iir::*; - /// let mut i = Biquad::proportional(3); - /// i.set_u(3); - /// assert_eq!(i.input_offset(), i32::ONE); - /// ``` - pub fn input_offset(&self) -> T { - self.u.div(self.forward_gain()) - } - - /// Convert input (`x`) offset to equivalent summing junction offset (`u`) and apply. - /// - /// In the case of a "PID" controller the response behavior of the controller - /// to the offset is "stabilizing", and not "tracking": its frequency response - /// is exclusively according to the lowest non-zero [`Action`] gain. - /// There is no high order ("faster") response as would be the case for a "tracking" - /// controller. - /// - /// ``` - /// # use idsp::iir::*; - /// let mut i = Biquad::proportional(3.0); - /// i.set_input_offset(2.0); - /// let x0 = 0.5; - /// let y0 = i.update(&mut [0.0; 5], x0); - /// assert_eq!(y0, (x0 + i.input_offset()) * i.forward_gain()); - /// ``` - /// - /// ``` - /// # use idsp::iir::*; - /// let mut i = Biquad::proportional(-i32::ONE); - /// i.set_input_offset(1); - /// assert_eq!(i.u(), -1); - /// ``` - /// - /// # Arguments - /// * `offset`: Input (`x`) offset. - pub fn set_input_offset(&mut self, offset: T) { - self.u = offset.mul(self.forward_gain()); - } - - /// Direct Form 1 Update - /// - /// Ingest a new input value into the filter, update the filter state, and - /// return the new output. Only the state `xy` is modified. - /// - /// ``` - /// # use idsp::iir::*; - /// let mut xy = core::array::from_fn(|i| i as _); - /// let x0 = 3.0; - /// let y0 = Biquad::IDENTITY.update(&mut xy, x0); - /// assert_eq!(y0, x0); - /// assert_eq!(xy, [x0, 0.0, -y0, 2.0, 3.0]); - /// ``` - /// - /// # Arguments - /// * `xy` - Current filter state. - /// On entry: `[x1, x2, -y1, -y2, -y3]` - /// On exit: `[x0, x1, -y0, -y1, -y2]` - /// * `x0` - New input. - /// - /// # Returns - /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` - /// - /// # Panics - /// Panics in debug mode if `!(self.min <= self.max)`. - pub fn update(&self, xy: &mut [T; 5], x0: T) -> T { - // `xy` contains x0 x1 -y0 -y1 -y2 - // Increment time x1 x2 -y1 -y2 -y3 - // Shift x1 x1 x2 -y1 -y2 - xy.copy_within(0..4, 1); - // Store x0 x0 x1 x2 -y1 -y2 - xy[0] = x0; - // Compute y0 - let y0 = self - .u - .macc(xy.iter().copied().zip(self.ba.iter().copied())) - .clamp(self.min, self.max); - // Store -y0 x0 x1 -y0 -y1 -y2 - xy[2] = -y0; - y0 - } - - /// Direct Form 1 Update - /// - /// Ingest a new input value into the filter, update the filter state, and - /// return the new output. Only the state `xy` is modified. - /// - /// ``` - /// # use idsp::iir::*; - /// let mut xy = core::array::from_fn(|i| i as _); - /// let x0 = 3.0; - /// let y0 = Biquad::IDENTITY.update_df1(&mut xy, x0); - /// assert_eq!(y0, x0); - /// assert_eq!(xy, [x0, 0.0, -y0, 2.0]); - /// ``` - /// - /// # Arguments - /// * `xy` - Current filter state. - /// On entry: `[x1, x2, -y1, -y2]` - /// On exit: `[x0, x1, -y0, -y1]` - /// * `x0` - New input. - /// - /// # Returns - /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` - /// - /// # Panics - /// Panics in debug mode if `!(self.min <= self.max)`. - pub fn update_df1(&self, xy: &mut [T; 4], x0: T) -> T { - // `xy` contains x0 x1 -y0 -y1 - // Increment time x1 x2 -y1 -y2 - // Compute y0 - let y0 = self - .u - .macc( - core::iter::once(x0) - .chain(xy.iter().copied()) - .zip(self.ba.iter().copied()), - ) - .clamp(self.min, self.max); - // Shift x1 x1 -y1 -y2 - xy[1] = xy[0]; - // Store x0 x0 x1 -y1 -y2 - xy[0] = x0; - // Shift x0 x1 -y0 -y1 - xy[3] = xy[2]; - // Store -y0 x0 x1 -y0 -y1 - xy[2] = -y0; - y0 - } - - /// Ingest new input and perform a Direct Form 2 Transposed update. - /// - /// ``` - /// # use idsp::iir::*; - /// let mut xy = core::array::from_fn(|i| i as _); - /// let x0 = 3.0; - /// let y0 = Biquad::IDENTITY.update_df2t(&mut xy, x0); - /// assert_eq!(y0, x0); - /// assert_eq!(xy, [1.0, 0.0]); - /// ``` - /// - /// # Arguments - /// * `s` - Current filter state. - /// On entry: `[b1*x1 + b2*x2 - a1*y1 - a2*y2, b2*x1 - a2*y1]` - /// On exit: `[b1*x0 + b2*x1 - a1*y0 - a2*y1, b2*x0 - a2*y0]` - /// * `x0` - New input. - /// - /// # Returns - /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` - /// - /// # Panics - /// Panics in debug mode if `!(self.min <= self.max)`. - pub fn update_df2t(&self, u: &mut [T; 2], x0: T) -> T { - let y0 = (u[0] + self.ba[0].mul(x0)).clamp(self.min, self.max); - u[0] = u[1] + self.ba[1].mul(x0) - self.ba[3].mul(y0); - u[1] = self.u + self.ba[2].mul(x0) - self.ba[4].mul(y0); - y0 - } -} - -#[derive(Copy, Clone, Debug, PartialEq, PartialOrd, Serialize, Deserialize)] -enum Shape { - InverseQ(T), - Bandwidth(T), - Slope(T), -} - -impl Default for Shape { - fn default() -> Self { - Self::InverseQ(T::SQRT_2()) - } -} - -/// Standard audio biquad filter builder -/// -/// -#[derive(Clone, Debug, PartialEq, PartialOrd, Serialize, Deserialize)] -pub struct Filter { - /// Angular critical frequency (in units of sampling frequency) - /// Corner frequency, or 3dB cutoff frequency, - w0: T, - /// Passband gain - gain: T, - /// Shelf gain (only for peaking, lowshelf, highshelf) - /// Relative to passband gain - shelf: T, - /// Inverse Q - shape: Shape, -} - -impl Default for Filter { - fn default() -> Self { - Self { - w0: T::zero(), - gain: T::one(), - shape: Shape::default(), - shelf: T::one(), - } - } -} - -impl Filter -where - T: 'static + Float + FloatConst, - f32: AsPrimitive, -{ - pub fn frequency(self, critical_frequency: T, sample_frequency: T) -> Self { - self.critical_frequency(critical_frequency / sample_frequency) - } - - pub fn critical_frequency(self, critical_frequency: T) -> Self { - self.angular_critical_frequency(T::TAU() * critical_frequency) - } - - pub fn angular_critical_frequency(mut self, w0: T) -> Self { - self.w0 = w0; - self - } - - pub fn gain(mut self, k: T) -> Self { - self.gain = k; - self - } - - pub fn gain_db(self, k_db: T) -> Self { - self.gain(10.0.as_().powf(k_db / 20.0.as_())) - } - - pub fn shelf(mut self, a: T) -> Self { - self.shelf = a; - self - } - - pub fn shelf_db(self, k_db: T) -> Self { - self.gain(10.0.as_().powf(k_db / 20.0.as_())) - } - - pub fn inverse_q(mut self, qi: T) -> Self { - self.shape = Shape::InverseQ(qi); - self - } - - pub fn q(self, q: T) -> Self { - self.inverse_q(T::one() / q) - } - - /// Set [`FilterBuilder::frequency()`] first. - /// In octaves. - pub fn bandwidth(mut self, bw: T) -> Self { - self.shape = Shape::Bandwidth(bw); - self - } - - /// Set [`FilterBuilder::gain()`] first. - pub fn shelf_slope(mut self, s: T) -> Self { - self.shape = Shape::Slope(s); - self - } - - fn qi(&self) -> T { - match self.shape { - Shape::InverseQ(qi) => qi, - Shape::Bandwidth(bw) => { - 2.0.as_() * (T::LN_2() / 2.0.as_() * bw * self.w0 / self.w0.sin()).sinh() - } - Shape::Slope(s) => { - ((self.gain + T::one() / self.gain) * (T::one() / s - T::one()) + 2.0.as_()).sqrt() - } - } - } - - fn alpha(&self) -> T { - 0.5.as_() * self.w0.sin() * self.qi() - } - - /// Lowpass biquad filter. - /// - /// ``` - /// use idsp::iir::*; - /// let ba = Filter::default().critical_frequency(0.1).lowpass(); - /// println!("{ba:?}"); - /// ``` - pub fn lowpass(self) -> [T; 6] { - let fcos = self.w0.cos(); - let alpha = self.alpha(); - let b = self.gain * 0.5.as_() * (T::one() - fcos); - [ - b, - b + b, - b, - T::one() + alpha, - -(fcos + fcos), - T::one() - alpha, - ] - } - - pub fn highpass(self) -> [T; 6] { - let fcos = self.w0.cos(); - let alpha = self.alpha(); - let b = self.gain * 0.5.as_() * (T::one() + fcos); - [ - b, - -(b + b), - b, - T::one() + alpha, - -(fcos + fcos), - T::one() - alpha, - ] - } - - pub fn bandpass(self) -> [T; 6] { - let fcos = self.w0.cos(); - let alpha = self.alpha(); - let b = self.gain * alpha; - [ - b, - T::zero(), - b, - T::one() + alpha, - -(fcos + fcos), - T::one() - alpha, - ] - } - - pub fn notch(self) -> [T; 6] { - let fcos = self.w0.cos(); - let alpha = self.alpha(); - [ - self.gain, - -(fcos + fcos) * self.gain, - self.gain, - T::one() + alpha, - -(fcos + fcos), - T::one() - alpha, - ] - } - - pub fn allpass(self) -> [T; 6] { - let fcos = self.w0.cos(); - let alpha = self.alpha(); - [ - (T::one() - alpha) * self.gain, - -(fcos + fcos) * self.gain, - (T::one() + alpha) * self.gain, - T::one() + alpha, - -(fcos + fcos), - T::one() - alpha, - ] - } - - pub fn peaking(self) -> [T; 6] { - let fcos = self.w0.cos(); - let alpha = self.alpha(); - [ - (T::one() + alpha * self.shelf) * self.gain, - -(fcos + fcos) * self.gain, - (T::one() - alpha * self.shelf) * self.gain, - T::one() + alpha / self.shelf, - -(fcos + fcos), - T::one() - alpha / self.shelf, - ] - } - - pub fn lowshelf(self) -> [T; 6] { - let fcos = self.w0.cos(); - let sp1 = self.shelf + T::one(); - let sm1 = self.shelf - T::one(); - let tsa = 2.0.as_() * self.shelf.sqrt() * self.alpha(); - [ - self.shelf * self.gain * (sp1 - sm1 * fcos + tsa), - 2.0.as_() * self.shelf * self.gain * (sm1 - sp1 * fcos), - self.shelf * self.gain * (sp1 - sm1 * fcos - tsa), - sp1 + sm1 * fcos + tsa, - (-2.0).as_() * (sm1 + sp1 * fcos), - sp1 + sm1 * fcos - tsa, - ] - } - - pub fn highshelf(self) -> [T; 6] { - let fcos = self.w0.cos(); - let sp1 = self.shelf + T::one(); - let sm1 = self.shelf - T::one(); - let tsa = 2.0.as_() * self.shelf.sqrt() * self.alpha(); - [ - self.shelf * self.gain * (sp1 + sm1 * fcos + tsa), - (-2.0).as_() * self.shelf * self.gain * (sm1 + sp1 * fcos), - self.shelf * self.gain * (sp1 + sm1 * fcos - tsa), - sp1 - sm1 * fcos + tsa, - 2.0.as_() * (sm1 - sp1 * fcos), - sp1 - sm1 * fcos - tsa, - ] - } - - // TODO - // PI-notch - // - // SOS cascades: - // butterworth - // elliptic - // chebychev1/2 - // bessel -} - -/// PID controller builder -/// -/// Builds `Biquad` from action gains, gain limits, input offset and output limits. -/// -/// ``` -/// # use idsp::iir::*; -/// let b: Biquad = Pid::default() -/// .period(1e-3) -/// .gain(Action::Ki, 1e-3) -/// .gain(Action::Kp, 1.0) -/// .gain(Action::Kd, 1e2) -/// .limit(Action::Ki, 1e3) -/// .limit(Action::Kd, 1e1) -/// .build() -/// .unwrap() -/// .into(); -/// ``` -#[derive(Debug, Clone, PartialEq, PartialOrd, Serialize, Deserialize)] -pub struct Pid { - period: T, - gains: [T; 5], - limits: [T; 5], -} - -impl Default for Pid { - fn default() -> Self { - Self { - period: T::one(), - gains: [T::zero(); 5], - limits: [T::infinity(); 5], - } - } -} - -/// [`Pid::build()`] errors -#[derive(Copy, Clone, Debug, PartialEq, Eq, Ord, PartialOrd, Serialize, Deserialize)] -#[non_exhaustive] -pub enum PidError { - /// The action gains cover more than three successive orders - OrderRange, -} - -/// PID action -/// -/// This enumerates the five possible PID style actions of a [`Biquad`] -#[derive(Copy, Clone, Debug, PartialEq, Eq, Ord, PartialOrd, Serialize, Deserialize)] -pub enum Action { - /// Double integrating, -40 dB per decade - Kii = 0, - /// Integrating, -20 dB per decade - Ki = 1, - /// Proportional - Kp = 2, - /// Derivative=, 20 dB per decade - Kd = 3, - /// Double derivative, 40 dB per decade - Kdd = 4, -} - -impl> Pid { - /// Sample period - /// - /// # Arguments - /// * `period`: Sample period in some units, e.g. SI seconds - pub fn period(mut self, period: T) -> Self { - self.period = period; - self - } - - /// Gain for a given action - /// - /// Gain units are `output/input * time.powi(order)` where - /// * `output` are output (`y`) units - /// * `input` are input (`x`) units - /// * `time` are sample period units, e.g. SI seconds - /// * `order` is the action order: the frequency exponent - /// (`-1` for integrating, `0` for proportional, etc.) - /// - /// Note that inverse time units correspond to angular frequency units. - /// Gains are accurate in the low frequency limit. Towards Nyquist, the - /// frequency response is warped. - /// - /// ``` - /// # use idsp::iir::*; - /// let tau = 1e-3; - /// let ki = 1e-4; - /// let i: Biquad = Pid::default() - /// .period(tau) - /// .gain(Action::Ki, ki) - /// .build() - /// .unwrap() - /// .into(); - /// let x0 = 5.0; - /// let y0 = i.update(&mut [0.0; 5], x0); - /// assert!((y0 / (x0 * ki / tau) - 1.0).abs() < 2.0 * f32::EPSILON); - /// ``` - /// - /// # Arguments - /// * `action`: Action to control - /// * `gain`: Gain value - pub fn gain(mut self, action: Action, gain: T) -> Self { - self.gains[action as usize] = gain; - self - } - - /// Gain limit for a given action - /// - /// Gain limit units are `output/input`. See also [`Pid::gain()`]. - /// Multiple gains and limits may interact and lead to peaking. - /// - /// ``` - /// # use idsp::iir::*; - /// let ki_limit = 1e3; - /// let i: Biquad = Pid::default() - /// .gain(Action::Ki, 8.0) - /// .limit(Action::Ki, ki_limit) - /// .build() - /// .unwrap() - /// .into(); - /// let mut xy = [0.0; 5]; - /// let x0 = 5.0; - /// for _ in 0..1000 { - /// i.update(&mut xy, x0); - /// } - /// let y0 = i.update(&mut xy, x0); - /// assert!((y0 / (x0 * ki_limit) - 1.0f32).abs() < 1e-3); - /// ``` - /// - /// # Arguments - /// * `action`: Action to limit in gain - /// * `limit`: Gain limit - pub fn limit(mut self, action: Action, limit: T) -> Self { - self.limits[action as usize] = limit; - self - } - - /// Perform checks, compute coefficients and return `Biquad`. - /// - /// No attempt is made to detect NaNs, non-finite gains, non-positive period, - /// zero gain limits, or gain/limit sign mismatches. - /// These will consequently result in NaNs/infinities, peaking, or notches in - /// the Biquad coefficients. - /// - /// Gain limits for zero gain actions or for proportional action are ignored. - /// - /// ``` - /// # use idsp::iir::*; - /// let i: Biquad = Pid::default().gain(Action::Kp, 3.0).build().unwrap().into(); - /// assert_eq!(i, Biquad::proportional(3.0)); - /// ``` - pub fn build>(self) -> Result<[C; 5], PidError> - where - T: AsPrimitive, - { - const KP: usize = Action::Kp as usize; - - // Determine highest denominator (feedback, `a`) order - let low = self - .gains - .iter() - .take(KP) - .position(|g| !g.is_zero()) - .unwrap_or(KP); - - if self.gains.iter().skip(low + 3).any(|g| !g.is_zero()) { - return Err(PidError::OrderRange); - } - - // Derivative/integration kernels - let kernels = [ - [C::ONE, C::ZERO, C::ZERO], - [C::ONE, C::NEG_ONE, C::ZERO], - [C::ONE, C::NEG_ONE + C::NEG_ONE, C::ONE], - ]; - - // Scale gains, compute limits, quantize - let mut zi = self.period.powi(low as i32 - KP as i32); - let mut gl = [[T::zero(); 2]; 3]; - for (gli, (i, (ggi, lli))) in gl.iter_mut().zip( - self.gains - .iter() - .zip(self.limits.iter()) - .enumerate() - .skip(low), - ) { - gli[0] = *ggi * zi; - gli[1] = if i == KP { T::one() } else { gli[0] / *lli }; - zi = zi * self.period; - } - let a0i = T::one() / gl.iter().map(|gli| gli[1]).sum(); - - // Coefficients - let mut ba = [[C::ZERO; 2]; 3]; - for (gli, ki) in gl.iter().zip(kernels.iter()) { - let (g, l) = (C::quantize(gli[0] * a0i), C::quantize(gli[1] * a0i)); - for (j, baj) in ba.iter_mut().enumerate() { - *baj = [baj[0] + ki[j].mul(g), baj[1] + ki[j].mul(l)]; - } - } - - Ok([ba[0][0], ba[1][0], ba[2][0], ba[1][1], ba[2][1]]) - } -} - -#[cfg(test)] -mod test { - use super::*; - - #[test] - fn pid() { - let b: Biquad = Pid::default() - .period(1.0) - .gain(Action::Ki, 1e-3) - .gain(Action::Kp, 1.0) - .gain(Action::Kd, 1e2) - .limit(Action::Ki, 1e3) - .limit(Action::Kd, 1e1) - .build() - .unwrap() - .into(); - let want = [ - 9.18190826, - -18.27272561, - 9.09090826, - -1.90909074, - 0.90909083, - ]; - for (ba_have, ba_want) in b.ba.iter().zip(want.iter()) { - assert!( - (ba_have / ba_want - 1.0).abs() < 2.0 * f32::EPSILON, - "have {:?} != want {want:?}", - &b.ba, - ); - } - } - - #[test] - fn pid_i32() { - let b: Biquad = Pid::default() - .period(1.0) - .gain(Action::Ki, 1e-5) - .gain(Action::Kp, 1e-2) - .gain(Action::Kd, 1e0) - .limit(Action::Ki, 1e1) - .limit(Action::Kd, 1e-1) - .build() - .unwrap() - .into(); - println!("{b:?}"); - } - - #[test] - fn units() { - let ki = 5e-2; - let tau = 3e-3; - let b: Biquad = Pid::default() - .period(tau) - .gain(Action::Ki, ki) - .build() - .unwrap() - .into(); - let mut xy = [0.0; 5]; - for i in 1..10 { - let y_have = b.update(&mut xy, 1.0); - let y_want = (i as f32) * (ki / tau); - assert!( - (y_have / y_want - 1.0).abs() < 3.0 * f32::EPSILON, - "{i}: have {y_have} != {y_want}" - ); - } - } - - #[test] - fn lowpass_gen() { - let ba = Biquad::::from( - Filter::default() - .critical_frequency(2e-9f64) - .gain(2e7) - .lowpass(), - ); - println!("{:?}", ba); - } -} diff --git a/src/iir/biquad.rs b/src/iir/biquad.rs new file mode 100644 index 0000000..d433f5b --- /dev/null +++ b/src/iir/biquad.rs @@ -0,0 +1,480 @@ +use num_traits::{AsPrimitive, Float}; +use serde::{Deserialize, Serialize}; + +use crate::FilterNum; + +/// Filter architecture +/// +/// Direct Form 1 (DF1) and Direct Form 2 transposed (DF2T) are the only IIR filter +/// structures with an (effective bin the case of TDF2) single summing junction +/// this allows clamping of the output before feedback. +/// +/// DF1 allows atomic coefficient change because only x/y are pipelined. +/// The summing junctuion pipelining of TDF2 would require incremental +/// coefficient changes and is thus less amenable to online tuning. +/// +/// DF2T needs less state storage (2 instead of 4). This is in addition to the coefficient storage +/// (5 plus 2 limits plus 1 offset) +/// This implementation already saves storage by decoupling coefficients/limits and offset from state +/// and thus supports both (a) sharing a single filter between multiple states ("channels") and (b) +/// rapid switching of filters (tuning, transfer) for a given state without copying. +/// +/// DF2T is less efficient and accurate for fixed-point architectures as quantization +/// happens at each intermediate summing junction in addition to the output quantization. This is +/// especially true for common `i64 + i32 * i32 -> i64` MACC architectures. +/// +/// # Coefficients and state +/// +/// `[T; 5]` is both the IIR state and coefficients type. +/// +/// To represent the IIR state (input and output memory) during [`Biquad::update()`] +/// this contains the three inputs `[x0, x1, x2]` and the two outputs `[y1, y2]` +/// concatenated. Lower indices correspond to more recent samples. +/// To represent the IIR coefficients, this contains the feed-forward +/// coefficients `[b0, b1, b2]` followd by the negated feed-back coefficients +/// `[a1, a2]`, all five normalized such that `a0 = 1`. +/// Note that between filter [`Biquad::update()`] the `xy` state contains +/// `[x0, x1, y0, y1, y2]`. +/// +/// The IIR coefficients can be mapped to other transfer function +/// representations, for example as described in +/// +/// # IIR filter as PID controller +/// +/// Contains the coeeficients `ba`, the summing junction offset `u`, and the +/// output limits `min` and `max`. Data is represented in floating-point +/// for all internal signals, input and output. +/// +/// This implementation achieves several important properties: +/// +/// * Its transfer function is universal in the sense that any biquadratic +/// transfer function can be implemented (high-passes, gain limits, second +/// order integrators with inherent anti-windup, notches etc) without code +/// changes preserving all features. +/// * It inherits a universal implementation of "integrator anti-windup", also +/// and especially in the presence of set-point changes and in the presence +/// of proportional or derivative gain without any back-off that would reduce +/// steady-state output range. +/// * It has universal derivative-kick (undesired, unlimited, and un-physical +/// amplification of set-point changes by the derivative term) avoidance. +/// * An offset at the input of an IIR filter (a.k.a. "set-point") is +/// equivalent to an offset at the summing junction (in output units). +/// They are related by the overall (DC feed-forward) gain of the filter. +/// * It stores only previous outputs and inputs. These have direct and +/// invariant interpretation (independent of coefficients and offset). +/// Therefore it can trivially implement bump-less transfer between any +/// coefficients/offset sets. +/// * Cascading multiple IIR filters allows stable and robust +/// implementation of transfer functions beyond bequadratic terms. +/// +/// See also . +/// +/// +/// Offset and limiting disabled to suit lowpass applications. +/// Coefficient scaling fixed and optimized such that -2 is representable. +/// Tailored to low-passes, PID, II etc, where the integration rule is [1, -2, 1]. +/// Since the relevant coefficients `a1` and `a2` are negated, we also negate the +/// stored `y1` and `y2` in the state. +/// Note that `xy` contains the negative `y1` and `y2`, such that `-a1` +#[derive(Copy, Clone, Debug, Deserialize, Serialize, PartialEq, PartialOrd)] +pub struct Biquad { + ba: [T; 5], + u: T, + min: T, + max: T, +} + +impl Default for Biquad { + fn default() -> Self { + Self { + ba: [T::ZERO; 5], + u: T::ZERO, + min: T::MIN, + max: T::MAX, + } + } +} + +impl From<[T; 5]> for Biquad { + fn from(ba: [T; 5]) -> Self { + Self { + ba, + ..Default::default() + } + } +} + +impl From<[C; 6]> for Biquad +where + T: FilterNum + AsPrimitive, + C: Float + AsPrimitive, +{ + fn from(ba: [C; 6]) -> Self { + let ia0 = C::one() / ba[3]; + Self::from([ + T::quantize(ba[0] * ia0), + T::quantize(ba[1] * ia0), + T::quantize(ba[2] * ia0), + // b[3]: a0*ia0 + T::quantize(ba[4] * ia0), + T::quantize(ba[5] * ia0), + ]) + } +} + +impl From> for [C; 6] +where + T: FilterNum + AsPrimitive, + C: 'static + Copy, +{ + fn from(value: Biquad) -> Self { + let ba = value.ba(); + [ + ba[0].as_(), + ba[1].as_(), + ba[2].as_(), + T::ONE.as_(), + ba[3].as_(), + ba[4].as_(), + ] + } +} + +impl Biquad { + /// A "hold" filter that ingests input and maintains output + /// + /// ``` + /// # use idsp::iir::*; + /// let mut xy = core::array::from_fn(|i| i as _); + /// let x0 = 7.0; + /// let y0 = Biquad::HOLD.update(&mut xy, x0); + /// assert_eq!(y0, -2.0); + /// assert_eq!(xy, [x0, 0.0, -y0, -y0, 3.0]); + /// ``` + pub const HOLD: Self = Self { + ba: [T::ZERO, T::ZERO, T::ZERO, T::NEG_ONE, T::ZERO], + u: T::ZERO, + min: T::MIN, + max: T::MAX, + }; + + /// A unity gain filter + /// + /// ``` + /// # use idsp::iir::*; + /// let x0 = 3.0; + /// let y0 = Biquad::IDENTITY.update(&mut [0.0; 5], x0); + /// assert_eq!(y0, x0); + /// ``` + pub const IDENTITY: Self = Self::proportional(T::ONE); + + /// A filter with the given proportional gain at all frequencies + /// + /// ``` + /// # use idsp::iir::*; + /// let x0 = 2.0; + /// let k = 5.0; + /// let y0 = Biquad::proportional(k).update(&mut [0.0; 5], x0); + /// assert_eq!(y0, x0 * k); + /// ``` + pub const fn proportional(k: T) -> Self { + Self { + ba: [k, T::ZERO, T::ZERO, T::ZERO, T::ZERO], + u: T::ZERO, + min: T::MIN, + max: T::MAX, + } + } + + /// Filter coefficients + /// + /// IIR filter tap gains (`ba`) are an array `[b0, b1, b2, a1, a2]` such that + /// [`Biquad::update(&mut xy, x0)`] returns + /// `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)`. + /// + /// ``` + /// # use idsp::FilterNum; + /// # use idsp::iir::*; + /// assert_eq!(Biquad::::IDENTITY.ba()[0], i32::ONE); + /// assert_eq!(Biquad::::HOLD.ba()[3], -i32::ONE); + /// ``` + pub fn ba(&self) -> &[T; 5] { + &self.ba + } + + /// Mutable reference to the filter coefficients. + /// + /// See [`Biquad::ba()`]. + /// + /// ``` + /// # use idsp::FilterNum; + /// # use idsp::iir::*; + /// let mut i = Biquad::default(); + /// i.ba_mut()[0] = i32::ONE; + /// assert_eq!(i, Biquad::IDENTITY); + /// ``` + pub fn ba_mut(&mut self) -> &mut [T; 5] { + &mut self.ba + } + + /// Summing junction offset + /// + /// This offset is applied to the output `y0` summing junction + /// on top of the feed-forward (`b`) and feed-back (`a`) terms. + /// The feedback samples are taken at the summing junction and + /// thus also include (and feed back) this offset. + pub fn u(&self) -> T { + self.u + } + + /// Set the summing junction offset + /// + /// See [`Biquad::u()`]. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::default(); + /// i.set_u(5); + /// assert_eq!(i.update(&mut [0; 5], 0), 5); + /// ``` + pub fn set_u(&mut self, u: T) { + self.u = u; + } + + /// Lower output limit + /// + /// Guaranteed minimum output value. + /// The value is inclusive. + /// The clamping also cleanly affects the feedback terms. + /// + /// Note: For fixed point filters `Biquad`, `T::MIN` should not be passed + /// to `min()` since the `y` samples stored in + /// the filter state are negated. Instead use `-T::MAX` as the lowest + /// possible limit. + /// + /// ``` + /// # use idsp::iir::*; + /// assert_eq!(Biquad::::default().min(), -i32::MAX); + /// ``` + pub fn min(&self) -> T { + self.min + } + + /// Set the lower output limit + /// + /// See [`Biquad::min()`]. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::default(); + /// i.set_min(7); + /// assert_eq!(i.update(&mut [0; 5], 0), 7); + /// ``` + pub fn set_min(&mut self, min: T) { + self.min = min; + } + + /// Upper output limit + /// + /// Guaranteed maximum output value. + /// The value is inclusive. + /// The clamping also cleanly affects the feedback terms. + /// + /// ``` + /// # use idsp::iir::*; + /// assert_eq!(Biquad::::default().max(), i32::MAX); + /// ``` + pub fn max(&self) -> T { + self.max + } + + /// Set the upper output limit + /// + /// See [`Biquad::max()`]. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::default(); + /// i.set_max(-7); + /// assert_eq!(i.update(&mut [0; 5], 0), -7); + /// ``` + pub fn set_max(&mut self, max: T) { + self.max = max; + } + + /// Compute the overall (DC/proportional feed-forward) gain. + /// + /// ``` + /// # use idsp::iir::*; + /// assert_eq!(Biquad::proportional(3.0).forward_gain(), 3.0); + /// ``` + /// + /// # Returns + /// The sum of the `b` feed-forward coefficients. + pub fn forward_gain(&self) -> T { + self.ba.iter().take(3).copied().sum() + } + + /// Compute input-referred (`x`) offset. + /// + /// ``` + /// # use idsp::FilterNum; + /// # use idsp::iir::*; + /// let mut i = Biquad::proportional(3); + /// i.set_u(3); + /// assert_eq!(i.input_offset(), i32::ONE); + /// ``` + pub fn input_offset(&self) -> T { + self.u.div(self.forward_gain()) + } + + /// Convert input (`x`) offset to equivalent summing junction offset (`u`) and apply. + /// + /// In the case of a "PID" controller the response behavior of the controller + /// to the offset is "stabilizing", and not "tracking": its frequency response + /// is exclusively according to the lowest non-zero [`Action`] gain. + /// There is no high order ("faster") response as would be the case for a "tracking" + /// controller. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::proportional(3.0); + /// i.set_input_offset(2.0); + /// let x0 = 0.5; + /// let y0 = i.update(&mut [0.0; 5], x0); + /// assert_eq!(y0, (x0 + i.input_offset()) * i.forward_gain()); + /// ``` + /// + /// ``` + /// # use idsp::FilterNum; + /// # use idsp::iir::*; + /// let mut i = Biquad::proportional(-i32::ONE); + /// i.set_input_offset(1); + /// assert_eq!(i.u(), -1); + /// ``` + /// + /// # Arguments + /// * `offset`: Input (`x`) offset. + pub fn set_input_offset(&mut self, offset: T) { + self.u = offset.mul(self.forward_gain()); + } + + /// Direct Form 1 Update + /// + /// Ingest a new input value into the filter, update the filter state, and + /// return the new output. Only the state `xy` is modified. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut xy = core::array::from_fn(|i| i as _); + /// let x0 = 3.0; + /// let y0 = Biquad::IDENTITY.update(&mut xy, x0); + /// assert_eq!(y0, x0); + /// assert_eq!(xy, [x0, 0.0, -y0, 2.0, 3.0]); + /// ``` + /// + /// # Arguments + /// * `xy` - Current filter state. + /// On entry: `[x1, x2, -y1, -y2, -y3]` + /// On exit: `[x0, x1, -y0, -y1, -y2]` + /// * `x0` - New input. + /// + /// # Returns + /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` + /// + /// # Panics + /// Panics in debug mode if `!(self.min <= self.max)`. + pub fn update(&self, xy: &mut [T; 5], x0: T) -> T { + // `xy` contains x0 x1 -y0 -y1 -y2 + // Increment time x1 x2 -y1 -y2 -y3 + // Shift x1 x1 x2 -y1 -y2 + xy.copy_within(0..4, 1); + // Store x0 x0 x1 x2 -y1 -y2 + xy[0] = x0; + // Compute y0 + let y0 = self + .u + .macc(xy.iter().copied().zip(self.ba.iter().copied())) + .clamp(self.min, self.max); + // Store -y0 x0 x1 -y0 -y1 -y2 + xy[2] = -y0; + y0 + } + + /// Direct Form 1 Update + /// + /// Ingest a new input value into the filter, update the filter state, and + /// return the new output. Only the state `xy` is modified. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut xy = core::array::from_fn(|i| i as _); + /// let x0 = 3.0; + /// let y0 = Biquad::IDENTITY.update_df1(&mut xy, x0); + /// assert_eq!(y0, x0); + /// assert_eq!(xy, [x0, 0.0, -y0, 2.0]); + /// ``` + /// + /// # Arguments + /// * `xy` - Current filter state. + /// On entry: `[x1, x2, -y1, -y2]` + /// On exit: `[x0, x1, -y0, -y1]` + /// * `x0` - New input. + /// + /// # Returns + /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` + /// + /// # Panics + /// Panics in debug mode if `!(self.min <= self.max)`. + pub fn update_df1(&self, xy: &mut [T; 4], x0: T) -> T { + // `xy` contains x0 x1 -y0 -y1 + // Increment time x1 x2 -y1 -y2 + // Compute y0 + let y0 = self + .u + .macc( + core::iter::once(x0) + .chain(xy.iter().copied()) + .zip(self.ba.iter().copied()), + ) + .clamp(self.min, self.max); + // Shift x1 x1 -y1 -y2 + xy[1] = xy[0]; + // Store x0 x0 x1 -y1 -y2 + xy[0] = x0; + // Shift x0 x1 -y0 -y1 + xy[3] = xy[2]; + // Store -y0 x0 x1 -y0 -y1 + xy[2] = -y0; + y0 + } + + /// Ingest new input and perform a Direct Form 2 Transposed update. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut xy = core::array::from_fn(|i| i as _); + /// let x0 = 3.0; + /// let y0 = Biquad::IDENTITY.update_df2t(&mut xy, x0); + /// assert_eq!(y0, x0); + /// assert_eq!(xy, [1.0, 0.0]); + /// ``` + /// + /// # Arguments + /// * `s` - Current filter state. + /// On entry: `[b1*x1 + b2*x2 - a1*y1 - a2*y2, b2*x1 - a2*y1]` + /// On exit: `[b1*x0 + b2*x1 - a1*y0 - a2*y1, b2*x0 - a2*y0]` + /// * `x0` - New input. + /// + /// # Returns + /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` + /// + /// # Panics + /// Panics in debug mode if `!(self.min <= self.max)`. + pub fn update_df2t(&self, u: &mut [T; 2], x0: T) -> T { + let y0 = (u[0] + self.ba[0].mul(x0)).clamp(self.min, self.max); + u[0] = u[1] + self.ba[1].mul(x0) - self.ba[3].mul(y0); + u[1] = self.u + self.ba[2].mul(x0) - self.ba[4].mul(y0); + y0 + } +} diff --git a/src/iir/coefficients.rs b/src/iir/coefficients.rs new file mode 100644 index 0000000..44a7c35 --- /dev/null +++ b/src/iir/coefficients.rs @@ -0,0 +1,348 @@ +use num_traits::{AsPrimitive, Float, FloatConst}; +use serde::{Deserialize, Serialize}; + +#[derive(Copy, Clone, Debug, PartialEq, PartialOrd, Serialize, Deserialize)] +enum Shape { + InverseQ(T), + Bandwidth(T), + Slope(T), +} + +impl Default for Shape { + fn default() -> Self { + Self::InverseQ(T::SQRT_2()) + } +} + +/// Standard audio biquad filter builder +/// +/// +#[derive(Copy, Clone, Debug, PartialEq, PartialOrd, Serialize, Deserialize)] +pub struct Filter { + /// Angular critical frequency (in units of sampling frequency) + /// Corner frequency, or 3dB cutoff frequency, + w0: T, + /// Passband gain + gain: T, + /// Shelf gain (only for peaking, lowshelf, highshelf) + /// Relative to passband gain + shelf: T, + /// Inverse Q + shape: Shape, +} + +impl Default for Filter { + fn default() -> Self { + Self { + w0: T::zero(), + gain: T::one(), + shape: Shape::default(), + shelf: T::one(), + } + } +} + +impl Filter +where + T: 'static + Float + FloatConst, + f32: AsPrimitive, +{ + pub fn frequency(self, critical_frequency: T, sample_frequency: T) -> Self { + self.critical_frequency(critical_frequency / sample_frequency) + } + + pub fn critical_frequency(self, critical_frequency: T) -> Self { + self.angular_critical_frequency(T::TAU() * critical_frequency) + } + + pub fn angular_critical_frequency(mut self, w0: T) -> Self { + self.w0 = w0; + self + } + + pub fn gain(mut self, k: T) -> Self { + self.gain = k; + self + } + + pub fn gain_db(self, k_db: T) -> Self { + self.gain(10.0.as_().powf(k_db / 20.0.as_())) + } + + pub fn shelf(mut self, a: T) -> Self { + self.shelf = a; + self + } + + pub fn shelf_db(self, k_db: T) -> Self { + self.gain(10.0.as_().powf(k_db / 20.0.as_())) + } + + pub fn inverse_q(mut self, qi: T) -> Self { + self.shape = Shape::InverseQ(qi); + self + } + + pub fn q(self, q: T) -> Self { + self.inverse_q(T::one() / q) + } + + /// Set [`FilterBuilder::frequency()`] first. + /// In octaves. + pub fn bandwidth(mut self, bw: T) -> Self { + self.shape = Shape::Bandwidth(bw); + self + } + + /// Set [`FilterBuilder::gain()`] first. + pub fn shelf_slope(mut self, s: T) -> Self { + self.shape = Shape::Slope(s); + self + } + + fn qi(&self) -> T { + match self.shape { + Shape::InverseQ(qi) => qi, + Shape::Bandwidth(bw) => { + 2.0.as_() * (T::LN_2() / 2.0.as_() * bw * self.w0 / self.w0.sin()).sinh() + } + Shape::Slope(s) => { + ((self.gain + T::one() / self.gain) * (T::one() / s - T::one()) + 2.0.as_()).sqrt() + } + } + } + + fn alpha(&self) -> T { + 0.5.as_() * self.w0.sin() * self.qi() + } + + /// Lowpass biquad filter. + /// + /// ``` + /// use idsp::iir::*; + /// let ba = Filter::default().critical_frequency(0.1).lowpass(); + /// println!("{ba:?}"); + /// ``` + pub fn lowpass(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + let b = self.gain * 0.5.as_() * (T::one() - fcos); + [ + b, + b + b, + b, + T::one() + alpha, + -(fcos + fcos), + T::one() - alpha, + ] + } + + pub fn highpass(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + let b = self.gain * 0.5.as_() * (T::one() + fcos); + [ + b, + -(b + b), + b, + T::one() + alpha, + -(fcos + fcos), + T::one() - alpha, + ] + } + + pub fn bandpass(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + let b = self.gain * alpha; + [ + b, + T::zero(), + b, + T::one() + alpha, + -(fcos + fcos), + T::one() - alpha, + ] + } + + pub fn notch(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + [ + self.gain, + -(fcos + fcos) * self.gain, + self.gain, + T::one() + alpha, + -(fcos + fcos), + T::one() - alpha, + ] + } + + pub fn allpass(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + [ + (T::one() - alpha) * self.gain, + -(fcos + fcos) * self.gain, + (T::one() + alpha) * self.gain, + T::one() + alpha, + -(fcos + fcos), + T::one() - alpha, + ] + } + + pub fn peaking(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + [ + (T::one() + alpha * self.shelf) * self.gain, + -(fcos + fcos) * self.gain, + (T::one() - alpha * self.shelf) * self.gain, + T::one() + alpha / self.shelf, + -(fcos + fcos), + T::one() - alpha / self.shelf, + ] + } + + pub fn lowshelf(self) -> [T; 6] { + let fcos = self.w0.cos(); + let sp1 = self.shelf + T::one(); + let sm1 = self.shelf - T::one(); + let tsa = 2.0.as_() * self.shelf.sqrt() * self.alpha(); + [ + self.shelf * self.gain * (sp1 - sm1 * fcos + tsa), + 2.0.as_() * self.shelf * self.gain * (sm1 - sp1 * fcos), + self.shelf * self.gain * (sp1 - sm1 * fcos - tsa), + sp1 + sm1 * fcos + tsa, + (-2.0).as_() * (sm1 + sp1 * fcos), + sp1 + sm1 * fcos - tsa, + ] + } + + pub fn highshelf(self) -> [T; 6] { + let fcos = self.w0.cos(); + let sp1 = self.shelf + T::one(); + let sm1 = self.shelf - T::one(); + let tsa = 2.0.as_() * self.shelf.sqrt() * self.alpha(); + [ + self.shelf * self.gain * (sp1 + sm1 * fcos + tsa), + (-2.0).as_() * self.shelf * self.gain * (sm1 + sp1 * fcos), + self.shelf * self.gain * (sp1 + sm1 * fcos - tsa), + sp1 - sm1 * fcos + tsa, + 2.0.as_() * (sm1 - sp1 * fcos), + sp1 - sm1 * fcos - tsa, + ] + } + + // TODO + // PI-notch + // + // SOS cascades: + // butterworth + // elliptic + // chebychev1/2 + // bessel +} + +#[cfg(test)] +mod test { + use super::*; + + use core::f64; + use num_complex::Complex64; + + use crate::iir::*; + + #[test] + fn lowpass_gen() { + let ba = Biquad::::from( + Filter::default() + .critical_frequency(2e-9f64) + .gain(2e7) + .lowpass(), + ); + println!("{:?}", ba); + } + + fn polyval(p: &[f64], x: Complex64) -> Complex64 { + p.iter() + .fold( + (Complex64::default(), Complex64::new(1.0, 0.0)), + |(a, xi), pi| (a + xi * *pi, xi * x), + ) + .0 + } + + fn freqz(b: &[f64], a: &[f64], f: f64) -> Complex64 { + let z = Complex64::new(0.0, -f64::consts::TAU * f).exp(); + polyval(b, z) / polyval(a, z) + } + + #[derive(Copy, Clone, Debug, PartialEq, PartialOrd)] + enum Tol { + GainDb(f64, f64), + GainBelowDb(f64), + GainAboveDb(f64), + } + impl Tol { + fn check(&self, h: Complex64) -> bool { + let g = 10.0 * h.norm_sqr().log10(); + match self { + Self::GainDb(want, tol) => (g - want).abs() <= *tol, + Self::GainAboveDb(want) => g >= *want, + Self::GainBelowDb(want) => g <= *want, + } + } + } + + fn check(f: f64, g: Tol, ba: &[f64; 6]) { + let h = freqz(&ba[..3], &ba[3..], f); + let hp = h.to_polar(); + assert!( + g.check(h), + "freq {f}: response {h}={hp:?} does not meet {g:?}" + ); + } + + #[test] + fn lowpass() { + let ba = Filter::default() + .critical_frequency(0.01) + .gain_db(20.0) + .lowpass(); + println!("{ba:?}"); + + let bai = Biquad::::from(ba).into(); + println!("{bai:?}"); + + for (f, g) in [ + (1e-3, Tol::GainDb(20.0, 0.01)), + (0.01, Tol::GainDb(17.0, 0.1)), + (4e-1, Tol::GainBelowDb(-40.0)), + ] { + check(f, g, &ba); + check(f, g, &bai); + } + } + + #[test] + fn highpass() { + let ba = Filter::default() + .critical_frequency(0.1) + .gain_db(-2.0) + .highpass(); + println!("{ba:?}"); + + let bai = Biquad::::from(ba).into(); + println!("{bai:?}"); + + for (f, g) in [ + (1e-3, Tol::GainBelowDb(-40.0)), + (0.1, Tol::GainDb(-5.0, 0.1)), + (4e-1, Tol::GainDb(-2.0, 0.01)), + ] { + check(f, g, &ba); + check(f, g, &bai); + } + } +} diff --git a/src/iir/mod.rs b/src/iir/mod.rs new file mode 100644 index 0000000..20713fb --- /dev/null +++ b/src/iir/mod.rs @@ -0,0 +1,6 @@ +mod biquad; +pub use biquad::*; +mod coefficients; +pub use coefficients::*; +mod pid; +pub use pid::*; diff --git a/src/iir/pid.rs b/src/iir/pid.rs new file mode 100644 index 0000000..4ccd1a8 --- /dev/null +++ b/src/iir/pid.rs @@ -0,0 +1,279 @@ +use core::iter::Sum; + +use num_traits::{AsPrimitive, Float}; +use serde::{Deserialize, Serialize}; + +use crate::FilterNum; + +/// PID controller builder +/// +/// Builds `Biquad` from action gains, gain limits, input offset and output limits. +/// +/// ``` +/// # use idsp::iir::*; +/// let b: Biquad = Pid::default() +/// .period(1e-3) +/// .gain(Action::Ki, 1e-3) +/// .gain(Action::Kp, 1.0) +/// .gain(Action::Kd, 1e2) +/// .limit(Action::Ki, 1e3) +/// .limit(Action::Kd, 1e1) +/// .build() +/// .unwrap() +/// .into(); +/// ``` +#[derive(Debug, Clone, PartialEq, PartialOrd, Serialize, Deserialize)] +pub struct Pid { + period: T, + gains: [T; 5], + limits: [T; 5], +} + +impl Default for Pid { + fn default() -> Self { + Self { + period: T::one(), + gains: [T::zero(); 5], + limits: [T::infinity(); 5], + } + } +} + +/// [`Pid::build()`] errors +#[derive(Copy, Clone, Debug, PartialEq, Eq, Ord, PartialOrd, Serialize, Deserialize)] +#[non_exhaustive] +pub enum PidError { + /// The action gains cover more than three successive orders + OrderRange, +} + +/// PID action +/// +/// This enumerates the five possible PID style actions of a [`Biquad`] +#[derive(Copy, Clone, Debug, PartialEq, Eq, Ord, PartialOrd, Serialize, Deserialize)] +pub enum Action { + /// Double integrating, -40 dB per decade + Kii = 0, + /// Integrating, -20 dB per decade + Ki = 1, + /// Proportional + Kp = 2, + /// Derivative=, 20 dB per decade + Kd = 3, + /// Double derivative, 40 dB per decade + Kdd = 4, +} + +impl> Pid { + /// Sample period + /// + /// # Arguments + /// * `period`: Sample period in some units, e.g. SI seconds + pub fn period(mut self, period: T) -> Self { + self.period = period; + self + } + + /// Gain for a given action + /// + /// Gain units are `output/input * time.powi(order)` where + /// * `output` are output (`y`) units + /// * `input` are input (`x`) units + /// * `time` are sample period units, e.g. SI seconds + /// * `order` is the action order: the frequency exponent + /// (`-1` for integrating, `0` for proportional, etc.) + /// + /// Note that inverse time units correspond to angular frequency units. + /// Gains are accurate in the low frequency limit. Towards Nyquist, the + /// frequency response is warped. + /// + /// ``` + /// # use idsp::iir::*; + /// let tau = 1e-3; + /// let ki = 1e-4; + /// let i: Biquad = Pid::default() + /// .period(tau) + /// .gain(Action::Ki, ki) + /// .build() + /// .unwrap() + /// .into(); + /// let x0 = 5.0; + /// let y0 = i.update(&mut [0.0; 5], x0); + /// assert!((y0 / (x0 * ki / tau) - 1.0).abs() < 2.0 * f32::EPSILON); + /// ``` + /// + /// # Arguments + /// * `action`: Action to control + /// * `gain`: Gain value + pub fn gain(mut self, action: Action, gain: T) -> Self { + self.gains[action as usize] = gain; + self + } + + /// Gain limit for a given action + /// + /// Gain limit units are `output/input`. See also [`Pid::gain()`]. + /// Multiple gains and limits may interact and lead to peaking. + /// + /// ``` + /// # use idsp::iir::*; + /// let ki_limit = 1e3; + /// let i: Biquad = Pid::default() + /// .gain(Action::Ki, 8.0) + /// .limit(Action::Ki, ki_limit) + /// .build() + /// .unwrap() + /// .into(); + /// let mut xy = [0.0; 5]; + /// let x0 = 5.0; + /// for _ in 0..1000 { + /// i.update(&mut xy, x0); + /// } + /// let y0 = i.update(&mut xy, x0); + /// assert!((y0 / (x0 * ki_limit) - 1.0f32).abs() < 1e-3); + /// ``` + /// + /// # Arguments + /// * `action`: Action to limit in gain + /// * `limit`: Gain limit + pub fn limit(mut self, action: Action, limit: T) -> Self { + self.limits[action as usize] = limit; + self + } + + /// Perform checks, compute coefficients and return `Biquad`. + /// + /// No attempt is made to detect NaNs, non-finite gains, non-positive period, + /// zero gain limits, or gain/limit sign mismatches. + /// These will consequently result in NaNs/infinities, peaking, or notches in + /// the Biquad coefficients. + /// + /// Gain limits for zero gain actions or for proportional action are ignored. + /// + /// ``` + /// # use idsp::iir::*; + /// let i: Biquad = Pid::default().gain(Action::Kp, 3.0).build().unwrap().into(); + /// assert_eq!(i, Biquad::proportional(3.0)); + /// ``` + pub fn build>(self) -> Result<[C; 5], PidError> + where + T: AsPrimitive, + { + const KP: usize = Action::Kp as usize; + + // Determine highest denominator (feedback, `a`) order + let low = self + .gains + .iter() + .take(KP) + .position(|g| !g.is_zero()) + .unwrap_or(KP); + + if self.gains.iter().skip(low + 3).any(|g| !g.is_zero()) { + return Err(PidError::OrderRange); + } + + // Derivative/integration kernels + let kernels = [ + [C::ONE, C::ZERO, C::ZERO], + [C::ONE, C::NEG_ONE, C::ZERO], + [C::ONE, C::NEG_ONE + C::NEG_ONE, C::ONE], + ]; + + // Scale gains, compute limits, quantize + let mut zi = self.period.powi(low as i32 - KP as i32); + let mut gl = [[T::zero(); 2]; 3]; + for (gli, (i, (ggi, lli))) in gl.iter_mut().zip( + self.gains + .iter() + .zip(self.limits.iter()) + .enumerate() + .skip(low), + ) { + gli[0] = *ggi * zi; + gli[1] = if i == KP { T::one() } else { gli[0] / *lli }; + zi = zi * self.period; + } + let a0i = T::one() / gl.iter().map(|gli| gli[1]).sum(); + + // Coefficients + let mut ba = [[C::ZERO; 2]; 3]; + for (gli, ki) in gl.iter().zip(kernels.iter()) { + let (g, l) = (C::quantize(gli[0] * a0i), C::quantize(gli[1] * a0i)); + for (j, baj) in ba.iter_mut().enumerate() { + *baj = [baj[0] + ki[j].mul(g), baj[1] + ki[j].mul(l)]; + } + } + + Ok([ba[0][0], ba[1][0], ba[2][0], ba[1][1], ba[2][1]]) + } +} + +#[cfg(test)] +mod test { + use crate::iir::*; + + #[test] + fn pid() { + let b: Biquad = Pid::default() + .period(1.0) + .gain(Action::Ki, 1e-3) + .gain(Action::Kp, 1.0) + .gain(Action::Kd, 1e2) + .limit(Action::Ki, 1e3) + .limit(Action::Kd, 1e1) + .build() + .unwrap() + .into(); + let want = [ + 9.18190826, + -18.27272561, + 9.09090826, + -1.90909074, + 0.90909083, + ]; + for (ba_have, ba_want) in b.ba().iter().zip(want.iter()) { + assert!( + (ba_have / ba_want - 1.0).abs() < 2.0 * f32::EPSILON, + "have {:?} != want {want:?}", + b.ba(), + ); + } + } + + #[test] + fn pid_i32() { + let b: Biquad = Pid::default() + .period(1.0) + .gain(Action::Ki, 1e-5) + .gain(Action::Kp, 1e-2) + .gain(Action::Kd, 1e0) + .limit(Action::Ki, 1e1) + .limit(Action::Kd, 1e-1) + .build() + .unwrap() + .into(); + println!("{b:?}"); + } + + #[test] + fn units() { + let ki = 5e-2; + let tau = 3e-3; + let b: Biquad = Pid::default() + .period(tau) + .gain(Action::Ki, ki) + .build() + .unwrap() + .into(); + let mut xy = [0.0; 5]; + for i in 1..10 { + let y_have = b.update(&mut xy, 1.0); + let y_want = (i as f32) * (ki / tau); + assert!( + (y_have / y_want - 1.0).abs() < 3.0 * f32::EPSILON, + "{i}: have {y_have} != {y_want}" + ); + } + } +} diff --git a/src/lib.rs b/src/lib.rs index aaf6a51..2d42319 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -24,6 +24,8 @@ pub use rpll::*; mod unwrap; pub use unwrap::*; pub mod hbf; +mod num; +pub use num::*; #[cfg(test)] pub mod testing; diff --git a/src/num.rs b/src/num.rs new file mode 100644 index 0000000..93a6d65 --- /dev/null +++ b/src/num.rs @@ -0,0 +1,115 @@ +use core::{ + iter::Sum, + ops::{Add, Neg, Sub}, +}; +use num_traits::{AsPrimitive, Float}; + +/// Helper trait unifying fixed point and floating point coefficients/samples +pub trait FilterNum: + Copy + + PartialEq + + Neg + + Sub + + Add + + Sum +where + // + AsPrimitive, + Self: 'static, +{ + /// Multiplicative identity + const ONE: Self; + /// Negative multiplicative identity, equal to `-Self::ONE`. + const NEG_ONE: Self; + /// Additive identity + const ZERO: Self; + /// Lowest value + const MIN: Self; + /// Highest value + const MAX: Self; + // type ACCU: AsPrimitive; + /// Multiply-accumulate `self + sum(x*a)` + /// + /// Proper scaling and potentially using a wide accumulator. + fn macc(self, xa: impl Iterator) -> Self; + /// Multiplication (scaled) + fn mul(self, other: Self) -> Self; + /// Division (scaled) + fn div(self, other: Self) -> Self; + /// Clamp `self` such that `min <= self <= max`. + /// + /// # Panic + /// May panics in debug mode if `max < min`. + fn clamp(self, min: Self, max: Self) -> Self; + /// Scale and quantize a floating point value. + fn quantize(value: C) -> Self + where + Self: AsPrimitive, + C: Float + AsPrimitive; +} + +macro_rules! impl_float { + ($T:ty) => { + impl FilterNum for $T { + const ONE: Self = 1.0; + const NEG_ONE: Self = -Self::ONE; + const ZERO: Self = 0.0; + const MIN: Self = Self::NEG_INFINITY; + const MAX: Self = Self::INFINITY; + fn macc(self, xa: impl Iterator) -> Self { + xa.fold(self, |y, (a, x)| a.mul_add(x, y)) + // xa.fold(self, |y, (a, x)| y + a * x) + } + fn clamp(self, min: Self, max: Self) -> Self { + <$T>::clamp(self, min, max) + } + fn div(self, other: Self) -> Self { + self / other + } + fn mul(self, other: Self) -> Self { + self * other + } + fn quantize>(value: C) -> Self { + value.as_() + } + } + }; +} +impl_float!(f32); +impl_float!(f64); + +macro_rules! impl_int { + ($T:ty, $A:ty, $Q:literal) => { + impl FilterNum for $T { + const ONE: Self = 1 << $Q; + const NEG_ONE: Self = -1 << $Q; + const ZERO: Self = 0; + // Need to avoid `$T::MIN*$T::MIN` overflow. + const MIN: Self = -<$T>::MAX; + const MAX: Self = <$T>::MAX; + fn macc(self, xa: impl Iterator) -> Self { + self + (xa.fold(1 << ($Q - 1), |y, (a, x)| y + a as $A * x as $A) >> $Q) as Self + } + fn clamp(self, min: Self, max: Self) -> Self { + Ord::clamp(self, min, max) + } + fn div(self, other: Self) -> Self { + (((self as $A) << $Q) / other as $A) as Self + } + fn mul(self, other: Self) -> Self { + (((1 << ($Q - 1)) + self as $A * other as $A) >> $Q) as Self + } + fn quantize(value: C) -> Self + where + Self: AsPrimitive, + C: Float + AsPrimitive, + { + (value * (1 << $Q).as_()).round().as_() + } + } + }; +} +// Q2.X chosen to be able to exactly and inclusively represent -2 as `-1 << X + 1` +impl_int!(i8, i16, 6); +impl_int!(i16, i32, 14); +impl_int!(i32, i64, 30); +impl_int!(i64, i128, 62); From 478e1c664cb1a6fdb5ed97b95a5339231311a7ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 9 Jan 2024 16:35:08 +0100 Subject: [PATCH 08/26] make df1 state just four samples --- CHANGELOG.md | 3 +- benches/micro.rs | 6 +-- src/iir/biquad.rs | 103 ++++++++++------------------------------ src/iir/coefficients.rs | 68 +++++++++++++------------- src/iir/pid.rs | 10 ++-- src/num.rs | 36 +++++++++----- 6 files changed, 94 insertions(+), 132 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0f0d90f..1fb5129 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,7 +11,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 * `hbf` FIRs, symmetric FIRs, half band filters, HBF decimators and interpolators * `iir::Pid`, `iir:Filter` a builder for PID coefficients and the collection of standard Biquad filters -* `iir::Biquad::{HOLD, proportional, identity}` +* `iir::Biquad::{HOLD, IDENTITY, proportional}` +* `iir::Biquad` getter/setter * `iir`: support for other integers (i8, i16, i128) * `iir::Biquad`: support for reduced DF1 state and DF2T state diff --git a/benches/micro.rs b/benches/micro.rs index 2869ba0..74a5b7f 100644 --- a/benches/micro.rs +++ b/benches/micro.rs @@ -54,7 +54,7 @@ fn pll_bench() { fn iir_int_bench() { let dut = iir::Biquad::default(); - let mut xy = [0; 5]; + let mut xy = [0; 4]; println!( "int_iir::IIR::update(s, x): {}", bench_env(0x2832, |x| dut.update(&mut xy, *x)) @@ -63,7 +63,7 @@ fn iir_int_bench() { fn iir_f32_bench() { let dut = iir::Biquad::::default(); - let mut xy = [0.0; 5]; + let mut xy = [0.0; 4]; println!( "int::IIR::::update(s, x): {}", bench_env(0.32241, |x| dut.update(&mut xy, *x)) @@ -72,7 +72,7 @@ fn iir_f32_bench() { fn iir_f64_bench() { let dut = iir::Biquad::::default(); - let mut xy = [0.0; 5]; + let mut xy = [0.0; 4]; println!( "int::IIR::::update(s, x): {}", bench_env(0.32241, |x| dut.update(&mut xy, *x)) diff --git a/src/iir/biquad.rs b/src/iir/biquad.rs index d433f5b..eaa48b7 100644 --- a/src/iir/biquad.rs +++ b/src/iir/biquad.rs @@ -25,16 +25,16 @@ use crate::FilterNum; /// /// # Coefficients and state /// -/// `[T; 5]` is both the IIR state and coefficients type. +/// `[T; 5]` is the coefficients type. /// /// To represent the IIR state (input and output memory) during [`Biquad::update()`] -/// this contains the three inputs `[x0, x1, x2]` and the two outputs `[y1, y2]` +/// this contains the two previous inputs and output `[x1, x2, y1, y2]` /// concatenated. Lower indices correspond to more recent samples. /// To represent the IIR coefficients, this contains the feed-forward /// coefficients `[b0, b1, b2]` followd by the negated feed-back coefficients /// `[a1, a2]`, all five normalized such that `a0 = 1`. /// Note that between filter [`Biquad::update()`] the `xy` state contains -/// `[x0, x1, y0, y1, y2]`. +/// `[x0, x1, y0, y1]`. /// /// The IIR coefficients can be mapped to other transfer function /// representations, for example as described in @@ -148,8 +148,8 @@ impl Biquad { /// let mut xy = core::array::from_fn(|i| i as _); /// let x0 = 7.0; /// let y0 = Biquad::HOLD.update(&mut xy, x0); - /// assert_eq!(y0, -2.0); - /// assert_eq!(xy, [x0, 0.0, -y0, -y0, 3.0]); + /// assert_eq!(y0, 2.0); + /// assert_eq!(xy, [x0, 0.0, y0, y0]); /// ``` pub const HOLD: Self = Self { ba: [T::ZERO, T::ZERO, T::ZERO, T::NEG_ONE, T::ZERO], @@ -163,7 +163,7 @@ impl Biquad { /// ``` /// # use idsp::iir::*; /// let x0 = 3.0; - /// let y0 = Biquad::IDENTITY.update(&mut [0.0; 5], x0); + /// let y0 = Biquad::IDENTITY.update(&mut [0.0; 4], x0); /// assert_eq!(y0, x0); /// ``` pub const IDENTITY: Self = Self::proportional(T::ONE); @@ -174,7 +174,7 @@ impl Biquad { /// # use idsp::iir::*; /// let x0 = 2.0; /// let k = 5.0; - /// let y0 = Biquad::proportional(k).update(&mut [0.0; 5], x0); + /// let y0 = Biquad::proportional(k).update(&mut [0.0; 4], x0); /// assert_eq!(y0, x0 * k); /// ``` pub const fn proportional(k: T) -> Self { @@ -235,7 +235,7 @@ impl Biquad { /// # use idsp::iir::*; /// let mut i = Biquad::default(); /// i.set_u(5); - /// assert_eq!(i.update(&mut [0; 5], 0), 5); + /// assert_eq!(i.update(&mut [0; 4], 0), 5); /// ``` pub fn set_u(&mut self, u: T) { self.u = u; @@ -268,7 +268,7 @@ impl Biquad { /// # use idsp::iir::*; /// let mut i = Biquad::default(); /// i.set_min(7); - /// assert_eq!(i.update(&mut [0; 5], 0), 7); + /// assert_eq!(i.update(&mut [0; 4], 0), 7); /// ``` pub fn set_min(&mut self, min: T) { self.min = min; @@ -296,7 +296,7 @@ impl Biquad { /// # use idsp::iir::*; /// let mut i = Biquad::default(); /// i.set_max(-7); - /// assert_eq!(i.update(&mut [0; 5], 0), -7); + /// assert_eq!(i.update(&mut [0; 4], 0), -7); /// ``` pub fn set_max(&mut self, max: T) { self.max = max; @@ -332,7 +332,7 @@ impl Biquad { /// /// In the case of a "PID" controller the response behavior of the controller /// to the offset is "stabilizing", and not "tracking": its frequency response - /// is exclusively according to the lowest non-zero [`Action`] gain. + /// is exclusively according to the lowest non-zero [`crate::iir::Action`] gain. /// There is no high order ("faster") response as would be the case for a "tracking" /// controller. /// @@ -341,7 +341,7 @@ impl Biquad { /// let mut i = Biquad::proportional(3.0); /// i.set_input_offset(2.0); /// let x0 = 0.5; - /// let y0 = i.update(&mut [0.0; 5], x0); + /// let y0 = i.update(&mut [0.0; 4], x0); /// assert_eq!(y0, (x0 + i.input_offset()) * i.forward_gain()); /// ``` /// @@ -370,85 +370,37 @@ impl Biquad { /// let x0 = 3.0; /// let y0 = Biquad::IDENTITY.update(&mut xy, x0); /// assert_eq!(y0, x0); - /// assert_eq!(xy, [x0, 0.0, -y0, 2.0, 3.0]); + /// assert_eq!(xy, [x0, 0.0, y0, 2.0]); /// ``` /// /// # Arguments /// * `xy` - Current filter state. - /// On entry: `[x1, x2, -y1, -y2, -y3]` - /// On exit: `[x0, x1, -y0, -y1, -y2]` + /// On entry: `[x1, x2, y1, y2]` + /// On exit: `[x0, x1, y0, y1]` /// * `x0` - New input. /// /// # Returns - /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` - /// - /// # Panics - /// Panics in debug mode if `!(self.min <= self.max)`. - pub fn update(&self, xy: &mut [T; 5], x0: T) -> T { - // `xy` contains x0 x1 -y0 -y1 -y2 - // Increment time x1 x2 -y1 -y2 -y3 - // Shift x1 x1 x2 -y1 -y2 - xy.copy_within(0..4, 1); - // Store x0 x0 x1 x2 -y1 -y2 - xy[0] = x0; - // Compute y0 - let y0 = self - .u - .macc(xy.iter().copied().zip(self.ba.iter().copied())) - .clamp(self.min, self.max); - // Store -y0 x0 x1 -y0 -y1 -y2 - xy[2] = -y0; - y0 - } - - /// Direct Form 1 Update - /// - /// Ingest a new input value into the filter, update the filter state, and - /// return the new output. Only the state `xy` is modified. - /// - /// ``` - /// # use idsp::iir::*; - /// let mut xy = core::array::from_fn(|i| i as _); - /// let x0 = 3.0; - /// let y0 = Biquad::IDENTITY.update_df1(&mut xy, x0); - /// assert_eq!(y0, x0); - /// assert_eq!(xy, [x0, 0.0, -y0, 2.0]); - /// ``` - /// - /// # Arguments - /// * `xy` - Current filter state. - /// On entry: `[x1, x2, -y1, -y2]` - /// On exit: `[x0, x1, -y0, -y1]` - /// * `x0` - New input. - /// - /// # Returns - /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` - /// - /// # Panics - /// Panics in debug mode if `!(self.min <= self.max)`. + /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` pub fn update_df1(&self, xy: &mut [T; 4], x0: T) -> T { - // `xy` contains x0 x1 -y0 -y1 - // Increment time x1 x2 -y1 -y2 - // Compute y0 let y0 = self .u - .macc( - core::iter::once(x0) - .chain(xy.iter().copied()) - .zip(self.ba.iter().copied()), - ) + .macc([x0, xy[0], xy[1], -xy[2], -xy[3]].into_iter().zip(self.ba)) .clamp(self.min, self.max); - // Shift x1 x1 -y1 -y2 xy[1] = xy[0]; - // Store x0 x0 x1 -y1 -y2 xy[0] = x0; - // Shift x0 x1 -y0 -y1 xy[3] = xy[2]; - // Store -y0 x0 x1 -y0 -y1 - xy[2] = -y0; + xy[2] = y0; y0 } + /// Direct Form 1 update + /// + /// See [`Biquad::update_df1()`]. + #[inline] + pub fn update(&self, xy: &mut [T; 4], x0: T) -> T { + self.update_df1(xy, x0) + } + /// Ingest new input and perform a Direct Form 2 Transposed update. /// /// ``` @@ -468,9 +420,6 @@ impl Biquad { /// /// # Returns /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` - /// - /// # Panics - /// Panics in debug mode if `!(self.min <= self.max)`. pub fn update_df2t(&self, u: &mut [T; 2], x0: T) -> T { let y0 = (u[0] + self.ba[0].mul(x0)).clamp(self.min, self.max); u[0] = u[1] + self.ba[1].mul(x0) - self.ba[3].mul(y0); diff --git a/src/iir/coefficients.rs b/src/iir/coefficients.rs index 44a7c35..76574f2 100644 --- a/src/iir/coefficients.rs +++ b/src/iir/coefficients.rs @@ -87,14 +87,14 @@ where self.inverse_q(T::one() / q) } - /// Set [`FilterBuilder::frequency()`] first. + /// Set [`Filter::frequency()`] first. /// In octaves. pub fn bandwidth(mut self, bw: T) -> Self { self.shape = Shape::Bandwidth(bw); self } - /// Set [`FilterBuilder::gain()`] first. + /// Set [`Filter::gain()`] first. pub fn shelf_slope(mut self, s: T) -> Self { self.shape = Shape::Slope(s); self @@ -282,14 +282,12 @@ mod test { enum Tol { GainDb(f64, f64), GainBelowDb(f64), - GainAboveDb(f64), } impl Tol { fn check(&self, h: Complex64) -> bool { let g = 10.0 * h.norm_sqr().log10(); match self { Self::GainDb(want, tol) => (g - want).abs() <= *tol, - Self::GainAboveDb(want) => g >= *want, Self::GainBelowDb(want) => g <= *want, } } @@ -304,45 +302,45 @@ mod test { ); } - #[test] - fn lowpass() { - let ba = Filter::default() - .critical_frequency(0.01) - .gain_db(20.0) - .lowpass(); + fn check_coeffs(ba: &[f64; 6], fg: &[(f64, Tol)]) { println!("{ba:?}"); - let bai = Biquad::::from(ba).into(); + let bai = Biquad::::from(*ba).into(); println!("{bai:?}"); - for (f, g) in [ - (1e-3, Tol::GainDb(20.0, 0.01)), - (0.01, Tol::GainDb(17.0, 0.1)), - (4e-1, Tol::GainBelowDb(-40.0)), - ] { - check(f, g, &ba); - check(f, g, &bai); + for (f, g) in fg { + check(*f, *g, ba); + check(*f, *g, &bai); } } #[test] - fn highpass() { - let ba = Filter::default() - .critical_frequency(0.1) - .gain_db(-2.0) - .highpass(); - println!("{ba:?}"); - - let bai = Biquad::::from(ba).into(); - println!("{bai:?}"); + fn lowpass() { + check_coeffs( + &Filter::default() + .critical_frequency(0.01) + .gain_db(20.0) + .lowpass(), + &[ + (1e-3, Tol::GainDb(20.0, 0.01)), + (0.01, Tol::GainDb(17.0, 0.1)), + (4e-1, Tol::GainBelowDb(-40.0)), + ], + ); + } - for (f, g) in [ - (1e-3, Tol::GainBelowDb(-40.0)), - (0.1, Tol::GainDb(-5.0, 0.1)), - (4e-1, Tol::GainDb(-2.0, 0.01)), - ] { - check(f, g, &ba); - check(f, g, &bai); - } + #[test] + fn highpass() { + check_coeffs( + &Filter::default() + .critical_frequency(0.1) + .gain_db(-2.0) + .highpass(), + &[ + (1e-3, Tol::GainBelowDb(-40.0)), + (0.1, Tol::GainDb(-5.0, 0.1)), + (4e-1, Tol::GainDb(-2.0, 0.01)), + ], + ); } } diff --git a/src/iir/pid.rs b/src/iir/pid.rs index 4ccd1a8..e28f1a9 100644 --- a/src/iir/pid.rs +++ b/src/iir/pid.rs @@ -22,7 +22,7 @@ use crate::FilterNum; /// .unwrap() /// .into(); /// ``` -#[derive(Debug, Clone, PartialEq, PartialOrd, Serialize, Deserialize)] +#[derive(Debug, Clone, Copy, PartialEq, PartialOrd, Serialize, Deserialize)] pub struct Pid { period: T, gains: [T; 5], @@ -49,7 +49,7 @@ pub enum PidError { /// PID action /// -/// This enumerates the five possible PID style actions of a [`Biquad`] +/// This enumerates the five possible PID style actions of a [`crate::iir::Biquad`] #[derive(Copy, Clone, Debug, PartialEq, Eq, Ord, PartialOrd, Serialize, Deserialize)] pub enum Action { /// Double integrating, -40 dB per decade @@ -98,7 +98,7 @@ impl> Pid { /// .unwrap() /// .into(); /// let x0 = 5.0; - /// let y0 = i.update(&mut [0.0; 5], x0); + /// let y0 = i.update(&mut [0.0; 4], x0); /// assert!((y0 / (x0 * ki / tau) - 1.0).abs() < 2.0 * f32::EPSILON); /// ``` /// @@ -124,7 +124,7 @@ impl> Pid { /// .build() /// .unwrap() /// .into(); - /// let mut xy = [0.0; 5]; + /// let mut xy = [0.0; 4]; /// let x0 = 5.0; /// for _ in 0..1000 { /// i.update(&mut xy, x0); @@ -266,7 +266,7 @@ mod test { .build() .unwrap() .into(); - let mut xy = [0.0; 5]; + let mut xy = [0.0; 4]; for i in 1..10 { let y_have = b.update(&mut xy, 1.0); let y_want = (i as f32) * (ki / tau); diff --git a/src/num.rs b/src/num.rs index 93a6d65..91d2d71 100644 --- a/src/num.rs +++ b/src/num.rs @@ -13,8 +13,7 @@ pub trait FilterNum: + Add + Sum where - // + AsPrimitive, - Self: 'static, + Self: 'static + AsPrimitive, { /// Multiplicative identity const ONE: Self; @@ -26,7 +25,8 @@ where const MIN: Self; /// Highest value const MAX: Self; - // type ACCU: AsPrimitive; + /// Accumulator type + type ACCU: AsPrimitive; /// Multiply-accumulate `self + sum(x*a)` /// /// Proper scaling and potentially using a wide accumulator. @@ -36,9 +36,7 @@ where /// Division (scaled) fn div(self, other: Self) -> Self; /// Clamp `self` such that `min <= self <= max`. - /// - /// # Panic - /// May panics in debug mode if `max < min`. + /// Undefined result if `max < min`. fn clamp(self, min: Self, max: Self) -> Self; /// Scale and quantize a floating point value. fn quantize(value: C) -> Self @@ -55,12 +53,20 @@ macro_rules! impl_float { const ZERO: Self = 0.0; const MIN: Self = Self::NEG_INFINITY; const MAX: Self = Self::INFINITY; + type ACCU = $T; fn macc(self, xa: impl Iterator) -> Self { - xa.fold(self, |y, (a, x)| a.mul_add(x, y)) - // xa.fold(self, |y, (a, x)| y + a * x) + // a.mul_add(x, y) is std/libm only + xa.fold(self, |u, (a, x)| u + a * x) } fn clamp(self, min: Self, max: Self) -> Self { - <$T>::clamp(self, min, max) + // <$T>::clamp() is slow and checks + if self < min { + min + } else if self > max { + max + } else { + self + } } fn div(self, other: Self) -> Self { self / other @@ -86,11 +92,19 @@ macro_rules! impl_int { // Need to avoid `$T::MIN*$T::MIN` overflow. const MIN: Self = -<$T>::MAX; const MAX: Self = <$T>::MAX; + type ACCU = $A; fn macc(self, xa: impl Iterator) -> Self { - self + (xa.fold(1 << ($Q - 1), |y, (a, x)| y + a as $A * x as $A) >> $Q) as Self + self + (xa.fold(1 << ($Q - 1), |u, (a, x)| u + a as $A * x as $A) >> $Q) as Self } fn clamp(self, min: Self, max: Self) -> Self { - Ord::clamp(self, min, max) + // Ord::clamp() is slow and checks + if self < min { + min + } else if self > max { + max + } else { + self + } } fn div(self, other: Self) -> Self { (((self as $A) << $Q) / other as $A) as Self From 71a0944873df564ab706ebacb300f26c7b366c78 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 9 Jan 2024 17:23:05 +0100 Subject: [PATCH 09/26] fewer copying in From --- src/iir/biquad.rs | 8 +-- src/iir/coefficients.rs | 127 +++++++++++++++++++++++++++++----------- 2 files changed, 97 insertions(+), 38 deletions(-) diff --git a/src/iir/biquad.rs b/src/iir/biquad.rs index eaa48b7..9018961 100644 --- a/src/iir/biquad.rs +++ b/src/iir/biquad.rs @@ -104,12 +104,12 @@ impl From<[T; 5]> for Biquad { } } -impl From<[C; 6]> for Biquad +impl From<&[C; 6]> for Biquad where T: FilterNum + AsPrimitive, C: Float + AsPrimitive, { - fn from(ba: [C; 6]) -> Self { + fn from(ba: &[C; 6]) -> Self { let ia0 = C::one() / ba[3]; Self::from([ T::quantize(ba[0] * ia0), @@ -122,12 +122,12 @@ where } } -impl From> for [C; 6] +impl From<&Biquad> for [C; 6] where T: FilterNum + AsPrimitive, C: 'static + Copy, { - fn from(value: Biquad) -> Self { + fn from(value: &Biquad) -> Self { let ba = value.ba(); [ ba[0].as_(), diff --git a/src/iir/coefficients.rs b/src/iir/coefficients.rs index 76574f2..a04b9dc 100644 --- a/src/iir/coefficients.rs +++ b/src/iir/coefficients.rs @@ -3,7 +3,9 @@ use serde::{Deserialize, Serialize}; #[derive(Copy, Clone, Debug, PartialEq, PartialOrd, Serialize, Deserialize)] enum Shape { + /// Inverse W InverseQ(T), + /// Relative bandwidth in octaves Bandwidth(T), Slope(T), } @@ -75,7 +77,7 @@ where } pub fn shelf_db(self, k_db: T) -> Self { - self.gain(10.0.as_().powf(k_db / 20.0.as_())) + self.shelf(10.0.as_().powf(k_db / 20.0.as_())) } pub fn inverse_q(mut self, qi: T) -> Self { @@ -87,14 +89,13 @@ where self.inverse_q(T::one() / q) } - /// Set [`Filter::frequency()`] first. - /// In octaves. + /// Relative bandwidth in octaves. pub fn bandwidth(mut self, bw: T) -> Self { self.shape = Shape::Bandwidth(bw); self } - /// Set [`Filter::gain()`] first. + /// Shelf slope. pub fn shelf_slope(mut self, s: T) -> Self { self.shape = Shape::Slope(s); self @@ -112,20 +113,26 @@ where } } - fn alpha(&self) -> T { - 0.5.as_() * self.w0.sin() * self.qi() + fn fcos_alpha(&self) -> (T, T) { + let (fsin, fcos) = self.w0.sin_cos(); + (fcos, 0.5.as_() * fsin * self.qi()) } - /// Lowpass biquad filter. + /// Low pass filter + /// + /// Builds second order biquad low pass filter coefficients. /// /// ``` /// use idsp::iir::*; /// let ba = Filter::default().critical_frequency(0.1).lowpass(); - /// println!("{ba:?}"); + /// let iir = Biquad::::from(&ba); + /// let mut xy = [0; 4]; + /// let x = vec![3, -4, 5, 7, -3, 2]; + /// let y: Vec<_> = x.iter().map(|x0| iir.update(&mut xy, *x0)).collect(); + /// assert_eq!(y, [0, 0, 0, 1, 2, 2]); /// ``` pub fn lowpass(self) -> [T; 6] { - let fcos = self.w0.cos(); - let alpha = self.alpha(); + let (fcos, alpha) = self.fcos_alpha(); let b = self.gain * 0.5.as_() * (T::one() - fcos); [ b, @@ -137,9 +144,21 @@ where ] } + /// High pass filter + /// + /// Builds second order biquad high pass filter coefficients. + /// + /// ``` + /// use idsp::iir::*; + /// let ba = Filter::default().critical_frequency(0.1).highpass(); + /// let iir = Biquad::::from(&ba); + /// let mut xy = [0; 4]; + /// let x = vec![3, -4, 5, 7, -3, 2]; + /// let y: Vec<_> = x.iter().map(|x0| iir.update(&mut xy, *x0)).collect(); + /// assert_eq!(y, [2, -4, 5, 3, -6, 1]); + /// ``` pub fn highpass(self) -> [T; 6] { - let fcos = self.w0.cos(); - let alpha = self.alpha(); + let (fcos, alpha) = self.fcos_alpha(); let b = self.gain * 0.5.as_() * (T::one() + fcos); [ b, @@ -151,14 +170,24 @@ where ] } + /// Band pass + /// + /// ``` + /// use idsp::iir::*; + /// let ba = Filter::default() + /// .frequency(1000.0, 48e3) + /// .q(5.0) + /// .gain_db(3.0) + /// .bandpass(); + /// println!("{ba:?}"); + /// ``` pub fn bandpass(self) -> [T; 6] { - let fcos = self.w0.cos(); - let alpha = self.alpha(); + let (fcos, alpha) = self.fcos_alpha(); let b = self.gain * alpha; [ b, T::zero(), - b, + -b, T::one() + alpha, -(fcos + fcos), T::one() - alpha, @@ -166,8 +195,7 @@ where } pub fn notch(self) -> [T; 6] { - let fcos = self.w0.cos(); - let alpha = self.alpha(); + let (fcos, alpha) = self.fcos_alpha(); [ self.gain, -(fcos + fcos) * self.gain, @@ -179,8 +207,7 @@ where } pub fn allpass(self) -> [T; 6] { - let fcos = self.w0.cos(); - let alpha = self.alpha(); + let (fcos, alpha) = self.fcos_alpha(); [ (T::one() - alpha) * self.gain, -(fcos + fcos) * self.gain, @@ -192,8 +219,7 @@ where } pub fn peaking(self) -> [T; 6] { - let fcos = self.w0.cos(); - let alpha = self.alpha(); + let (fcos, alpha) = self.fcos_alpha(); [ (T::one() + alpha * self.shelf) * self.gain, -(fcos + fcos) * self.gain, @@ -204,11 +230,22 @@ where ] } + /// Low shelf + /// + /// ``` + /// use idsp::iir::*; + /// let ba = Filter::default() + /// .frequency(1000.0, 48e3) + /// .shelf_slope(2.0) + /// .shelf_db(20.0) + /// .lowshelf(); + /// println!("{ba:?}"); + /// ``` pub fn lowshelf(self) -> [T; 6] { - let fcos = self.w0.cos(); + let (fcos, alpha) = self.fcos_alpha(); + let tsa = 2.0.as_() * self.shelf.sqrt() * alpha; let sp1 = self.shelf + T::one(); let sm1 = self.shelf - T::one(); - let tsa = 2.0.as_() * self.shelf.sqrt() * self.alpha(); [ self.shelf * self.gain * (sp1 - sm1 * fcos + tsa), 2.0.as_() * self.shelf * self.gain * (sm1 - sp1 * fcos), @@ -220,10 +257,10 @@ where } pub fn highshelf(self) -> [T; 6] { - let fcos = self.w0.cos(); + let (fcos, alpha) = self.fcos_alpha(); + let tsa = 2.0.as_() * self.shelf.sqrt() * alpha; let sp1 = self.shelf + T::one(); let sm1 = self.shelf - T::one(); - let tsa = 2.0.as_() * self.shelf.sqrt() * self.alpha(); [ self.shelf * self.gain * (sp1 + sm1 * fcos + tsa), (-2.0).as_() * self.shelf * self.gain * (sm1 + sp1 * fcos), @@ -236,14 +273,15 @@ where // TODO // PI-notch - // - // SOS cascades: - // butterworth - // elliptic - // chebychev1/2 - // bessel } +// TODO +// SOS cascades: +// butterworth +// elliptic +// chebychev1/2 +// bessel + #[cfg(test)] mod test { use super::*; @@ -256,7 +294,7 @@ mod test { #[test] fn lowpass_gen() { let ba = Biquad::::from( - Filter::default() + &Filter::default() .critical_frequency(2e-9f64) .gain(2e7) .lowpass(), @@ -305,11 +343,15 @@ mod test { fn check_coeffs(ba: &[f64; 6], fg: &[(f64, Tol)]) { println!("{ba:?}"); - let bai = Biquad::::from(*ba).into(); + for (f, g) in fg { + check(*f, *g, ba); + } + + // Quantize + let bai = (&Biquad::::from(ba)).into(); println!("{bai:?}"); for (f, g) in fg { - check(*f, *g, ba); check(*f, *g, &bai); } } @@ -343,4 +385,21 @@ mod test { ], ); } + + #[test] + fn notch() { + check_coeffs( + &Filter::default() + .critical_frequency(0.02) + .bandwidth(2.0) + .notch(), + &[ + (1e-4, Tol::GainDb(0.0, 0.01)), + (0.01, Tol::GainDb(-3.0, 0.1)), + (0.02, Tol::GainBelowDb(-80.0)), + (0.04, Tol::GainDb(-3.0, 0.1)), + (4e-1, Tol::GainDb(0.0, 0.01)), + ], + ); + } } From b676ff5231f2de677a001cc2e9ba1d5a792154d5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 9 Jan 2024 22:21:13 +0100 Subject: [PATCH 10/26] iir: refine accu --- src/iir/biquad.rs | 10 +++++++--- src/num.rs | 13 +++++-------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/src/iir/biquad.rs b/src/iir/biquad.rs index 9018961..9b9c6bc 100644 --- a/src/iir/biquad.rs +++ b/src/iir/biquad.rs @@ -384,7 +384,11 @@ impl Biquad { pub fn update_df1(&self, xy: &mut [T; 4], x0: T) -> T { let y0 = self .u - .macc([x0, xy[0], xy[1], -xy[2], -xy[3]].into_iter().zip(self.ba)) + .macc( + [x0, xy[0], xy[1], -xy[2], -xy[3]] + .into_iter() + .zip(self.ba.iter().copied()), + ) .clamp(self.min, self.max); xy[1] = xy[0]; xy[0] = x0; @@ -422,8 +426,8 @@ impl Biquad { /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` pub fn update_df2t(&self, u: &mut [T; 2], x0: T) -> T { let y0 = (u[0] + self.ba[0].mul(x0)).clamp(self.min, self.max); - u[0] = u[1] + self.ba[1].mul(x0) - self.ba[3].mul(y0); - u[1] = self.u + self.ba[2].mul(x0) - self.ba[4].mul(y0); + u[0] = u[1] + self.ba[1].mul(x0) + self.ba[3].mul(-y0); + u[1] = self.u + self.ba[2].mul(x0) + self.ba[4].mul(-y0); y0 } } diff --git a/src/num.rs b/src/num.rs index 91d2d71..612a354 100644 --- a/src/num.rs +++ b/src/num.rs @@ -1,17 +1,12 @@ use core::{ iter::Sum, - ops::{Add, Neg, Sub}, + ops::{Add, Mul, Neg}, }; use num_traits::{AsPrimitive, Float}; /// Helper trait unifying fixed point and floating point coefficients/samples pub trait FilterNum: - Copy - + PartialEq - + Neg - + Sub - + Add - + Sum + Copy + PartialEq + Neg + Add + Sum where Self: 'static + AsPrimitive, { @@ -26,7 +21,9 @@ where /// Highest value const MAX: Self; /// Accumulator type - type ACCU: AsPrimitive; + type ACCU: AsPrimitive + + Add + + Mul; /// Multiply-accumulate `self + sum(x*a)` /// /// Proper scaling and potentially using a wide accumulator. From e8a4ea34c25c963dcdaab9224be15a6974f1038e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 10 Jan 2024 12:01:58 +0100 Subject: [PATCH 11/26] add comment on PLL --- README.md | 2 +- src/pll.rs | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 7663b9e..5a1c16a 100644 --- a/README.md +++ b/README.md @@ -30,7 +30,7 @@ An extension trait for the `num::Complex` type featuring especially a `std`-like ## PLL, RPLL -High accuracy, zero-assumption, fully robust, forward and reciprocal PLLs with dynamically adjustable time constant and arbitrary (in the Nyquist sampling sense) capture range. +High accuracy, zero-assumption, fully robust, forward and reciprocal PLLs with dynamically adjustable time constant and arbitrary (in the Nyquist sampling sense) capture range, and noise shaping. ## Unwrapper, Accu, saturating_scale diff --git a/src/pll.rs b/src/pll.rs index 01cdac6..9822501 100644 --- a/src/pll.rs +++ b/src/pll.rs @@ -33,6 +33,8 @@ use serde::{Deserialize, Serialize}; /// /// The extension to I^3,I^2,I behavior to track chirps phase-accurately or to i64 data to /// increase resolution for extremely narrowband applications is obvious. +/// +/// This PLL implements first order noise shaping to reduce quantization errors. #[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct PLL { // last input phase From 2e7bc0d2df45cc01696c9a006cac2c0fba6b5ff5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 10 Jan 2024 17:06:04 +0100 Subject: [PATCH 12/26] iir: optimize i32 and f32 cases --- src/iir/biquad.rs | 120 +++++++++++++++++++++++++--------------- src/iir/coefficients.rs | 14 +++-- src/num.rs | 107 +++++++++++++++++++++++++---------- src/pll.rs | 2 +- 4 files changed, 166 insertions(+), 77 deletions(-) diff --git a/src/iir/biquad.rs b/src/iir/biquad.rs index 9b9c6bc..fe521db 100644 --- a/src/iir/biquad.rs +++ b/src/iir/biquad.rs @@ -145,7 +145,7 @@ impl Biquad { /// /// ``` /// # use idsp::iir::*; - /// let mut xy = core::array::from_fn(|i| i as _); + /// let mut xy = [0.0, 1.0, 2.0, 3.0]; /// let x0 = 7.0; /// let y0 = Biquad::HOLD.update(&mut xy, x0); /// assert_eq!(y0, 2.0); @@ -364,70 +364,102 @@ impl Biquad { /// Ingest a new input value into the filter, update the filter state, and /// return the new output. Only the state `xy` is modified. /// + /// ## `N=4` Direct Form 1 + /// + /// `xy` contains: + /// * On entry: `[x1, x2, y1, y2]` + /// * On exit: `[x0, x1, y0, y1]` + /// /// ``` /// # use idsp::iir::*; - /// let mut xy = core::array::from_fn(|i| i as _); - /// let x0 = 3.0; + /// let mut xy = [0.0, 1.0, 2.0, 3.0]; + /// let x0 = 4.0; /// let y0 = Biquad::IDENTITY.update(&mut xy, x0); /// assert_eq!(y0, x0); /// assert_eq!(xy, [x0, 0.0, y0, 2.0]); /// ``` /// - /// # Arguments - /// * `xy` - Current filter state. - /// On entry: `[x1, x2, y1, y2]` - /// On exit: `[x0, x1, y0, y1]` - /// * `x0` - New input. + /// ## `N=5` Direct Form 1 with first order noise shaping /// - /// # Returns - /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` - pub fn update_df1(&self, xy: &mut [T; 4], x0: T) -> T { - let y0 = self - .u - .macc( - [x0, xy[0], xy[1], -xy[2], -xy[3]] - .into_iter() - .zip(self.ba.iter().copied()), - ) - .clamp(self.min, self.max); - xy[1] = xy[0]; - xy[0] = x0; - xy[3] = xy[2]; - xy[2] = y0; - y0 - } - - /// Direct Form 1 update + /// `xy` contains: + /// * On entry: `[x1, x2, y1, y2, e1]` + /// * On exit: `[x0, x1, y0, y1, e0]` /// - /// See [`Biquad::update_df1()`]. - #[inline] - pub fn update(&self, xy: &mut [T; 4], x0: T) -> T { - self.update_df1(xy, x0) - } - - /// Ingest new input and perform a Direct Form 2 Transposed update. + /// Note: This isn't useful for floating point. + /// + /// ## `N=2` Direct Form 2 transposed + /// + /// Note: Don't use this for fixed point. Quantization happens at each state store operation. + /// Ideally the state would be `T::ACCU` but then for fixed point it would use equal amount + /// of storage compared to DF1 for little to no gain in performance and none in functionality. + /// There are also no guard bits. + /// + /// `xy` contains: + /// * On entry: `[b1*x1 + b2*x2 - a1*y1 - a2*y2, b2*x1 - a2*y1]` + /// * On exit: `[b1*x0 + b2*x1 - a1*y0 - a2*y1, b2*x0 - a2*y0]` /// /// ``` /// # use idsp::iir::*; - /// let mut xy = core::array::from_fn(|i| i as _); + /// let mut xy = [0.0, 1.0]; /// let x0 = 3.0; - /// let y0 = Biquad::IDENTITY.update_df2t(&mut xy, x0); + /// let y0 = Biquad::IDENTITY.update(&mut xy, x0); /// assert_eq!(y0, x0); /// assert_eq!(xy, [1.0, 0.0]); /// ``` /// /// # Arguments - /// * `s` - Current filter state. - /// On entry: `[b1*x1 + b2*x2 - a1*y1 - a2*y2, b2*x1 - a2*y1]` - /// On exit: `[b1*x0 + b2*x1 - a1*y0 - a2*y1, b2*x0 - a2*y0]` + /// * `xy` - Current filter state. /// * `x0` - New input. /// /// # Returns /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` - pub fn update_df2t(&self, u: &mut [T; 2], x0: T) -> T { - let y0 = (u[0] + self.ba[0].mul(x0)).clamp(self.min, self.max); - u[0] = u[1] + self.ba[1].mul(x0) + self.ba[3].mul(-y0); - u[1] = self.u + self.ba[2].mul(x0) + self.ba[4].mul(-y0); - y0 + pub fn update(&self, xy: &mut [T; N], x0: T) -> T { + match N { + // DF1 + 4 => { + let (y0, _) = T::macc( + self.ba + .iter() + .copied() + .zip([x0, xy[0], xy[1], -xy[2], -xy[3]]), + self.u, + T::ZERO, + self.min, + self.max, + ); + xy[1] = xy[0]; + xy[0] = x0; + xy[3] = xy[2]; + xy[2] = y0; + y0 + } + // DF1 with noise shaping for fixed point + 5 => { + let (y0, e0) = T::macc( + self.ba + .iter() + .copied() + .zip([x0, xy[0], xy[1], -xy[2], -xy[3]]), + self.u, + xy[4], + self.min, + self.max, + ); + xy[4] = e0; + xy[1] = xy[0]; + xy[0] = x0; + xy[3] = xy[2]; + xy[2] = y0; + y0 + } + // DF2T for floating point + 2 => { + let y0 = (xy[0] + self.ba[0].mul(x0)).clip(self.min, self.max); + xy[0] = xy[1] + self.ba[1].mul(x0) + self.ba[3].mul(-y0); + xy[1] = self.u + self.ba[2].mul(x0) + self.ba[4].mul(-y0); + y0 + } + _ => unimplemented!(), + } } } diff --git a/src/iir/coefficients.rs b/src/iir/coefficients.rs index a04b9dc..1259d43 100644 --- a/src/iir/coefficients.rs +++ b/src/iir/coefficients.rs @@ -124,12 +124,15 @@ where /// /// ``` /// use idsp::iir::*; - /// let ba = Filter::default().critical_frequency(0.1).lowpass(); + /// let ba = Filter::default() + /// .critical_frequency(0.1) + /// .gain(1000.0) + /// .lowpass(); /// let iir = Biquad::::from(&ba); /// let mut xy = [0; 4]; /// let x = vec![3, -4, 5, 7, -3, 2]; /// let y: Vec<_> = x.iter().map(|x0| iir.update(&mut xy, *x0)).collect(); - /// assert_eq!(y, [0, 0, 0, 1, 2, 2]); + /// assert_eq!(y, [5, 3, 9, 25, 42, 49]); /// ``` pub fn lowpass(self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); @@ -150,12 +153,15 @@ where /// /// ``` /// use idsp::iir::*; - /// let ba = Filter::default().critical_frequency(0.1).highpass(); + /// let ba = Filter::default() + /// .critical_frequency(0.1) + /// .gain(1000.0) + /// .highpass(); /// let iir = Biquad::::from(&ba); /// let mut xy = [0; 4]; /// let x = vec![3, -4, 5, 7, -3, 2]; /// let y: Vec<_> = x.iter().map(|x0| iir.update(&mut xy, *x0)).collect(); - /// assert_eq!(y, [2, -4, 5, 3, -6, 1]); + /// assert_eq!(y, [5, -9, 11, 12, -1, 17]); /// ``` pub fn highpass(self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); diff --git a/src/num.rs b/src/num.rs index 612a354..3735e38 100644 --- a/src/num.rs +++ b/src/num.rs @@ -6,9 +6,7 @@ use num_traits::{AsPrimitive, Float}; /// Helper trait unifying fixed point and floating point coefficients/samples pub trait FilterNum: - Copy + PartialEq + Neg + Add + Sum -where - Self: 'static + AsPrimitive, + 'static + Copy + Neg + Add + Sum + AsPrimitive { /// Multiplicative identity const ONE: Self; @@ -23,18 +21,28 @@ where /// Accumulator type type ACCU: AsPrimitive + Add - + Mul; - /// Multiply-accumulate `self + sum(x*a)` - /// + + Mul + + Sum; + /// Proper scaling and potentially using a wide accumulator. - fn macc(self, xa: impl Iterator) -> Self; + /// Clamp `self` such that `min <= self <= max`. + /// Undefined result if `max < min`. + fn macc( + xa: impl Iterator, + u: Self, + e1: Self, + min: Self, + max: Self, + ) -> (Self, Self); + + fn clip(self, min: Self, max: Self) -> Self; + /// Multiplication (scaled) fn mul(self, other: Self) -> Self; + /// Division (scaled) fn div(self, other: Self) -> Self; - /// Clamp `self` such that `min <= self <= max`. - /// Undefined result if `max < min`. - fn clamp(self, min: Self, max: Self) -> Self; + /// Scale and quantize a floating point value. fn quantize(value: C) -> Self where @@ -46,17 +54,26 @@ macro_rules! impl_float { ($T:ty) => { impl FilterNum for $T { const ONE: Self = 1.0; - const NEG_ONE: Self = -Self::ONE; + const NEG_ONE: Self = -1.0; const ZERO: Self = 0.0; - const MIN: Self = Self::NEG_INFINITY; - const MAX: Self = Self::INFINITY; - type ACCU = $T; - fn macc(self, xa: impl Iterator) -> Self { - // a.mul_add(x, y) is std/libm only - xa.fold(self, |u, (a, x)| u + a * x) + const MIN: Self = <$T>::NEG_INFINITY; + const MAX: Self = <$T>::INFINITY; + type ACCU = Self; + + fn macc( + xa: impl Iterator, + u: Self, + _e1: Self, + min: Self, + max: Self, + ) -> (Self, Self) { + (xa.fold(u, |u, (x, a)| u + x * a).clip(min, max), 0.0) } - fn clamp(self, min: Self, max: Self) -> Self { + + fn clip(self, min: Self, max: Self) -> Self { // <$T>::clamp() is slow and checks + // this calls fminf/fmaxf + // self.max(min).min(max) if self < min { min } else if self > max { @@ -65,12 +82,15 @@ macro_rules! impl_float { self } } + fn div(self, other: Self) -> Self { self / other } + fn mul(self, other: Self) -> Self { self * other } + fn quantize>(value: C) -> Self { value.as_() } @@ -81,7 +101,7 @@ impl_float!(f32); impl_float!(f64); macro_rules! impl_int { - ($T:ty, $A:ty, $Q:literal) => { + ($T:ty, $U:ty, $A:ty, $Q:literal) => { impl FilterNum for $T { const ONE: Self = 1 << $Q; const NEG_ONE: Self = -1 << $Q; @@ -90,10 +110,36 @@ macro_rules! impl_int { const MIN: Self = -<$T>::MAX; const MAX: Self = <$T>::MAX; type ACCU = $A; - fn macc(self, xa: impl Iterator) -> Self { - self + (xa.fold(1 << ($Q - 1), |u, (a, x)| u + a as $A * x as $A) >> $Q) as Self + + fn macc( + xa: impl Iterator, + u: Self, + e1: Self, + min: Self, + max: Self, + ) -> (Self, Self) { + const S: usize = core::mem::size_of::<$T>() * 8; + // Guard bits + const G: usize = S - $Q; + // Combine offset (u << $Q) with previous quantization error e1 + let s0 = (((u >> G) as $A) << S) | (((u << $Q) | e1) as $U as $A); + let s = xa.fold(s0, |s, (x, a)| s + x as $A * a as $A); + let sh = (s >> S) as $T; + // Ord::clamp() is slow and checks + // This clamping truncates the lowest G bits of the value and the limits. + let y = if sh < min >> G { + min + } else if sh > max >> G { + max + } else { + (s >> $Q) as $T + }; + // Quantization error + let e = (s & ((1 << $Q) - 1)) as $T; + (y, e) } - fn clamp(self, min: Self, max: Self) -> Self { + + fn clip(self, min: Self, max: Self) -> Self { // Ord::clamp() is slow and checks if self < min { min @@ -103,12 +149,15 @@ macro_rules! impl_int { self } } + fn div(self, other: Self) -> Self { - (((self as $A) << $Q) / other as $A) as Self + (((self as $A) << $Q) / other as $A) as $T } + fn mul(self, other: Self) -> Self { - (((1 << ($Q - 1)) + self as $A * other as $A) >> $Q) as Self + (((1 << ($Q - 1)) + self as $A * other as $A) >> $Q) as $T } + fn quantize(value: C) -> Self where Self: AsPrimitive, @@ -120,7 +169,9 @@ macro_rules! impl_int { }; } // Q2.X chosen to be able to exactly and inclusively represent -2 as `-1 << X + 1` -impl_int!(i8, i16, 6); -impl_int!(i16, i32, 14); -impl_int!(i32, i64, 30); -impl_int!(i64, i128, 62); +// This is necessary to meet a1 = -2 +// It also create 2 guard bits for clamping in the accumulator which is often enough. +impl_int!(i8, u8, i16, 6); +impl_int!(i16, u16, i32, 14); +impl_int!(i32, u32, i64, 30); +impl_int!(i64, u64, i128, 62); diff --git a/src/pll.rs b/src/pll.rs index 9822501..ee11ebb 100644 --- a/src/pll.rs +++ b/src/pll.rs @@ -33,7 +33,7 @@ use serde::{Deserialize, Serialize}; /// /// The extension to I^3,I^2,I behavior to track chirps phase-accurately or to i64 data to /// increase resolution for extremely narrowband applications is obvious. -/// +/// /// This PLL implements first order noise shaping to reduce quantization errors. #[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct PLL { From b8066f36991dfc8c7750574faa5ee68d96a1e020 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 10 Jan 2024 23:56:02 +0100 Subject: [PATCH 13/26] iir: simplify implementation --- src/iir/biquad.rs | 65 +++++++++++++++++++---------------------- src/iir/pid.rs | 14 ++++----- src/num.rs | 74 ++++++++++++++++------------------------------- 3 files changed, 61 insertions(+), 92 deletions(-) diff --git a/src/iir/biquad.rs b/src/iir/biquad.rs index fe521db..4b64447 100644 --- a/src/iir/biquad.rs +++ b/src/iir/biquad.rs @@ -247,14 +247,12 @@ impl Biquad { /// The value is inclusive. /// The clamping also cleanly affects the feedback terms. /// - /// Note: For fixed point filters `Biquad`, `T::MIN` should not be passed - /// to `min()` since the `y` samples stored in - /// the filter state are negated. Instead use `-T::MAX` as the lowest - /// possible limit. + /// For fixed point types, during the comparison, + /// the lowest two bits of value and limit are truncated. /// /// ``` /// # use idsp::iir::*; - /// assert_eq!(Biquad::::default().min(), -i32::MAX); + /// assert_eq!(Biquad::::default().min(), i32::MIN); /// ``` pub fn min(&self) -> T { self.min @@ -267,8 +265,8 @@ impl Biquad { /// ``` /// # use idsp::iir::*; /// let mut i = Biquad::default(); - /// i.set_min(7); - /// assert_eq!(i.update(&mut [0; 4], 0), 7); + /// i.set_min(4); + /// assert_eq!(i.update(&mut [0; 4], 0), 4); /// ``` pub fn set_min(&mut self, min: T) { self.min = min; @@ -280,6 +278,11 @@ impl Biquad { /// The value is inclusive. /// The clamping also cleanly affects the feedback terms. /// + /// For fixed point types, during the comparison, + /// the lowest two bits of value and limit are truncated. + /// The behavior is as if those two bits were 0 in the case + /// of `min` and one in the case of `max`. + /// /// ``` /// # use idsp::iir::*; /// assert_eq!(Biquad::::default().max(), i32::MAX); @@ -295,8 +298,8 @@ impl Biquad { /// ``` /// # use idsp::iir::*; /// let mut i = Biquad::default(); - /// i.set_max(-7); - /// assert_eq!(i.update(&mut [0; 4], 0), -7); + /// i.set_max(-5); + /// assert_eq!(i.update(&mut [0; 4], 0), -5); /// ``` pub fn set_max(&mut self, max: T) { self.max = max; @@ -312,7 +315,7 @@ impl Biquad { /// # Returns /// The sum of the `b` feed-forward coefficients. pub fn forward_gain(&self) -> T { - self.ba.iter().take(3).copied().sum() + self.ba[0] + self.ba[1] + self.ba[2] } /// Compute input-referred (`x`) offset. @@ -325,7 +328,7 @@ impl Biquad { /// assert_eq!(i.input_offset(), i32::ONE); /// ``` pub fn input_offset(&self) -> T { - self.u.div(self.forward_gain()) + self.u.div_scaled(self.forward_gain()) } /// Convert input (`x`) offset to equivalent summing junction offset (`u`) and apply. @@ -356,7 +359,7 @@ impl Biquad { /// # Arguments /// * `offset`: Input (`x`) offset. pub fn set_input_offset(&mut self, offset: T) { - self.u = offset.mul(self.forward_gain()); + self.u = offset.mul_scaled(self.forward_gain()); } /// Direct Form 1 Update @@ -417,16 +420,12 @@ impl Biquad { match N { // DF1 4 => { - let (y0, _) = T::macc( - self.ba - .iter() - .copied() - .zip([x0, xy[0], xy[1], -xy[2], -xy[3]]), - self.u, - T::ZERO, - self.min, - self.max, - ); + let s = self.ba[0].as_() * x0.as_() + + self.ba[1].as_() * xy[0].as_() + + self.ba[2].as_() * xy[1].as_() + - self.ba[3].as_() * xy[2].as_() + - self.ba[4].as_() * xy[3].as_(); + let (y0, _) = self.u.macc(s, self.min, self.max, T::ZERO); xy[1] = xy[0]; xy[0] = x0; xy[3] = xy[2]; @@ -435,16 +434,12 @@ impl Biquad { } // DF1 with noise shaping for fixed point 5 => { - let (y0, e0) = T::macc( - self.ba - .iter() - .copied() - .zip([x0, xy[0], xy[1], -xy[2], -xy[3]]), - self.u, - xy[4], - self.min, - self.max, - ); + let s = self.ba[0].as_() * x0.as_() + + self.ba[1].as_() * xy[0].as_() + + self.ba[2].as_() * xy[1].as_() + - self.ba[3].as_() * xy[2].as_() + - self.ba[4].as_() * xy[3].as_(); + let (y0, e0) = self.u.macc(s, self.min, self.max, xy[4]); xy[4] = e0; xy[1] = xy[0]; xy[0] = x0; @@ -454,9 +449,9 @@ impl Biquad { } // DF2T for floating point 2 => { - let y0 = (xy[0] + self.ba[0].mul(x0)).clip(self.min, self.max); - xy[0] = xy[1] + self.ba[1].mul(x0) + self.ba[3].mul(-y0); - xy[1] = self.u + self.ba[2].mul(x0) + self.ba[4].mul(-y0); + let y0 = (xy[0] + self.ba[0].mul_scaled(x0)).clip(self.min, self.max); + xy[0] = xy[1] + self.ba[1].mul_scaled(x0) - self.ba[3].mul_scaled(y0); + xy[1] = self.u + self.ba[2].mul_scaled(x0) - self.ba[4].mul_scaled(y0); y0 } _ => unimplemented!(), diff --git a/src/iir/pid.rs b/src/iir/pid.rs index e28f1a9..890c5b2 100644 --- a/src/iir/pid.rs +++ b/src/iir/pid.rs @@ -1,5 +1,3 @@ -use core::iter::Sum; - use num_traits::{AsPrimitive, Float}; use serde::{Deserialize, Serialize}; @@ -64,7 +62,7 @@ pub enum Action { Kdd = 4, } -impl> Pid { +impl Pid { /// Sample period /// /// # Arguments @@ -175,9 +173,9 @@ impl> Pid { // Derivative/integration kernels let kernels = [ - [C::ONE, C::ZERO, C::ZERO], - [C::ONE, C::NEG_ONE, C::ZERO], - [C::ONE, C::NEG_ONE + C::NEG_ONE, C::ONE], + [T::one(), T::zero(), T::zero()], + [T::one(), -T::one(), T::zero()], + [T::one(), -(T::one() + T::one()), T::one()], ]; // Scale gains, compute limits, quantize @@ -194,14 +192,14 @@ impl> Pid { gli[1] = if i == KP { T::one() } else { gli[0] / *lli }; zi = zi * self.period; } - let a0i = T::one() / gl.iter().map(|gli| gli[1]).sum(); + let a0i = T::one() / (gl[0][1] + gl[1][1] + gl[2][1]); // Coefficients let mut ba = [[C::ZERO; 2]; 3]; for (gli, ki) in gl.iter().zip(kernels.iter()) { let (g, l) = (C::quantize(gli[0] * a0i), C::quantize(gli[1] * a0i)); for (j, baj) in ba.iter_mut().enumerate() { - *baj = [baj[0] + ki[j].mul(g), baj[1] + ki[j].mul(l)]; + *baj = [baj[0] + ki[j].as_() * g, baj[1] + ki[j].as_() * l]; } } diff --git a/src/num.rs b/src/num.rs index 3735e38..3eb3a85 100644 --- a/src/num.rs +++ b/src/num.rs @@ -1,13 +1,9 @@ -use core::{ - iter::Sum, - ops::{Add, Mul, Neg}, -}; -use num_traits::{AsPrimitive, Float}; +use num_traits::{AsPrimitive, Float, Num}; /// Helper trait unifying fixed point and floating point coefficients/samples -pub trait FilterNum: - 'static + Copy + Neg + Add + Sum + AsPrimitive -{ +pub trait FilterNum: 'static + Copy + Num + AsPrimitive { + /// Scale + const SHIFT: u32; /// Multiplicative identity const ONE: Self; /// Negative multiplicative identity, equal to `-Self::ONE`. @@ -19,29 +15,20 @@ pub trait FilterNum: /// Highest value const MAX: Self; /// Accumulator type - type ACCU: AsPrimitive - + Add - + Mul - + Sum; + type ACCU: AsPrimitive + Num; /// Proper scaling and potentially using a wide accumulator. /// Clamp `self` such that `min <= self <= max`. /// Undefined result if `max < min`. - fn macc( - xa: impl Iterator, - u: Self, - e1: Self, - min: Self, - max: Self, - ) -> (Self, Self); + fn macc(self, s: Self::ACCU, min: Self, max: Self, e1: Self) -> (Self, Self); fn clip(self, min: Self, max: Self) -> Self; /// Multiplication (scaled) - fn mul(self, other: Self) -> Self; + fn mul_scaled(self, other: Self) -> Self; /// Division (scaled) - fn div(self, other: Self) -> Self; + fn div_scaled(self, other: Self) -> Self; /// Scale and quantize a floating point value. fn quantize(value: C) -> Self @@ -53,6 +40,7 @@ pub trait FilterNum: macro_rules! impl_float { ($T:ty) => { impl FilterNum for $T { + const SHIFT: u32 = 0; const ONE: Self = 1.0; const NEG_ONE: Self = -1.0; const ZERO: Self = 0.0; @@ -60,14 +48,8 @@ macro_rules! impl_float { const MAX: Self = <$T>::INFINITY; type ACCU = Self; - fn macc( - xa: impl Iterator, - u: Self, - _e1: Self, - min: Self, - max: Self, - ) -> (Self, Self) { - (xa.fold(u, |u, (x, a)| u + x * a).clip(min, max), 0.0) + fn macc(self, s: Self::ACCU, min: Self, max: Self, _e1: Self) -> (Self, Self) { + ((self + s).clip(min, max), 0.0) } fn clip(self, min: Self, max: Self) -> Self { @@ -83,11 +65,11 @@ macro_rules! impl_float { } } - fn div(self, other: Self) -> Self { + fn div_scaled(self, other: Self) -> Self { self / other } - fn mul(self, other: Self) -> Self { + fn mul_scaled(self, other: Self) -> Self { self * other } @@ -103,40 +85,34 @@ impl_float!(f64); macro_rules! impl_int { ($T:ty, $U:ty, $A:ty, $Q:literal) => { impl FilterNum for $T { + const SHIFT: u32 = $Q; const ONE: Self = 1 << $Q; const NEG_ONE: Self = -1 << $Q; const ZERO: Self = 0; - // Need to avoid `$T::MIN*$T::MIN` overflow. - const MIN: Self = -<$T>::MAX; + const MIN: Self = <$T>::MIN; const MAX: Self = <$T>::MAX; type ACCU = $A; - fn macc( - xa: impl Iterator, - u: Self, - e1: Self, - min: Self, - max: Self, - ) -> (Self, Self) { + fn macc(self, mut s: Self::ACCU, min: Self, max: Self, e1: Self) -> (Self, Self) { const S: usize = core::mem::size_of::<$T>() * 8; // Guard bits const G: usize = S - $Q; // Combine offset (u << $Q) with previous quantization error e1 - let s0 = (((u >> G) as $A) << S) | (((u << $Q) | e1) as $U as $A); - let s = xa.fold(s0, |s, (x, a)| s + x as $A * a as $A); - let sh = (s >> S) as $T; + s += (((self >> G) as $A) << S) | (((self << $Q) | e1) as $U as $A); // Ord::clamp() is slow and checks // This clamping truncates the lowest G bits of the value and the limits. - let y = if sh < min >> G { + debug_assert_eq!(min & ((1 << G) - 1), 0); + debug_assert_eq!(max & ((1 << G) - 1), (1 << G) - 1); + let y0 = if (s >> S) as $T < (min >> G) { min - } else if sh > max >> G { + } else if (s >> S) as $T > (max >> G) { max } else { (s >> $Q) as $T }; // Quantization error - let e = (s & ((1 << $Q) - 1)) as $T; - (y, e) + let e0 = s as $T & ((1 << $Q) - 1); + (y0, e0) } fn clip(self, min: Self, max: Self) -> Self { @@ -150,11 +126,11 @@ macro_rules! impl_int { } } - fn div(self, other: Self) -> Self { + fn div_scaled(self, other: Self) -> Self { (((self as $A) << $Q) / other as $A) as $T } - fn mul(self, other: Self) -> Self { + fn mul_scaled(self, other: Self) -> Self { (((1 << ($Q - 1)) + self as $A * other as $A) >> $Q) as $T } From d52c5fb4bea95ee304f4858fefac1e9d1b6b5f66 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 11 Jan 2024 13:45:55 +0100 Subject: [PATCH 14/26] coefficients: comments, docs, tests --- src/iir/coefficients.rs | 251 ++++++++++++++++++++++++++++++++++++---- src/iir/pid.rs | 22 ++-- 2 files changed, 239 insertions(+), 34 deletions(-) diff --git a/src/iir/coefficients.rs b/src/iir/coefficients.rs index 1259d43..486bd80 100644 --- a/src/iir/coefficients.rs +++ b/src/iir/coefficients.rs @@ -18,7 +18,7 @@ impl Default for Shape { /// Standard audio biquad filter builder /// -/// +/// #[derive(Copy, Clone, Debug, PartialEq, PartialOrd, Serialize, Deserialize)] pub struct Filter { /// Angular critical frequency (in units of sampling frequency) @@ -49,53 +49,119 @@ where T: 'static + Float + FloatConst, f32: AsPrimitive, { + /// Set crititcal frequency from absolute units. + /// + /// # Arguments + /// * `critical_frequency`: "Relevant" or "corner" or "center" frequency + /// in the same units as `sample_frequency` + /// * `sample_frequency`: The sample frequency in the same units as `critical_frequency`. + /// E.g. both in SI Hertz or `rad/s`. pub fn frequency(self, critical_frequency: T, sample_frequency: T) -> Self { self.critical_frequency(critical_frequency / sample_frequency) } - pub fn critical_frequency(self, critical_frequency: T) -> Self { - self.angular_critical_frequency(T::TAU() * critical_frequency) + /// Set relative critical frequency + /// + /// # Arguments + /// * `f0`: Relative critical frequency in units of the sample frequency. + /// Must be `0 <= f0 <= 0.5`. + pub fn critical_frequency(self, f0: T) -> Self { + self.angular_critical_frequency(T::TAU() * f0) } + /// Set relative critical angular frequency + /// + /// # Arguments + /// * `w0`: Relative critical angular frequency. + /// Must be `0 <= w0 <= π`. Defaults to `0.0`. pub fn angular_critical_frequency(mut self, w0: T) -> Self { self.w0 = w0; self } + /// Set reference gain + /// + /// # Arguments + /// * `k`: Linear reference gain. Defaults to `1.0`. pub fn gain(mut self, k: T) -> Self { self.gain = k; self } + /// Set reference gain in dB + /// + /// # Arguments + /// * `k_db`: Reference gain in dB. Defaults to `0.0`. pub fn gain_db(self, k_db: T) -> Self { self.gain(10.0.as_().powf(k_db / 20.0.as_())) } + /// Set linear shelf gain + /// + /// Used only for `peaking`, `highshelf`, `lowshelf` filters. + /// + /// # Arguments + /// * `a`: Linear shelf gain. Defaults to `0.0`. pub fn shelf(mut self, a: T) -> Self { self.shelf = a; self } - pub fn shelf_db(self, k_db: T) -> Self { - self.shelf(10.0.as_().powf(k_db / 20.0.as_())) + /// Set shelf gain in dB + /// + /// Used only for `peaking`, `highshelf`, `lowshelf` filters. + /// + /// # Arguments + /// * `a_db`: Linear shelf gain. Defaults to `0.0`. + pub fn shelf_db(self, a_db: T) -> Self { + self.shelf(10.0.as_().powf(a_db / 40.0.as_())) } + /// Set inverse Q parameter of the filter + /// + /// The inverse "steepness"/"narrowness" of the filter transition. + /// Defaults `sqrt(2)` which is as steep as possible without overshoot. + /// + /// # Arguments + /// * `qi`: Inverse Q parameter. pub fn inverse_q(mut self, qi: T) -> Self { self.shape = Shape::InverseQ(qi); self } + /// Set Q parameter of the filter + /// + /// The "steepness"/"narrowness" of the filter transition. + /// Defaults `1/sqrt(2)` which is as steep as possible without overshoot. + /// + /// This affects the same parameter as `bandwidth()` and `shelf_slope()`. + /// Use only one of them. + /// + /// # Arguments + /// * `q`: Q parameter. pub fn q(self, q: T) -> Self { self.inverse_q(T::one() / q) } - /// Relative bandwidth in octaves. + /// Set the relative bandwidth + /// + /// This affects the same parameter as `inverse_q()` and `shelf_slope()`. + /// Use only one of them. + /// + /// # Arguments + /// * `bw`: Bandwidth in octaves pub fn bandwidth(mut self, bw: T) -> Self { self.shape = Shape::Bandwidth(bw); self } - /// Shelf slope. + /// Set the shelf slope. + /// + /// This affects the same parameter as `inverse_q()` and `bandwidth()`. + /// Use only one of them. + /// + /// # Arguments + /// * `s`: Shelf slope. A slope of `1.0` is maximally steep without overshoot. pub fn shelf_slope(mut self, s: T) -> Self { self.shape = Shape::Slope(s); self @@ -139,10 +205,10 @@ where let b = self.gain * 0.5.as_() * (T::one() - fcos); [ b, - b + b, + (2.0).as_() * b, b, T::one() + alpha, - -(fcos + fcos), + (-2.0).as_() * fcos, T::one() - alpha, ] } @@ -168,10 +234,10 @@ where let b = self.gain * 0.5.as_() * (T::one() + fcos); [ b, - -(b + b), + (-2.0).as_() * b, b, T::one() + alpha, - -(fcos + fcos), + (-2.0).as_() * fcos, T::one() - alpha, ] } @@ -195,49 +261,63 @@ where T::zero(), -b, T::one() + alpha, - -(fcos + fcos), + (-2.0).as_() * fcos, T::one() - alpha, ] } + /// A notch filter + /// + /// Has zero gain at the critical frequency. pub fn notch(self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); + let f2 = (-2.0).as_() * fcos; [ self.gain, - -(fcos + fcos) * self.gain, + f2 * self.gain, self.gain, T::one() + alpha, - -(fcos + fcos), + f2, T::one() - alpha, ] } + /// An allpass filter + /// + /// Has constant `gain` at all frequency but a variable phase shift. pub fn allpass(self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); + let f2 = (-2.0).as_() * fcos; [ (T::one() - alpha) * self.gain, - -(fcos + fcos) * self.gain, + f2 * self.gain, (T::one() + alpha) * self.gain, T::one() + alpha, - -(fcos + fcos), + f2, T::one() - alpha, ] } + /// A peaking/dip filter + /// + /// Has `gain*shelf_gain` at critical frequency and `gain` elsewhere. pub fn peaking(self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); + let f2 = (-2.0).as_() * fcos; [ (T::one() + alpha * self.shelf) * self.gain, - -(fcos + fcos) * self.gain, + f2 * self.gain, (T::one() - alpha * self.shelf) * self.gain, T::one() + alpha / self.shelf, - -(fcos + fcos), + f2, T::one() - alpha / self.shelf, ] } /// Low shelf /// + /// Approaches `gain*shelf_gain` below critical frequency and `gain` above. + /// /// ``` /// use idsp::iir::*; /// let ba = Filter::default() @@ -262,6 +342,9 @@ where ] } + /// Low shelf + /// + /// Approaches `gain*shelf_gain` above critical frequency and `gain` below. pub fn highshelf(self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); let tsa = 2.0.as_() * self.shelf.sqrt() * alpha; @@ -277,8 +360,21 @@ where ] } - // TODO - // PI-notch + /// I/HO + /// + /// Notch, integrating below, flat `shelf_gain` above + pub fn iho(self) -> [T; 6] { + let (fcos, alpha) = self.fcos_alpha(); + let a = (T::one() + fcos) / (10.0.sqrt().as_() * self.shelf); + [ + 2.0.as_() * self.gain * (T::one() + alpha), + (-4.0).as_() * self.gain * fcos, + 2.0.as_() * self.gain * (T::one() - alpha), + a + self.w0.sin(), + (-2.0).as_() * a, + a - self.w0.sin(), + ] + } } // TODO @@ -371,7 +467,7 @@ mod test { .lowpass(), &[ (1e-3, Tol::GainDb(20.0, 0.01)), - (0.01, Tol::GainDb(17.0, 0.1)), + (0.01, Tol::GainDb(17.0, 0.02)), (4e-1, Tol::GainBelowDb(-40.0)), ], ); @@ -386,12 +482,47 @@ mod test { .highpass(), &[ (1e-3, Tol::GainBelowDb(-40.0)), - (0.1, Tol::GainDb(-5.0, 0.1)), + (0.1, Tol::GainDb(-5.0, 0.02)), (4e-1, Tol::GainDb(-2.0, 0.01)), ], ); } + #[test] + fn bandpass() { + check_coeffs( + &Filter::default() + .critical_frequency(0.02) + .bandwidth(2.0) + .gain_db(3.0) + .bandpass(), + &[ + (1e-4, Tol::GainBelowDb(-35.0)), + (0.01, Tol::GainDb(0.0, 0.02)), + (0.02, Tol::GainDb(3.0, 0.01)), + (0.04, Tol::GainDb(0.0, 0.04)), + (4e-1, Tol::GainBelowDb(-25.0)), + ], + ); + } + + #[test] + fn allpass() { + check_coeffs( + &Filter::default() + .critical_frequency(0.02) + .gain_db(-10.0) + .allpass(), + &[ + (1e-4, Tol::GainDb(-10.0, 0.01)), + (0.01, Tol::GainDb(-10.0, 0.01)), + (0.02, Tol::GainDb(-10.0, 0.01)), + (0.04, Tol::GainDb(-10.0, 0.01)), + (4e-1, Tol::GainDb(-10.0, 0.01)), + ], + ); + } + #[test] fn notch() { check_coeffs( @@ -401,11 +532,81 @@ mod test { .notch(), &[ (1e-4, Tol::GainDb(0.0, 0.01)), - (0.01, Tol::GainDb(-3.0, 0.1)), - (0.02, Tol::GainBelowDb(-80.0)), - (0.04, Tol::GainDb(-3.0, 0.1)), + (0.01, Tol::GainDb(-3.0, 0.02)), + (0.02, Tol::GainBelowDb(-140.0)), + (0.04, Tol::GainDb(-3.0, 0.02)), (4e-1, Tol::GainDb(0.0, 0.01)), ], ); } + + #[test] + fn peaking() { + check_coeffs( + &Filter::default() + .critical_frequency(0.02) + .bandwidth(2.0) + .gain_db(-10.0) + .shelf_db(20.0) + .peaking(), + &[ + (1e-4, Tol::GainDb(-10.0, 0.01)), + (0.01, Tol::GainDb(0.0, 0.04)), + (0.02, Tol::GainDb(10.0, 0.01)), + (0.04, Tol::GainDb(0.0, 0.04)), + (4e-1, Tol::GainDb(-10.0, 0.05)), + ], + ); + } + + #[test] + fn highshelf() { + check_coeffs( + &Filter::default() + .critical_frequency(0.02) + .gain_db(-20.0) + .shelf_db(10.0) + .highshelf(), + &[ + (1e-6, Tol::GainDb(-20.0, 0.01)), + (1e-4, Tol::GainDb(-20.0, 0.01)), + (0.02, Tol::GainDb(-15.0, 0.01)), + (4e-1, Tol::GainDb(-10.0, 0.01)), + ], + ); + } + + #[test] + fn lowshelf() { + check_coeffs( + &Filter::default() + .critical_frequency(0.02) + .gain_db(-10.0) + .shelf_db(30.0) + .lowshelf(), + &[ + (1e-6, Tol::GainDb(20.0, 0.01)), + (1e-4, Tol::GainDb(20.0, 0.01)), + (0.02, Tol::GainDb(5.0, 0.01)), + (4e-1, Tol::GainDb(-10.0, 0.01)), + ], + ); + } + + #[test] + fn iho() { + check_coeffs( + &Filter::default() + .critical_frequency(0.01) + .gain_db(-20.0) + .shelf_db(20.0) + .q(10.) + .iho(), + &[ + (1e-5, Tol::GainDb(40.0, 0.01)), + (0.01, Tol::GainDb(-40.0, 0.05)), + (4.99e-1, Tol::GainDb(0.0, 0.01)), + ], + ); + } } diff --git a/src/iir/pid.rs b/src/iir/pid.rs index 890c5b2..42a7950 100644 --- a/src/iir/pid.rs +++ b/src/iir/pid.rs @@ -153,6 +153,9 @@ impl Pid { /// let i: Biquad = Pid::default().gain(Action::Kp, 3.0).build().unwrap().into(); /// assert_eq!(i, Biquad::proportional(3.0)); /// ``` + /// + /// # Panic + /// Will panic in debug mode on coefficient overflow. pub fn build>(self) -> Result<[C; 5], PidError> where T: AsPrimitive, @@ -171,14 +174,7 @@ impl Pid { return Err(PidError::OrderRange); } - // Derivative/integration kernels - let kernels = [ - [T::one(), T::zero(), T::zero()], - [T::one(), -T::one(), T::zero()], - [T::one(), -(T::one() + T::one()), T::one()], - ]; - - // Scale gains, compute limits, quantize + // Scale gains, compute limits let mut zi = self.period.powi(low as i32 - KP as i32); let mut gl = [[T::zero(); 2]; 3]; for (gli, (i, (ggi, lli))) in gl.iter_mut().zip( @@ -194,12 +190,20 @@ impl Pid { } let a0i = T::one() / (gl[0][1] + gl[1][1] + gl[2][1]); + // Derivative/integration kernels + let kernels = [ + [C::one(), C::zero(), C::zero()], + [C::one(), C::zero() - C::one(), C::zero()], + [C::one(), C::zero() - C::one() - C::one(), C::one()], + ]; + // Coefficients let mut ba = [[C::ZERO; 2]; 3]; for (gli, ki) in gl.iter().zip(kernels.iter()) { + // Quantize the gains and not the coefficients let (g, l) = (C::quantize(gli[0] * a0i), C::quantize(gli[1] * a0i)); for (j, baj) in ba.iter_mut().enumerate() { - *baj = [baj[0] + ki[j].as_() * g, baj[1] + ki[j].as_() * l]; + *baj = [baj[0] + ki[j] * g, baj[1] + ki[j] * l]; } } From 34ce0672939a34a95f1d96001758c845a13429e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 11 Jan 2024 15:53:33 +0100 Subject: [PATCH 15/26] float intrinsics --- src/num.rs | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/num.rs b/src/num.rs index 3eb3a85..d36e286 100644 --- a/src/num.rs +++ b/src/num.rs @@ -48,31 +48,28 @@ macro_rules! impl_float { const MAX: Self = <$T>::INFINITY; type ACCU = Self; + #[inline] fn macc(self, s: Self::ACCU, min: Self, max: Self, _e1: Self) -> (Self, Self) { ((self + s).clip(min, max), 0.0) } + #[inline] fn clip(self, min: Self, max: Self) -> Self { // <$T>::clamp() is slow and checks - // this calls fminf/fmaxf - // self.max(min).min(max) - if self < min { - min - } else if self > max { - max - } else { - self - } + self.max(min).min(max) } + #[inline] fn div_scaled(self, other: Self) -> Self { self / other } + #[inline] fn mul_scaled(self, other: Self) -> Self { self * other } + #[inline] fn quantize>(value: C) -> Self { value.as_() } @@ -93,6 +90,7 @@ macro_rules! impl_int { const MAX: Self = <$T>::MAX; type ACCU = $A; + #[inline] fn macc(self, mut s: Self::ACCU, min: Self, max: Self, e1: Self) -> (Self, Self) { const S: usize = core::mem::size_of::<$T>() * 8; // Guard bits @@ -115,6 +113,7 @@ macro_rules! impl_int { (y0, e0) } + #[inline] fn clip(self, min: Self, max: Self) -> Self { // Ord::clamp() is slow and checks if self < min { @@ -126,14 +125,17 @@ macro_rules! impl_int { } } + #[inline] fn div_scaled(self, other: Self) -> Self { (((self as $A) << $Q) / other as $A) as $T } + #[inline] fn mul_scaled(self, other: Self) -> Self { (((1 << ($Q - 1)) + self as $A * other as $A) >> $Q) as $T } + #[inline] fn quantize(value: C) -> Self where Self: AsPrimitive, From fb0a3a36b9ea79b8eaa4ced1004c37d50d2f3e3a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 11 Jan 2024 15:57:57 +0100 Subject: [PATCH 16/26] builders take mut references --- src/iir/coefficients.rs | 43 +++++++++++++++++++++-------------------- src/iir/pid.rs | 10 +++++----- 2 files changed, 27 insertions(+), 26 deletions(-) diff --git a/src/iir/coefficients.rs b/src/iir/coefficients.rs index 486bd80..3fd8981 100644 --- a/src/iir/coefficients.rs +++ b/src/iir/coefficients.rs @@ -3,10 +3,11 @@ use serde::{Deserialize, Serialize}; #[derive(Copy, Clone, Debug, PartialEq, PartialOrd, Serialize, Deserialize)] enum Shape { - /// Inverse W + /// Inverse Q, sqrt(2) for critical InverseQ(T), /// Relative bandwidth in octaves Bandwidth(T), + /// Slope steepnes, 1 for critical Slope(T), } @@ -56,7 +57,7 @@ where /// in the same units as `sample_frequency` /// * `sample_frequency`: The sample frequency in the same units as `critical_frequency`. /// E.g. both in SI Hertz or `rad/s`. - pub fn frequency(self, critical_frequency: T, sample_frequency: T) -> Self { + pub fn frequency(&mut self, critical_frequency: T, sample_frequency: T) -> &mut Self { self.critical_frequency(critical_frequency / sample_frequency) } @@ -65,7 +66,7 @@ where /// # Arguments /// * `f0`: Relative critical frequency in units of the sample frequency. /// Must be `0 <= f0 <= 0.5`. - pub fn critical_frequency(self, f0: T) -> Self { + pub fn critical_frequency(&mut self, f0: T) -> &mut Self { self.angular_critical_frequency(T::TAU() * f0) } @@ -74,7 +75,7 @@ where /// # Arguments /// * `w0`: Relative critical angular frequency. /// Must be `0 <= w0 <= π`. Defaults to `0.0`. - pub fn angular_critical_frequency(mut self, w0: T) -> Self { + pub fn angular_critical_frequency(&mut self, w0: T) -> &mut Self { self.w0 = w0; self } @@ -83,7 +84,7 @@ where /// /// # Arguments /// * `k`: Linear reference gain. Defaults to `1.0`. - pub fn gain(mut self, k: T) -> Self { + pub fn gain(&mut self, k: T) -> &mut Self { self.gain = k; self } @@ -92,7 +93,7 @@ where /// /// # Arguments /// * `k_db`: Reference gain in dB. Defaults to `0.0`. - pub fn gain_db(self, k_db: T) -> Self { + pub fn gain_db(&mut self, k_db: T) -> &mut Self { self.gain(10.0.as_().powf(k_db / 20.0.as_())) } @@ -102,7 +103,7 @@ where /// /// # Arguments /// * `a`: Linear shelf gain. Defaults to `0.0`. - pub fn shelf(mut self, a: T) -> Self { + pub fn shelf(&mut self, a: T) -> &mut Self { self.shelf = a; self } @@ -113,7 +114,7 @@ where /// /// # Arguments /// * `a_db`: Linear shelf gain. Defaults to `0.0`. - pub fn shelf_db(self, a_db: T) -> Self { + pub fn shelf_db(&mut self, a_db: T) -> &mut Self { self.shelf(10.0.as_().powf(a_db / 40.0.as_())) } @@ -124,7 +125,7 @@ where /// /// # Arguments /// * `qi`: Inverse Q parameter. - pub fn inverse_q(mut self, qi: T) -> Self { + pub fn inverse_q(&mut self, qi: T) -> &mut Self { self.shape = Shape::InverseQ(qi); self } @@ -139,7 +140,7 @@ where /// /// # Arguments /// * `q`: Q parameter. - pub fn q(self, q: T) -> Self { + pub fn q(&mut self, q: T) -> &mut Self { self.inverse_q(T::one() / q) } @@ -150,7 +151,7 @@ where /// /// # Arguments /// * `bw`: Bandwidth in octaves - pub fn bandwidth(mut self, bw: T) -> Self { + pub fn bandwidth(&mut self, bw: T) -> &mut Self { self.shape = Shape::Bandwidth(bw); self } @@ -162,7 +163,7 @@ where /// /// # Arguments /// * `s`: Shelf slope. A slope of `1.0` is maximally steep without overshoot. - pub fn shelf_slope(mut self, s: T) -> Self { + pub fn shelf_slope(&mut self, s: T) -> &mut Self { self.shape = Shape::Slope(s); self } @@ -200,7 +201,7 @@ where /// let y: Vec<_> = x.iter().map(|x0| iir.update(&mut xy, *x0)).collect(); /// assert_eq!(y, [5, 3, 9, 25, 42, 49]); /// ``` - pub fn lowpass(self) -> [T; 6] { + pub fn lowpass(&self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); let b = self.gain * 0.5.as_() * (T::one() - fcos); [ @@ -229,7 +230,7 @@ where /// let y: Vec<_> = x.iter().map(|x0| iir.update(&mut xy, *x0)).collect(); /// assert_eq!(y, [5, -9, 11, 12, -1, 17]); /// ``` - pub fn highpass(self) -> [T; 6] { + pub fn highpass(&self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); let b = self.gain * 0.5.as_() * (T::one() + fcos); [ @@ -253,7 +254,7 @@ where /// .bandpass(); /// println!("{ba:?}"); /// ``` - pub fn bandpass(self) -> [T; 6] { + pub fn bandpass(&self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); let b = self.gain * alpha; [ @@ -269,7 +270,7 @@ where /// A notch filter /// /// Has zero gain at the critical frequency. - pub fn notch(self) -> [T; 6] { + pub fn notch(&self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); let f2 = (-2.0).as_() * fcos; [ @@ -285,7 +286,7 @@ where /// An allpass filter /// /// Has constant `gain` at all frequency but a variable phase shift. - pub fn allpass(self) -> [T; 6] { + pub fn allpass(&self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); let f2 = (-2.0).as_() * fcos; [ @@ -301,7 +302,7 @@ where /// A peaking/dip filter /// /// Has `gain*shelf_gain` at critical frequency and `gain` elsewhere. - pub fn peaking(self) -> [T; 6] { + pub fn peaking(&self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); let f2 = (-2.0).as_() * fcos; [ @@ -327,7 +328,7 @@ where /// .lowshelf(); /// println!("{ba:?}"); /// ``` - pub fn lowshelf(self) -> [T; 6] { + pub fn lowshelf(&self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); let tsa = 2.0.as_() * self.shelf.sqrt() * alpha; let sp1 = self.shelf + T::one(); @@ -345,7 +346,7 @@ where /// Low shelf /// /// Approaches `gain*shelf_gain` above critical frequency and `gain` below. - pub fn highshelf(self) -> [T; 6] { + pub fn highshelf(&self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); let tsa = 2.0.as_() * self.shelf.sqrt() * alpha; let sp1 = self.shelf + T::one(); @@ -363,7 +364,7 @@ where /// I/HO /// /// Notch, integrating below, flat `shelf_gain` above - pub fn iho(self) -> [T; 6] { + pub fn iho(&self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); let a = (T::one() + fcos) / (10.0.sqrt().as_() * self.shelf); [ diff --git a/src/iir/pid.rs b/src/iir/pid.rs index 42a7950..c0159ce 100644 --- a/src/iir/pid.rs +++ b/src/iir/pid.rs @@ -67,7 +67,7 @@ impl Pid { /// /// # Arguments /// * `period`: Sample period in some units, e.g. SI seconds - pub fn period(mut self, period: T) -> Self { + pub fn period(&mut self, period: T) -> &mut Self { self.period = period; self } @@ -103,7 +103,7 @@ impl Pid { /// # Arguments /// * `action`: Action to control /// * `gain`: Gain value - pub fn gain(mut self, action: Action, gain: T) -> Self { + pub fn gain(&mut self, action: Action, gain: T) -> &mut Self { self.gains[action as usize] = gain; self } @@ -134,7 +134,7 @@ impl Pid { /// # Arguments /// * `action`: Action to limit in gain /// * `limit`: Gain limit - pub fn limit(mut self, action: Action, limit: T) -> Self { + pub fn limit(&mut self, action: Action, limit: T) -> &mut Self { self.limits[action as usize] = limit; self } @@ -155,8 +155,8 @@ impl Pid { /// ``` /// /// # Panic - /// Will panic in debug mode on coefficient overflow. - pub fn build>(self) -> Result<[C; 5], PidError> + /// Will panic in debug mode on fixed point coefficient overflow. + pub fn build>(&self) -> Result<[C; 5], PidError> where T: AsPrimitive, { From fc1cc489056c460d5ad727bc55bdf2ccfca4f55e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 11 Jan 2024 16:25:02 +0100 Subject: [PATCH 17/26] make linear shelf gain meaningful --- src/iir/coefficients.rs | 69 ++++++++++++++++++++++------------------- 1 file changed, 37 insertions(+), 32 deletions(-) diff --git a/src/iir/coefficients.rs b/src/iir/coefficients.rs index 3fd8981..d5408b8 100644 --- a/src/iir/coefficients.rs +++ b/src/iir/coefficients.rs @@ -102,7 +102,7 @@ where /// Used only for `peaking`, `highshelf`, `lowshelf` filters. /// /// # Arguments - /// * `a`: Linear shelf gain. Defaults to `0.0`. + /// * `a`: Linear shelf gain. Defaults to `1.0`. pub fn shelf(&mut self, a: T) -> &mut Self { self.shelf = a; self @@ -115,7 +115,7 @@ where /// # Arguments /// * `a_db`: Linear shelf gain. Defaults to `0.0`. pub fn shelf_db(&mut self, a_db: T) -> &mut Self { - self.shelf(10.0.as_().powf(a_db / 40.0.as_())) + self.shelf(10.0.as_().powf(a_db / 20.0.as_())) } /// Set inverse Q parameter of the filter @@ -168,6 +168,7 @@ where self } + /// Get inverse Q fn qi(&self) -> T { match self.shape { Shape::InverseQ(qi) => qi, @@ -180,6 +181,7 @@ where } } + /// Get (cos(w0), alpha=sin(w0)/(2*q)) fn fcos_alpha(&self) -> (T, T) { let (fsin, fcos) = self.w0.sin_cos(); (fcos, 0.5.as_() * fsin * self.qi()) @@ -304,14 +306,15 @@ where /// Has `gain*shelf_gain` at critical frequency and `gain` elsewhere. pub fn peaking(&self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); + let s = self.shelf.sqrt(); let f2 = (-2.0).as_() * fcos; [ - (T::one() + alpha * self.shelf) * self.gain, + (T::one() + alpha * s) * self.gain, f2 * self.gain, - (T::one() - alpha * self.shelf) * self.gain, - T::one() + alpha / self.shelf, + (T::one() - alpha * s) * self.gain, + T::one() + alpha / s, f2, - T::one() - alpha / self.shelf, + T::one() - alpha / s, ] } @@ -330,13 +333,14 @@ where /// ``` pub fn lowshelf(&self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); - let tsa = 2.0.as_() * self.shelf.sqrt() * alpha; - let sp1 = self.shelf + T::one(); - let sm1 = self.shelf - T::one(); + let s = self.shelf.sqrt(); + let tsa = 2.0.as_() * s.sqrt() * alpha; + let sp1 = s + T::one(); + let sm1 = s - T::one(); [ - self.shelf * self.gain * (sp1 - sm1 * fcos + tsa), - 2.0.as_() * self.shelf * self.gain * (sm1 - sp1 * fcos), - self.shelf * self.gain * (sp1 - sm1 * fcos - tsa), + s * self.gain * (sp1 - sm1 * fcos + tsa), + 2.0.as_() * s * self.gain * (sm1 - sp1 * fcos), + s * self.gain * (sp1 - sm1 * fcos - tsa), sp1 + sm1 * fcos + tsa, (-2.0).as_() * (sm1 + sp1 * fcos), sp1 + sm1 * fcos - tsa, @@ -348,13 +352,14 @@ where /// Approaches `gain*shelf_gain` above critical frequency and `gain` below. pub fn highshelf(&self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); - let tsa = 2.0.as_() * self.shelf.sqrt() * alpha; - let sp1 = self.shelf + T::one(); - let sm1 = self.shelf - T::one(); + let s = self.shelf.sqrt(); + let tsa = 2.0.as_() * s.sqrt() * alpha; + let sp1 = s + T::one(); + let sm1 = s - T::one(); [ - self.shelf * self.gain * (sp1 + sm1 * fcos + tsa), - (-2.0).as_() * self.shelf * self.gain * (sm1 + sp1 * fcos), - self.shelf * self.gain * (sp1 + sm1 * fcos - tsa), + s * self.gain * (sp1 + sm1 * fcos + tsa), + (-2.0).as_() * s * self.gain * (sm1 + sp1 * fcos), + s * self.gain * (sp1 + sm1 * fcos - tsa), sp1 - sm1 * fcos + tsa, 2.0.as_() * (sm1 - sp1 * fcos), sp1 - sm1 * fcos - tsa, @@ -366,7 +371,7 @@ where /// Notch, integrating below, flat `shelf_gain` above pub fn iho(&self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); - let a = (T::one() + fcos) / (10.0.sqrt().as_() * self.shelf); + let a = (T::one() + fcos) / self.shelf; [ 2.0.as_() * self.gain * (T::one() + alpha), (-4.0).as_() * self.gain * fcos, @@ -565,14 +570,14 @@ mod test { check_coeffs( &Filter::default() .critical_frequency(0.02) - .gain_db(-20.0) - .shelf_db(10.0) + .gain_db(-10.0) + .shelf_db(-20.0) .highshelf(), &[ - (1e-6, Tol::GainDb(-20.0, 0.01)), - (1e-4, Tol::GainDb(-20.0, 0.01)), - (0.02, Tol::GainDb(-15.0, 0.01)), - (4e-1, Tol::GainDb(-10.0, 0.01)), + (1e-6, Tol::GainDb(-10.0, 0.01)), + (1e-4, Tol::GainDb(-10.0, 0.01)), + (0.02, Tol::GainDb(-20.0, 0.01)), + (4e-1, Tol::GainDb(-30.0, 0.01)), ], ); } @@ -583,12 +588,12 @@ mod test { &Filter::default() .critical_frequency(0.02) .gain_db(-10.0) - .shelf_db(30.0) + .shelf_db(-20.0) .lowshelf(), &[ - (1e-6, Tol::GainDb(20.0, 0.01)), - (1e-4, Tol::GainDb(20.0, 0.01)), - (0.02, Tol::GainDb(5.0, 0.01)), + (1e-6, Tol::GainDb(-30.0, 0.01)), + (1e-4, Tol::GainDb(-30.0, 0.01)), + (0.02, Tol::GainDb(-20.0, 0.01)), (4e-1, Tol::GainDb(-10.0, 0.01)), ], ); @@ -600,13 +605,13 @@ mod test { &Filter::default() .critical_frequency(0.01) .gain_db(-20.0) - .shelf_db(20.0) + .shelf_db(10.0) .q(10.) .iho(), &[ (1e-5, Tol::GainDb(40.0, 0.01)), - (0.01, Tol::GainDb(-40.0, 0.05)), - (4.99e-1, Tol::GainDb(0.0, 0.01)), + (0.01, Tol::GainBelowDb(-40.0)), + (4.99e-1, Tol::GainDb(-10.0, 0.01)), ], ); } From f12c61ef51f771a799c48c749570a6c5384fdd77 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 11 Jan 2024 17:43:10 +0100 Subject: [PATCH 18/26] renames, tweak test --- src/iir/coefficients.rs | 56 ++++++++++++++++++++++++----------------- 1 file changed, 33 insertions(+), 23 deletions(-) diff --git a/src/iir/coefficients.rs b/src/iir/coefficients.rs index d5408b8..a0d096e 100644 --- a/src/iir/coefficients.rs +++ b/src/iir/coefficients.rs @@ -371,14 +371,15 @@ where /// Notch, integrating below, flat `shelf_gain` above pub fn iho(&self) -> [T; 6] { let (fcos, alpha) = self.fcos_alpha(); - let a = (T::one() + fcos) / self.shelf; + let fsin = 0.5.as_() * self.w0.sin(); + let a = (T::one() + fcos) / (2.0.as_() * self.shelf); [ - 2.0.as_() * self.gain * (T::one() + alpha), - (-4.0).as_() * self.gain * fcos, - 2.0.as_() * self.gain * (T::one() - alpha), - a + self.w0.sin(), + self.gain * (T::one() + alpha), + (-2.0).as_() * self.gain * fcos, + self.gain * (T::one() - alpha), + a + fsin, (-2.0).as_() * a, - a - self.w0.sin(), + a - fsin, ] } } @@ -400,14 +401,23 @@ mod test { use crate::iir::*; #[test] - fn lowpass_gen() { + #[ignore] + fn lowpass_noise_shaping() { let ba = Biquad::::from( &Filter::default() - .critical_frequency(2e-9f64) - .gain(2e7) + .critical_frequency(1e-5f64) + .gain(1e3) .lowpass(), ); println!("{:?}", ba); + let mut xy = [0; 5]; + for _ in 0..(1 << 24) { + ba.update(&mut xy, 1); + } + for _ in 0..10 { + ba.update(&mut xy, 1); + println!("{xy:?}"); + } } fn polyval(p: &[f64], x: Complex64) -> Complex64 { @@ -439,7 +449,7 @@ mod test { } } - fn check(f: f64, g: Tol, ba: &[f64; 6]) { + fn check_freqz(f: f64, g: Tol, ba: &[f64; 6]) { let h = freqz(&ba[..3], &ba[3..], f); let hp = h.to_polar(); assert!( @@ -448,25 +458,25 @@ mod test { ); } - fn check_coeffs(ba: &[f64; 6], fg: &[(f64, Tol)]) { + fn check_transfer(ba: &[f64; 6], fg: &[(f64, Tol)]) { println!("{ba:?}"); for (f, g) in fg { - check(*f, *g, ba); + check_freqz(*f, *g, ba); } - // Quantize + // Quantize and back let bai = (&Biquad::::from(ba)).into(); println!("{bai:?}"); for (f, g) in fg { - check(*f, *g, &bai); + check_freqz(*f, *g, &bai); } } #[test] fn lowpass() { - check_coeffs( + check_transfer( &Filter::default() .critical_frequency(0.01) .gain_db(20.0) @@ -481,7 +491,7 @@ mod test { #[test] fn highpass() { - check_coeffs( + check_transfer( &Filter::default() .critical_frequency(0.1) .gain_db(-2.0) @@ -496,7 +506,7 @@ mod test { #[test] fn bandpass() { - check_coeffs( + check_transfer( &Filter::default() .critical_frequency(0.02) .bandwidth(2.0) @@ -514,7 +524,7 @@ mod test { #[test] fn allpass() { - check_coeffs( + check_transfer( &Filter::default() .critical_frequency(0.02) .gain_db(-10.0) @@ -531,7 +541,7 @@ mod test { #[test] fn notch() { - check_coeffs( + check_transfer( &Filter::default() .critical_frequency(0.02) .bandwidth(2.0) @@ -548,7 +558,7 @@ mod test { #[test] fn peaking() { - check_coeffs( + check_transfer( &Filter::default() .critical_frequency(0.02) .bandwidth(2.0) @@ -567,7 +577,7 @@ mod test { #[test] fn highshelf() { - check_coeffs( + check_transfer( &Filter::default() .critical_frequency(0.02) .gain_db(-10.0) @@ -584,7 +594,7 @@ mod test { #[test] fn lowshelf() { - check_coeffs( + check_transfer( &Filter::default() .critical_frequency(0.02) .gain_db(-10.0) @@ -601,7 +611,7 @@ mod test { #[test] fn iho() { - check_coeffs( + check_transfer( &Filter::default() .critical_frequency(0.01) .gain_db(-20.0) From 118d55556bd3995b541726a3023b69098146dbb7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 11 Jan 2024 23:34:13 +0100 Subject: [PATCH 19/26] add simple state variable filter --- CHANGELOG.md | 1 + README.md | 5 +++++ src/lib.rs | 1 + src/svf.rs | 35 +++++++++++++++++++++++++++++++++++ 4 files changed, 42 insertions(+) create mode 100644 src/svf.rs diff --git a/CHANGELOG.md b/CHANGELOG.md index 1fb5129..c3c2c61 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,6 +15,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 * `iir::Biquad` getter/setter * `iir`: support for other integers (i8, i16, i128) * `iir::Biquad`: support for reduced DF1 state and DF2T state +* `svf`: state variable filter ### Removed diff --git a/README.md b/README.md index 5a1c16a..ecf2d85 100644 --- a/README.md +++ b/README.md @@ -50,3 +50,8 @@ Fast, infinitely cascadable, first- and second-order lowpass and the correspondi ## FIR filters Fast `f32` symmetric FIR filters, optimized half-band filters, half-band filter decimators and integators and cascades. + +## SVF filter + +Simple IIR state variable filter simultaneously providing highpass, lowpass, +bandpass, and notch filtering of a signal. diff --git a/src/lib.rs b/src/lib.rs index 2d42319..9af5826 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -26,6 +26,7 @@ pub use unwrap::*; pub mod hbf; mod num; pub use num::*; +pub mod svf; #[cfg(test)] pub mod testing; diff --git a/src/svf.rs b/src/svf.rs new file mode 100644 index 0000000..5ca408e --- /dev/null +++ b/src/svf.rs @@ -0,0 +1,35 @@ +use num_traits::{Float, FloatConst}; +use serde::{Deserialize, Serialize}; +pub struct State { + pub lp: T, + pub hp: T, + pub bp: T, +} + +impl State { + pub fn br(&self) -> T { + self.hp + self.lp + } +} + +#[derive(Copy, Clone, Debug, Deserialize, Serialize, PartialEq, PartialOrd)] +pub struct Svf { + f: T, + q: T, +} + +impl Svf { + pub fn set_frequency(&mut self, f0: T) { + self.f = (T::one() + T::one()) * (T::PI() * f0).sin(); + } + + pub fn set_q(&mut self, q: T) { + self.q = T::one()/q; + } + + pub fn update(&self, s: &mut State, x0: T) { + s.lp = s.bp * self.f + s.lp; + s.hp = x0 - s.lp - s.bp * self.q; + s.bp = s.hp * self.f + s.bp; + } +} From d2bd6be52216d0da968a906abb88439d47372239 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 12 Jan 2024 11:18:17 +0100 Subject: [PATCH 20/26] rm shift --- src/num.rs | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/num.rs b/src/num.rs index d36e286..b1e0b03 100644 --- a/src/num.rs +++ b/src/num.rs @@ -2,8 +2,6 @@ use num_traits::{AsPrimitive, Float, Num}; /// Helper trait unifying fixed point and floating point coefficients/samples pub trait FilterNum: 'static + Copy + Num + AsPrimitive { - /// Scale - const SHIFT: u32; /// Multiplicative identity const ONE: Self; /// Negative multiplicative identity, equal to `-Self::ONE`. @@ -40,7 +38,6 @@ pub trait FilterNum: 'static + Copy + Num + AsPrimitive { macro_rules! impl_float { ($T:ty) => { impl FilterNum for $T { - const SHIFT: u32 = 0; const ONE: Self = 1.0; const NEG_ONE: Self = -1.0; const ZERO: Self = 0.0; @@ -82,7 +79,6 @@ impl_float!(f64); macro_rules! impl_int { ($T:ty, $U:ty, $A:ty, $Q:literal) => { impl FilterNum for $T { - const SHIFT: u32 = $Q; const ONE: Self = 1 << $Q; const NEG_ONE: Self = -1 << $Q; const ZERO: Self = 0; From 50576e3efa24dfc9df0a393bf5453b53a9e1ccb3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 12 Jan 2024 14:17:05 +0100 Subject: [PATCH 21/26] docs * include readme as landing page in docs * warn on missing docs, add other pragmas * add missing docs * cleanup docs, modules * remove unused tools/utils --- README.md | 25 ++++++++--------------- src/accu.rs | 2 ++ src/complex.rs | 7 +++++++ src/filter.rs | 10 ++++++++++ src/hbf.rs | 18 ++++++++++++++++- src/iir/mod.rs | 2 ++ src/lib.rs | 9 ++++++--- src/lockin.rs | 3 +++ src/lowpass.rs | 15 ++++++++------ src/num.rs | 4 ++++ src/svf.rs | 21 +++++++++++++++++++- src/tools.rs | 54 -------------------------------------------------- 12 files changed, 88 insertions(+), 82 deletions(-) delete mode 100644 src/tools.rs diff --git a/README.md b/README.md index ecf2d85..fc61986 100644 --- a/README.md +++ b/README.md @@ -1,18 +1,13 @@ # Embedded DSP algorithms [![GitHub release](https://img.shields.io/github/v/release/quartiq/idsp?include_prereleases)](https://github.com/quartiq/idsp/releases) +[![crates.io](https://img.shields.io/crates/v/idsp.svg)](https://crates.io/crates/idsp) [![Documentation](https://img.shields.io/badge/docs-online-success)](https://docs.rs/idsp) [![QUARTIQ Matrix Chat](https://img.shields.io/matrix/quartiq:matrix.org)](https://matrix.to/#/#quartiq:matrix.org) [![Continuous Integration](https://github.com/quartiq/idsp/actions/workflows/ci.yml/badge.svg)](https://github.com/quartiq/idsp/actions/workflows/ci.yml) This crate contains some tuned DSP algorithms for general and especially embedded use. -Many of the algorithms are implemented on integer datatypes for reasons that become important in certain cases: - -* Speed: even with a hard floating point unit integer operations are faster. -* Accuracy: single precision FP has a 24 bit mantissa, `i32` has full 32 bit. -* No rounding errors. -* Natural wrap around (modulo) at the integer overflow: critical for phase/frequency applications. -* Natural definition of "full scale". +Many of the algorithms are implemented on integer (fixed point) datatypes. One comprehensive user for these algorithms is [Stabilizer](https://github.com/quartiq/stabilizer). @@ -24,10 +19,6 @@ This uses a small (128 element or 512 byte) LUT, smart octant (un)mapping, linea This returns a phase given a complex signal (a pair of in-phase/`x`/cosine and quadrature/`y`/sine). The RMS phase error is less than 5e-6 rad, max error is less than 1.2e-5 rad, i.e. 20.5 bit RMS, 19.1 bit max accuracy. The bias is minimal. -## ComplexExt - -An extension trait for the `num::Complex` type featuring especially a `std`-like API to the two functions above. - ## PLL, RPLL High accuracy, zero-assumption, fully robust, forward and reciprocal PLLs with dynamically adjustable time constant and arbitrary (in the Nyquist sampling sense) capture range, and noise shaping. @@ -36,13 +27,18 @@ High accuracy, zero-assumption, fully robust, forward and reciprocal PLLs with d Tools to handle, track, and unwrap phase signals or generate them. -## iir +## IIR/Biquad Fixed point (`i8`, `i16`, `i32`, `i64`) and floating point (`f32`, `f64`) biquad IIR filters. Robust and clean clipping and offset (anti-windup, no derivative kick, dynamically adjustable gains) suitable for PID controller applications. Direct Form 1 and Direct Form 2 Transposed supported. Coefficient sharing for multiple channels. +## State variable filter + +Simple IIR state variable filter simultaneously providing highpass, lowpass, +bandpass, and notch filtering of a signal. + ## Lowpass, Lockin Fast, infinitely cascadable, first- and second-order lowpass and the corresponding integration into a lockin amplifier algorithm. @@ -50,8 +46,3 @@ Fast, infinitely cascadable, first- and second-order lowpass and the correspondi ## FIR filters Fast `f32` symmetric FIR filters, optimized half-band filters, half-band filter decimators and integators and cascades. - -## SVF filter - -Simple IIR state variable filter simultaneously providing highpass, lowpass, -bandpass, and notch filtering of a signal. diff --git a/src/accu.rs b/src/accu.rs index 343361d..6bf2d91 100644 --- a/src/accu.rs +++ b/src/accu.rs @@ -1,5 +1,6 @@ use num_traits::ops::wrapping::WrappingAdd; +/// Wrapping Accumulator #[derive(Copy, Clone, Default, PartialEq, Eq, Debug)] pub struct Accu { state: T, @@ -7,6 +8,7 @@ pub struct Accu { } impl Accu { + /// Create a new accumulator with given initial state and step. pub fn new(state: T, step: T) -> Self { Self { state, step } } diff --git a/src/complex.rs b/src/complex.rs index 91ba4f4..27915b4 100644 --- a/src/complex.rs +++ b/src/complex.rs @@ -4,11 +4,17 @@ use super::{atan2, cossin}; /// Complex extension trait offering DSP (fast, good accuracy) functionality. pub trait ComplexExt { + /// Unit magnitude from angle fn from_angle(angle: T) -> Self; + /// Square of magnitude fn abs_sqr(&self) -> U; + /// Log2 approximation fn log2(&self) -> T; + /// Angle fn arg(&self) -> T; + /// Staturating addition fn saturating_add(&self, other: Self) -> Self; + /// Saturating subtraction fn saturating_sub(&self, other: Self) -> Self; } @@ -97,6 +103,7 @@ impl ComplexExt for Complex { /// Full scale fixed point multiplication. pub trait MulScaled { + /// Scaled multiplication for fixed point fn mul_scaled(self, other: T) -> Self; } diff --git a/src/filter.rs b/src/filter.rs index db1a85b..0433cc5 100644 --- a/src/filter.rs +++ b/src/filter.rs @@ -1,4 +1,9 @@ +/// Single inpout single output i32 filter pub trait Filter { + /// Filter configuration type. + /// + /// While the filter struct owns the state, + /// the configuration is decoupled to allow sharing. type Config; /// Update the filter with a new sample. /// @@ -16,6 +21,9 @@ pub trait Filter { fn set(&mut self, x: i32); } +/// Nyquist zero +/// +/// Filter with a flat transfer function and a transfer function zero at Nyquist. #[derive(Copy, Clone, Default)] pub struct Nyquist(i32); impl Filter for Nyquist { @@ -34,6 +42,7 @@ impl Filter for Nyquist { } } +/// Repeat another filter #[derive(Copy, Clone)] pub struct Repeat([T; N]); impl Filter for Repeat { @@ -54,6 +63,7 @@ impl Default for Repeat { } } +/// Combine two different filters in cascade #[derive(Copy, Clone, Default)] pub struct Cascade(T, U); impl Filter for Cascade { diff --git a/src/hbf.rs b/src/hbf.rs index 757837f..ad404b5 100644 --- a/src/hbf.rs +++ b/src/hbf.rs @@ -1,3 +1,7 @@ +//! Half-band filters and cascades +//! +//! Used to perform very efficient high-dynamic range rate changes by powers of two. + use core::{ iter::Sum, ops::{Add, Mul}, @@ -457,12 +461,18 @@ impl Default for HbfDecCascade { } impl HbfDecCascade { + /// Set cascade depth + /// + /// Sets the number of HBF filter stages to apply. #[inline] pub fn set_depth(&mut self, n: usize) { assert!(n <= 4); self.depth = n; } + /// Cascade depth + /// + /// The number of HBF filter stages to apply. #[inline] pub fn depth(&self) -> usize { self.depth @@ -543,7 +553,7 @@ impl Filter for HbfDecCascade { #[derive(Copy, Clone, Debug)] pub struct HbfIntCascade { depth: usize, - pub stages: ( + stages: ( HbfInt< 'static, f32, @@ -586,11 +596,17 @@ impl Default for HbfIntCascade { } impl HbfIntCascade { + /// Set cascade depth + /// + /// Sets the number of HBF filter stages to apply. pub fn set_depth(&mut self, n: usize) { assert!(n <= 4); self.depth = n; } + /// Cascade depth + /// + /// The number of HBF filter stages to apply. pub fn depth(&self) -> usize { self.depth } diff --git a/src/iir/mod.rs b/src/iir/mod.rs index 20713fb..700369d 100644 --- a/src/iir/mod.rs +++ b/src/iir/mod.rs @@ -1,3 +1,5 @@ +//! IIR filters, coefficients and applications + mod biquad; pub use biquad::*; mod coefficients; diff --git a/src/lib.rs b/src/lib.rs index 9af5826..78a72a7 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,7 +1,10 @@ -#![cfg_attr(not(test), no_std)] +#![cfg_attr(not(any(test, doctest, feature = "std")), no_std)] +#![doc = include_str!("../README.md")] +#![deny(rust_2018_compatibility)] +#![deny(rust_2018_idioms)] +#![warn(missing_docs)] +#![forbid(unsafe_code)] -mod tools; -pub use tools::*; mod atan2; pub use atan2::*; mod accu; diff --git a/src/lockin.rs b/src/lockin.rs index e81abc7..0ca8669 100644 --- a/src/lockin.rs +++ b/src/lockin.rs @@ -1,5 +1,8 @@ use super::{Complex, ComplexExt, Filter, MulScaled}; +/// Lockin filter +/// +/// Combines two [`Filter`] and an NCO to perform demodulation #[derive(Copy, Clone, Default)] pub struct Lockin { state: [T; 2], diff --git a/src/lowpass.rs b/src/lowpass.rs index 3f1bb7f..03ca250 100644 --- a/src/lowpass.rs +++ b/src/lowpass.rs @@ -7,7 +7,6 @@ use crate::Filter; /// /// The filter will cleanly saturate towards the `i32` range. /// -/// /// Both filters have been optimized for accuracy, dynamic range, and /// speed on Cortex-M7. #[derive(Copy, Clone)] @@ -33,9 +32,13 @@ impl Filter for Lowpass { /// `1 << 16 <= k <= q*(1 << 31)`. type Config = [i32; N]; fn update(&mut self, x: i32, k: &Self::Config) -> i32 { - let mut d = x.saturating_sub((self.0[0] >> 32) as i32) as i64 * k[0] as i64; + let mut d = x.saturating_sub(self.get()) as i64 * k[0] as i64; let y; - if N >= 2 { + if N == 1 { + self.0[0] += d; + y = self.get(); + self.0[0] += d; + } else if N == 2 { d += (self.0[1] >> 32) * k[1] as i64; self.0[1] += d; self.0[0] += self.0[1]; @@ -47,15 +50,15 @@ impl Filter for Lowpass { self.0[0] += self.0[1]; self.0[1] += d; } else { - self.0[0] += d; - y = self.get(); - self.0[0] += d; + unimplemented!() } y } + fn get(&self) -> i32 { (self.0[0] >> 32) as i32 } + fn set(&mut self, x: i32) { self.0[0] = (x as i64) << 32; } diff --git a/src/num.rs b/src/num.rs index b1e0b03..0c94963 100644 --- a/src/num.rs +++ b/src/num.rs @@ -20,6 +20,9 @@ pub trait FilterNum: 'static + Copy + Num + AsPrimitive { /// Undefined result if `max < min`. fn macc(self, s: Self::ACCU, min: Self, max: Self, e1: Self) -> (Self, Self); + /// Clamp to between min and max + /// + /// Undefined if `min > max`. fn clip(self, min: Self, max: Self) -> Self; /// Multiplication (scaled) @@ -33,6 +36,7 @@ pub trait FilterNum: 'static + Copy + Num + AsPrimitive { where Self: AsPrimitive, C: Float + AsPrimitive; + // TODO: range check and Result } macro_rules! impl_float { diff --git a/src/svf.rs b/src/svf.rs index 5ca408e..817d4cd 100644 --- a/src/svf.rs +++ b/src/svf.rs @@ -1,17 +1,28 @@ +//! State variable filter + use num_traits::{Float, FloatConst}; use serde::{Deserialize, Serialize}; + +/// Second order state variable filter state pub struct State { + /// Lowpass output pub lp: T, + /// Highpass output pub hp: T, + /// Bandpass output pub bp: T, } impl State { + /// Bandreject (notch) output pub fn br(&self) -> T { self.hp + self.lp } } +/// State variable filter +/// +/// #[derive(Copy, Clone, Debug, Deserialize, Serialize, PartialEq, PartialOrd)] pub struct Svf { f: T, @@ -19,14 +30,22 @@ pub struct Svf { } impl Svf { + /// Set the critical frequency + /// + /// In units of the sample frequency. pub fn set_frequency(&mut self, f0: T) { self.f = (T::one() + T::one()) * (T::PI() * f0).sin(); } + /// Set the Q parameter pub fn set_q(&mut self, q: T) { - self.q = T::one()/q; + self.q = T::one() / q; } + /// Update the filter + /// + /// Ingest an input sample and update state correspondingly. + /// Selected output(s) are available from [`State`]. pub fn update(&self, s: &mut State, x0: T) { s.lp = s.bp * self.f + s.lp; s.hp = x0 - s.lp - s.bp * self.q; diff --git a/src/tools.rs b/src/tools.rs deleted file mode 100644 index 90a9627..0000000 --- a/src/tools.rs +++ /dev/null @@ -1,54 +0,0 @@ -use core::ops::{Add, Mul, Neg}; -use num_traits::Zero; - -pub fn abs(x: T) -> T -where - T: PartialOrd + Zero + Neg, -{ - if x >= T::zero() { - x - } else { - -x - } -} - -// These are implemented here because core::f32 doesn't have them (yet). -// They are naive and don't handle inf/nan. -// `compiler-intrinsics`/llvm should have better (robust, universal, and -// faster) implementations. - -pub fn copysign(x: T, y: T) -> T -where - T: PartialOrd + Zero + Neg, -{ - if (x >= T::zero() && y >= T::zero()) || (x <= T::zero() && y <= T::zero()) { - x - } else { - -x - } -} - -// Multiply-accumulate vectors `x` and `a`. -// -// A.k.a. dot product. -// Rust/LLVM optimize this nicely. -pub fn macc(y0: T, x: &[T], a: &[T]) -> T -where - T: Add + Mul + Copy, -{ - x.iter() - .zip(a) - .map(|(x, a)| *x * *a) - .fold(y0, |y, xa| y + xa) -} - -pub fn macc_i32(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 { - // Rounding bias, half up - let y0 = ((y0 as i64) << shift) + (1 << (shift - 1)); - let y = x - .iter() - .zip(a) - .map(|(x, a)| *x as i64 * *a as i64) - .fold(y0, |y, xa| y + xa); - (y >> shift) as i32 -} From 653f1790a782af9281f87b1375a1a56702e51a90 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 12 Jan 2024 14:45:52 +0100 Subject: [PATCH 22/26] try iai-callgrind --- Cargo.toml | 1 + benches/micro.rs | 106 +++++------------------------------------------ 2 files changed, 12 insertions(+), 95 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 17c1f69..9bb5cd3 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -20,6 +20,7 @@ easybench = "1.0" rand = "0.8" ndarray = "0.15" rustfft = "6.1.0" +iai-callgrind = "0.10.0" # futuredsp = "0.0.6" # sdr = "0.7.0" diff --git a/benches/micro.rs b/benches/micro.rs index 74a5b7f..116b3a2 100644 --- a/benches/micro.rs +++ b/benches/micro.rs @@ -1,103 +1,19 @@ use core::f32::consts::PI; +use core::hint::black_box; -use easybench::bench_env; +use iai_callgrind::{library_benchmark, library_benchmark_group, main}; use idsp::{atan2, cossin, iir, Filter, Lowpass, PLL, RPLL}; -fn atan2_bench() { - let xi = (10 << 16) as i32; - let xf = xi as f32 / i32::MAX as f32; - - let yi = (-26_328 << 16) as i32; - let yf = yi as f32 / i32::MAX as f32; - - println!( - "atan2(yi, xi): {}", - bench_env((yi, xi), |(yi, xi)| atan2(*yi, *xi)) - ); - println!( - "yf.atan2(xf): {}", - bench_env((yf, xf), |(yf, xf)| yf.atan2(*xf)) - ); -} - -fn cossin_bench() { - let zi = -0x7304_2531_i32; - let zf = zi as f32 / i32::MAX as f32 * PI; - println!("cossin(zi): {}", bench_env(zi, |zi| cossin(*zi))); - println!("zf.sin_cos(): {}", bench_env(zf, |zf| zf.sin_cos())); -} - -fn rpll_bench() { - let mut dut = RPLL::new(8); - println!( - "RPLL::update(Some(t), 21, 20): {}", - bench_env(Some(0x241), |x| dut.update(*x, 21, 20)) - ); - println!( - "RPLL::update(Some(t), sf, sp): {}", - bench_env((Some(0x241), 21, 20), |(x, p, q)| dut.update(*x, *p, *q)) - ); -} - -fn pll_bench() { - let mut dut = PLL::default(); - println!( - "PLL::update(Some(t), 12, 12): {}", - bench_env(Some(0x241), |x| dut.update(*x, 12)) - ); - println!( - "PLL::update(Some(t), sf, sp): {}", - bench_env((Some(0x241), 21), |(x, p)| dut.update(*x, *p)) - ); +#[library_benchmark] +#[bench::some(-0x7304_2531_i32)] +fn bench_cossin(zi: i32) { + black_box(cossin(zi)); } -fn iir_int_bench() { - let dut = iir::Biquad::default(); - let mut xy = [0; 4]; - println!( - "int_iir::IIR::update(s, x): {}", - bench_env(0x2832, |x| dut.update(&mut xy, *x)) - ); -} - -fn iir_f32_bench() { - let dut = iir::Biquad::::default(); - let mut xy = [0.0; 4]; - println!( - "int::IIR::::update(s, x): {}", - bench_env(0.32241, |x| dut.update(&mut xy, *x)) - ); -} - -fn iir_f64_bench() { - let dut = iir::Biquad::::default(); - let mut xy = [0.0; 4]; - println!( - "int::IIR::::update(s, x): {}", - bench_env(0.32241, |x| dut.update(&mut xy, *x)) - ); -} - -fn lowpass_bench() { - let mut dut = Lowpass::<1>::default(); - println!( - "Lowpass::<1>::update(x, k): {}", - bench_env((0x32421, 14), |(x, k)| dut.update(*x, &[*k])) - ); - println!( - "Lowpass::<1>::update(x, 14): {}", - bench_env(0x32421, |x| dut.update(*x, &[14])) - ); -} +library_benchmark_group!( + name = bench_cossin_group; + benchmarks = bench_cossin +); -fn main() { - atan2_bench(); - cossin_bench(); - rpll_bench(); - pll_bench(); - iir_int_bench(); - iir_f32_bench(); - iir_f64_bench(); - lowpass_bench(); -} +main!(library_benchmark_groups = bench_cossin_group); From 95e1ba918c2d6bd54aedab4e2e6e32518db04cc7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 12 Jan 2024 14:58:47 +0100 Subject: [PATCH 23/26] remove benches, reduce dependencies --- Cargo.toml | 7 ------- benches/micro.rs | 19 ------------------- src/rpll.rs | 11 ++++------- 3 files changed, 4 insertions(+), 33 deletions(-) delete mode 100644 benches/micro.rs diff --git a/Cargo.toml b/Cargo.toml index 9bb5cd3..ae150bf 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -16,17 +16,10 @@ num-complex = { version = "0.4.0", features = ["serde"], default-features = fals num-traits = { version = "0.2.14", features = ["libm"], default-features = false} [dev-dependencies] -easybench = "1.0" rand = "0.8" -ndarray = "0.15" rustfft = "6.1.0" -iai-callgrind = "0.10.0" # futuredsp = "0.0.6" # sdr = "0.7.0" -[[bench]] -name = "micro" -harness = false - [profile.release] debug = 1 diff --git a/benches/micro.rs b/benches/micro.rs deleted file mode 100644 index 116b3a2..0000000 --- a/benches/micro.rs +++ /dev/null @@ -1,19 +0,0 @@ -use core::f32::consts::PI; -use core::hint::black_box; - -use iai_callgrind::{library_benchmark, library_benchmark_group, main}; - -use idsp::{atan2, cossin, iir, Filter, Lowpass, PLL, RPLL}; - -#[library_benchmark] -#[bench::some(-0x7304_2531_i32)] -fn bench_cossin(zi: i32) { - black_box(cossin(zi)); -} - -library_benchmark_group!( - name = bench_cossin_group; - benchmarks = bench_cossin -); - -main!(library_benchmark_groups = bench_cossin_group); diff --git a/src/rpll.rs b/src/rpll.rs index e307d96..0bf0b0b 100644 --- a/src/rpll.rs +++ b/src/rpll.rs @@ -93,7 +93,6 @@ impl RPLL { #[cfg(test)] mod test { use super::RPLL; - use ndarray::prelude::*; use rand::{prelude::*, rngs::StdRng}; use std::vec::Vec; @@ -175,14 +174,12 @@ mod test { self.run(t_settle); let (y, f) = self.run(n); - let y = Array::from(y); - let f = Array::from(f); // println!("{:?} {:?}", f, y); - let fm = f.mean().unwrap(); - let fs = f.std_axis(Axis(0), 0.).into_scalar(); - let ym = y.mean().unwrap(); - let ys = y.std_axis(Axis(0), 0.).into_scalar(); + let fm = f.iter().copied().sum::() / f.len() as f32; + let fs = f.iter().map(|f| (*f - fm).powi(2)).sum::().sqrt() / f.len() as f32; + let ym = y.iter().copied().sum::() / y.len() as f32; + let ys = y.iter().map(|y| (*y - ym).powi(2)).sum::().sqrt() / y.len() as f32; println!("f: {:.2e}±{:.2e}; y: {:.2e}±{:.2e}", fm, fs, ym, ys); From 6787d0a7f17f2879be184e157b0c45471dcff9f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 12 Jan 2024 17:46:27 +0100 Subject: [PATCH 24/26] docs --- README.md | 45 ++++++++++++---- src/iir/biquad.rs | 128 +++++++++++++++++++++++++++++----------------- src/iir/pid.rs | 4 +- src/num.rs | 6 +-- 4 files changed, 121 insertions(+), 62 deletions(-) diff --git a/README.md b/README.md index fc61986..1988c93 100644 --- a/README.md +++ b/README.md @@ -11,38 +11,63 @@ Many of the algorithms are implemented on integer (fixed point) datatypes. One comprehensive user for these algorithms is [Stabilizer](https://github.com/quartiq/stabilizer). -## Cosine/Sine `cossin` +## Fixed point + +### Cosine/Sine [`cossin()`] This uses a small (128 element or 512 byte) LUT, smart octant (un)mapping, linear interpolation and comprehensive analysis of corner cases to achieve a very clean signal (4e-6 RMS error, 9e-6 max error, 108 dB SNR typ), low spurs, and no bias with about 40 cortex-m instruction per call. It computes both cosine and sine (i.e. the complex signal) at once given a phase input. -## Two-argument arcus-tangens `atan2` +### Two-argument arcus-tangens [`atan2()`] This returns a phase given a complex signal (a pair of in-phase/`x`/cosine and quadrature/`y`/sine). The RMS phase error is less than 5e-6 rad, max error is less than 1.2e-5 rad, i.e. 20.5 bit RMS, 19.1 bit max accuracy. The bias is minimal. -## PLL, RPLL +## [`PLL`], [`RPLL`] High accuracy, zero-assumption, fully robust, forward and reciprocal PLLs with dynamically adjustable time constant and arbitrary (in the Nyquist sampling sense) capture range, and noise shaping. -## Unwrapper, Accu, saturating_scale +## [`Unwrapper`], [`Accu`], [`saturating_scale()`] Tools to handle, track, and unwrap phase signals or generate them. -## IIR/Biquad +## Float and Fixed point + +## [`iir`]/[`iir::Biquad`] Fixed point (`i8`, `i16`, `i32`, `i64`) and floating point (`f32`, `f64`) biquad IIR filters. -Robust and clean clipping and offset (anti-windup, no derivative kick, dynamically adjustable gains) suitable for PID controller applications. -Direct Form 1 and Direct Form 2 Transposed supported. +Robust and clean clipping and offset (anti-windup, no derivative kick, dynamically adjustable gains and gain limits) suitable for PID controller applications. +Three kinds of filter actions: Direct Form 1, Direct Form 2 Transposed, and Direct Form 1 with noise shaping supported. Coefficient sharing for multiple channels. -## State variable filter +Compared to [`biquad-rs`](https://crates.io/crates/biquad) this crates adds several additional important features: + +* fixed point implementations (`i32`, `i64`, etc.) in addition to `f32`/`f64` floating point +* additional [`iir::Filter`] builders (I/HO) +* decoupled [`iir::Biquad`] configuration and flat `[T; N]` state +* [`iir::Pid`] builder +* DF1 noise shaping for fixed point +* proper output limiting/clamping before feedback ("anti-windup") +* summing junction offset (for PID controller applications) + +Compared to [`fixed-filters`](https://crates.io/crates/fixed-filters) this crate: + +* Supports unified floating point and fixed point API +* decoupled [`iir::Biquad`] configuration and flat `[T; N]` state +* [`iir::Pid`] builder +* additional [`iir::Filter`] builders (I/HO) +* noise shaping for fixed point +* summing junction offset (for PID controller applications) + +## [`svf`] State variable filter Simple IIR state variable filter simultaneously providing highpass, lowpass, bandpass, and notch filtering of a signal. -## Lowpass, Lockin +## [`Lowpass`], [`Lockin`] Fast, infinitely cascadable, first- and second-order lowpass and the corresponding integration into a lockin amplifier algorithm. -## FIR filters +## FIR filters: [`hbf::HbfDec`], [`hbf::HbfInt`], [`hbf::HbfDecCascade`], [`hbf::HbfIntCascade`] Fast `f32` symmetric FIR filters, optimized half-band filters, half-band filter decimators and integators and cascades. +These are used in [`stabilizer-stream`](https://github.com/quartiq/stabilizer-stream) for online PSD calculation on log +frequency scale for arbitrarily large amounts of data. diff --git a/src/iir/biquad.rs b/src/iir/biquad.rs index 4b64447..95de077 100644 --- a/src/iir/biquad.rs +++ b/src/iir/biquad.rs @@ -1,51 +1,85 @@ use num_traits::{AsPrimitive, Float}; use serde::{Deserialize, Serialize}; -use crate::FilterNum; +use crate::Coefficient; -/// Filter architecture +/// Biquad IIR filter +/// +/// A biquadratic IIR filter supports up to two zeros and two poles in the transfer function. +/// It can be used to implement a wide range of responses to input signals. +/// +/// The Biquad performs the following operation to compute a new output sample `y0` from a new +/// input sample `x0` given its configuration and previous samples: +/// +/// `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` +/// +/// This implementation here saves storage and improves caching opportunities by decoupling +/// filter configuration (coefficients, limits and offset) from filter state +/// and thus supports both (a) sharing a single filter between multiple states ("channels") and (b) +/// rapid switching of filters (tuning, transfer) for a given state without copying either +/// state of configuration. +/// +/// # Filter architecture /// /// Direct Form 1 (DF1) and Direct Form 2 transposed (DF2T) are the only IIR filter /// structures with an (effective bin the case of TDF2) single summing junction /// this allows clamping of the output before feedback. /// -/// DF1 allows atomic coefficient change because only x/y are pipelined. +/// DF1 allows atomic coefficient change because only inputs and outputs are pipelined. /// The summing junctuion pipelining of TDF2 would require incremental /// coefficient changes and is thus less amenable to online tuning. /// -/// DF2T needs less state storage (2 instead of 4). This is in addition to the coefficient storage -/// (5 plus 2 limits plus 1 offset) -/// This implementation already saves storage by decoupling coefficients/limits and offset from state -/// and thus supports both (a) sharing a single filter between multiple states ("channels") and (b) -/// rapid switching of filters (tuning, transfer) for a given state without copying. +/// DF2T needs less state storage (2 instead of 4). This is in addition to the coefficient +/// storage (5 plus 2 limits plus 1 offset) /// /// DF2T is less efficient and accurate for fixed-point architectures as quantization /// happens at each intermediate summing junction in addition to the output quantization. This is /// especially true for common `i64 + i32 * i32 -> i64` MACC architectures. +/// One could use wide state storage for fixed point DF2T but that would negate the storage +/// and processing advantages. +/// +/// # Coefficients +/// +/// `ba: [T; 5] = [b0, b1, b2, a1, a2]` is the coefficients type. +/// To represent the IIR coefficients, this contains the feed-forward +/// coefficients `b0, b1, b2` followed by the feed-back coefficients +/// `a1, a2`, all five normalized such that `a0 = 1`. +/// +/// The summing junction of the filter also receives an offset `u`. +/// +/// The filter applies clamping such that `min <= y <= max`. +/// +/// See [`crate::iir::Filter`] and [`crate::iir::Pid`] for ways to generate coefficients. +/// +/// # Fixed point /// -/// # Coefficients and state +/// Coefficient scaling (see [`Coefficient`]) is fixed and optimized such that -2 is exactly +/// representable. This is tailored to low-passes, PID, II etc, where the integration rule is +/// [1, -2, 1]. /// -/// `[T; 5]` is the coefficients type. +/// There are two guard bits in the accumulator before clamping/limiting. +/// While this isn't enough to cover the worst case accumulator, it does catch many real world +/// overflow cases. +/// +/// # State /// /// To represent the IIR state (input and output memory) during [`Biquad::update()`] -/// this contains the two previous inputs and output `[x1, x2, y1, y2]` +/// the DF1 state contains the two previous inputs and output `[x1, x2, y1, y2]` /// concatenated. Lower indices correspond to more recent samples. -/// To represent the IIR coefficients, this contains the feed-forward -/// coefficients `[b0, b1, b2]` followd by the negated feed-back coefficients -/// `[a1, a2]`, all five normalized such that `a0 = 1`. -/// Note that between filter [`Biquad::update()`] the `xy` state contains -/// `[x0, x1, y0, y1]`. /// -/// The IIR coefficients can be mapped to other transfer function -/// representations, for example as described in +/// In the DF2T case the state contains `[b1*x1 + b2*x2 - a1*y1 - a2*y2, b2*x1 - a2*y1]` /// -/// # IIR filter as PID controller +/// In the DF1 case with first order noise shaping, the state contains `[x1, x2, y1, y2, e1]` +/// where `e0` is the accumulated quantization error. /// -/// Contains the coeeficients `ba`, the summing junction offset `u`, and the -/// output limits `min` and `max`. Data is represented in floating-point -/// for all internal signals, input and output. +/// # PID controller /// -/// This implementation achieves several important properties: +/// The IIR coefficients can be mapped to other transfer function +/// representations, for example PID controllers as described in +/// and +/// . +/// +/// Using a Biquad as a template for a PID controller achieves several important properties: /// /// * Its transfer function is universal in the sense that any biquadratic /// transfer function can be implemented (high-passes, gain limits, second @@ -66,16 +100,6 @@ use crate::FilterNum; /// coefficients/offset sets. /// * Cascading multiple IIR filters allows stable and robust /// implementation of transfer functions beyond bequadratic terms. -/// -/// See also . -/// -/// -/// Offset and limiting disabled to suit lowpass applications. -/// Coefficient scaling fixed and optimized such that -2 is representable. -/// Tailored to low-passes, PID, II etc, where the integration rule is [1, -2, 1]. -/// Since the relevant coefficients `a1` and `a2` are negated, we also negate the -/// stored `y1` and `y2` in the state. -/// Note that `xy` contains the negative `y1` and `y2`, such that `-a1` #[derive(Copy, Clone, Debug, Deserialize, Serialize, PartialEq, PartialOrd)] pub struct Biquad { ba: [T; 5], @@ -84,7 +108,7 @@ pub struct Biquad { max: T, } -impl Default for Biquad { +impl Default for Biquad { fn default() -> Self { Self { ba: [T::ZERO; 5], @@ -95,7 +119,7 @@ impl Default for Biquad { } } -impl From<[T; 5]> for Biquad { +impl From<[T; 5]> for Biquad { fn from(ba: [T; 5]) -> Self { Self { ba, @@ -106,7 +130,7 @@ impl From<[T; 5]> for Biquad { impl From<&[C; 6]> for Biquad where - T: FilterNum + AsPrimitive, + T: Coefficient + AsPrimitive, C: Float + AsPrimitive, { fn from(ba: &[C; 6]) -> Self { @@ -124,7 +148,7 @@ where impl From<&Biquad> for [C; 6] where - T: FilterNum + AsPrimitive, + T: Coefficient + AsPrimitive, C: 'static + Copy, { fn from(value: &Biquad) -> Self { @@ -140,7 +164,7 @@ where } } -impl Biquad { +impl Biquad { /// A "hold" filter that ingests input and maintains output /// /// ``` @@ -193,7 +217,7 @@ impl Biquad { /// `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)`. /// /// ``` - /// # use idsp::FilterNum; + /// # use idsp::Coefficient; /// # use idsp::iir::*; /// assert_eq!(Biquad::::IDENTITY.ba()[0], i32::ONE); /// assert_eq!(Biquad::::HOLD.ba()[3], -i32::ONE); @@ -207,7 +231,7 @@ impl Biquad { /// See [`Biquad::ba()`]. /// /// ``` - /// # use idsp::FilterNum; + /// # use idsp::Coefficient; /// # use idsp::iir::*; /// let mut i = Biquad::default(); /// i.ba_mut()[0] = i32::ONE; @@ -321,7 +345,7 @@ impl Biquad { /// Compute input-referred (`x`) offset. /// /// ``` - /// # use idsp::FilterNum; + /// # use idsp::Coefficient; /// # use idsp::iir::*; /// let mut i = Biquad::proportional(3); /// i.set_u(3); @@ -349,7 +373,7 @@ impl Biquad { /// ``` /// /// ``` - /// # use idsp::FilterNum; + /// # use idsp::Coefficient; /// # use idsp::iir::*; /// let mut i = Biquad::proportional(-i32::ONE); /// i.set_input_offset(1); @@ -384,18 +408,28 @@ impl Biquad { /// /// ## `N=5` Direct Form 1 with first order noise shaping /// + /// ``` + /// # use idsp::iir::*; + /// let mut xy = [1, 2, 3, 4, 5]; + /// let x0 = 6; + /// let y0 = Biquad::IDENTITY.update(&mut xy, x0); + /// assert_eq!(y0, x0); + /// assert_eq!(xy, [x0, 1, y0, 3, 5]); + /// ``` + /// /// `xy` contains: /// * On entry: `[x1, x2, y1, y2, e1]` /// * On exit: `[x0, x1, y0, y1, e0]` /// - /// Note: This isn't useful for floating point. + /// Note: This is only useful for fixed point filters. /// /// ## `N=2` Direct Form 2 transposed /// - /// Note: Don't use this for fixed point. Quantization happens at each state store operation. - /// Ideally the state would be `T::ACCU` but then for fixed point it would use equal amount - /// of storage compared to DF1 for little to no gain in performance and none in functionality. - /// There are also no guard bits. + /// Note: This is only useful for floating point filters. + /// Don't use this for fixed point: Quantization happens at each state store operation. + /// Ideally the state would be `[T::ACCU; 2]` but then for fixed point it would use equal amount + /// of storage compared to DF1 for no gain in performance and loss in functionality. + /// There are also no guard bits here. /// /// `xy` contains: /// * On entry: `[b1*x1 + b2*x2 - a1*y1 - a2*y2, b2*x1 - a2*y1]` diff --git a/src/iir/pid.rs b/src/iir/pid.rs index c0159ce..388ec09 100644 --- a/src/iir/pid.rs +++ b/src/iir/pid.rs @@ -1,7 +1,7 @@ use num_traits::{AsPrimitive, Float}; use serde::{Deserialize, Serialize}; -use crate::FilterNum; +use crate::Coefficient; /// PID controller builder /// @@ -156,7 +156,7 @@ impl Pid { /// /// # Panic /// Will panic in debug mode on fixed point coefficient overflow. - pub fn build>(&self) -> Result<[C; 5], PidError> + pub fn build>(&self) -> Result<[C; 5], PidError> where T: AsPrimitive, { diff --git a/src/num.rs b/src/num.rs index 0c94963..4ab24d5 100644 --- a/src/num.rs +++ b/src/num.rs @@ -1,7 +1,7 @@ use num_traits::{AsPrimitive, Float, Num}; /// Helper trait unifying fixed point and floating point coefficients/samples -pub trait FilterNum: 'static + Copy + Num + AsPrimitive { +pub trait Coefficient: 'static + Copy + Num + AsPrimitive { /// Multiplicative identity const ONE: Self; /// Negative multiplicative identity, equal to `-Self::ONE`. @@ -41,7 +41,7 @@ pub trait FilterNum: 'static + Copy + Num + AsPrimitive { macro_rules! impl_float { ($T:ty) => { - impl FilterNum for $T { + impl Coefficient for $T { const ONE: Self = 1.0; const NEG_ONE: Self = -1.0; const ZERO: Self = 0.0; @@ -82,7 +82,7 @@ impl_float!(f64); macro_rules! impl_int { ($T:ty, $U:ty, $A:ty, $Q:literal) => { - impl FilterNum for $T { + impl Coefficient for $T { const ONE: Self = 1 << $Q; const NEG_ONE: Self = -1 << $Q; const ZERO: Self = 0; From cbf656b6124387bdb72134810af8d20025c9a846 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 12 Jan 2024 17:58:54 +0100 Subject: [PATCH 25/26] doc formatting --- README.md | 30 ++++++++++++++++-------------- src/iir/biquad.rs | 10 +++++----- 2 files changed, 21 insertions(+), 19 deletions(-) diff --git a/README.md b/README.md index 1988c93..30b8ecd 100644 --- a/README.md +++ b/README.md @@ -13,27 +13,28 @@ One comprehensive user for these algorithms is [Stabilizer](https://github.com/q ## Fixed point -### Cosine/Sine [`cossin()`] +### Cosine/Sine -This uses a small (128 element or 512 byte) LUT, smart octant (un)mapping, linear interpolation and comprehensive analysis of corner cases to achieve a very clean signal (4e-6 RMS error, 9e-6 max error, 108 dB SNR typ), low spurs, and no bias with about 40 cortex-m instruction per call. It computes both cosine and sine (i.e. the complex signal) at once given a phase input. +[`cossin()`] uses a small (128 element or 512 byte) LUT, smart octant (un)mapping, linear interpolation and comprehensive analysis of corner cases to achieve a very clean signal (4e-6 RMS error, 9e-6 max error, 108 dB SNR typ), low spurs, and no bias with about 40 cortex-m instruction per call. It computes both cosine and sine (i.e. the complex signal) at once given a phase input. -### Two-argument arcus-tangens [`atan2()`] +### Two-argument arcus-tangens -This returns a phase given a complex signal (a pair of in-phase/`x`/cosine and quadrature/`y`/sine). The RMS phase error is less than 5e-6 rad, max error is less than 1.2e-5 rad, i.e. 20.5 bit RMS, 19.1 bit max accuracy. The bias is minimal. +[`atan2()`] returns a phase given a complex signal (a pair of in-phase/`x`/cosine and quadrature/`y`/sine). The RMS phase error is less than 5e-6 rad, max error is less than 1.2e-5 rad, i.e. 20.5 bit RMS, 19.1 bit max accuracy. The bias is minimal. -## [`PLL`], [`RPLL`] +## PLL, RPLL -High accuracy, zero-assumption, fully robust, forward and reciprocal PLLs with dynamically adjustable time constant and arbitrary (in the Nyquist sampling sense) capture range, and noise shaping. +[`PLL`], [`RPLL`]: High accuracy, zero-assumption, fully robust, forward and reciprocal PLLs with dynamically adjustable time constant and arbitrary (in the Nyquist sampling sense) capture range, and noise shaping. -## [`Unwrapper`], [`Accu`], [`saturating_scale()`] +## `Unwrapper`, `Accu`, `saturating_scale()` +[`Unwrapper`], [`Accu`], [`saturating_scale()`]: Tools to handle, track, and unwrap phase signals or generate them. ## Float and Fixed point -## [`iir`]/[`iir::Biquad`] +## IIR/Biquad -Fixed point (`i8`, `i16`, `i32`, `i64`) and floating point (`f32`, `f64`) biquad IIR filters. +[`iir::Biquad`] are fixed point (`i8`, `i16`, `i32`, `i64`) and floating point (`f32`, `f64`) biquad IIR filters. Robust and clean clipping and offset (anti-windup, no derivative kick, dynamically adjustable gains and gain limits) suitable for PID controller applications. Three kinds of filter actions: Direct Form 1, Direct Form 2 Transposed, and Direct Form 1 with noise shaping supported. Coefficient sharing for multiple channels. @@ -57,17 +58,18 @@ Compared to [`fixed-filters`](https://crates.io/crates/fixed-filters) this crate * noise shaping for fixed point * summing junction offset (for PID controller applications) -## [`svf`] State variable filter +## State variable filter -Simple IIR state variable filter simultaneously providing highpass, lowpass, +[`svf`] is a simple IIR state variable filter simultaneously providing highpass, lowpass, bandpass, and notch filtering of a signal. -## [`Lowpass`], [`Lockin`] +## `Lowpass`, `Lockin` -Fast, infinitely cascadable, first- and second-order lowpass and the corresponding integration into a lockin amplifier algorithm. +[`Lowpass`], [`Lockin`] are fast, infinitely cascadable, first- and second-order lowpass and the corresponding integration into a lockin amplifier algorithm. -## FIR filters: [`hbf::HbfDec`], [`hbf::HbfInt`], [`hbf::HbfDecCascade`], [`hbf::HbfIntCascade`] +## FIR filters +[`hbf::HbfDec`], [`hbf::HbfInt`], [`hbf::HbfDecCascade`], [`hbf::HbfIntCascade`]: Fast `f32` symmetric FIR filters, optimized half-band filters, half-band filter decimators and integators and cascades. These are used in [`stabilizer-stream`](https://github.com/quartiq/stabilizer-stream) for online PSD calculation on log frequency scale for arbitrarily large amounts of data. diff --git a/src/iir/biquad.rs b/src/iir/biquad.rs index 95de077..4450913 100644 --- a/src/iir/biquad.rs +++ b/src/iir/biquad.rs @@ -219,8 +219,8 @@ impl Biquad { /// ``` /// # use idsp::Coefficient; /// # use idsp::iir::*; - /// assert_eq!(Biquad::::IDENTITY.ba()[0], i32::ONE); - /// assert_eq!(Biquad::::HOLD.ba()[3], -i32::ONE); + /// assert_eq!(Biquad::::IDENTITY.ba()[0], ::ONE); + /// assert_eq!(Biquad::::HOLD.ba()[3], -::ONE); /// ``` pub fn ba(&self) -> &[T; 5] { &self.ba @@ -234,7 +234,7 @@ impl Biquad { /// # use idsp::Coefficient; /// # use idsp::iir::*; /// let mut i = Biquad::default(); - /// i.ba_mut()[0] = i32::ONE; + /// i.ba_mut()[0] = ::ONE; /// assert_eq!(i, Biquad::IDENTITY); /// ``` pub fn ba_mut(&mut self) -> &mut [T; 5] { @@ -349,7 +349,7 @@ impl Biquad { /// # use idsp::iir::*; /// let mut i = Biquad::proportional(3); /// i.set_u(3); - /// assert_eq!(i.input_offset(), i32::ONE); + /// assert_eq!(i.input_offset(), ::ONE); /// ``` pub fn input_offset(&self) -> T { self.u.div_scaled(self.forward_gain()) @@ -375,7 +375,7 @@ impl Biquad { /// ``` /// # use idsp::Coefficient; /// # use idsp::iir::*; - /// let mut i = Biquad::proportional(-i32::ONE); + /// let mut i = Biquad::proportional(-::ONE); /// i.set_input_offset(1); /// assert_eq!(i.u(), -1); /// ``` From 870f932c92bc039c1dd083ac7ad8a42d3b7487fc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Mon, 15 Jan 2024 16:29:32 +0100 Subject: [PATCH 26/26] add feature comparison/benchmark --- README.md | 53 +++++++++++++++++++++++++++++++++++------------------ 1 file changed, 35 insertions(+), 18 deletions(-) diff --git a/README.md b/README.md index 30b8ecd..f8ec790 100644 --- a/README.md +++ b/README.md @@ -39,24 +39,41 @@ Robust and clean clipping and offset (anti-windup, no derivative kick, dynamical Three kinds of filter actions: Direct Form 1, Direct Form 2 Transposed, and Direct Form 1 with noise shaping supported. Coefficient sharing for multiple channels. -Compared to [`biquad-rs`](https://crates.io/crates/biquad) this crates adds several additional important features: - -* fixed point implementations (`i32`, `i64`, etc.) in addition to `f32`/`f64` floating point -* additional [`iir::Filter`] builders (I/HO) -* decoupled [`iir::Biquad`] configuration and flat `[T; N]` state -* [`iir::Pid`] builder -* DF1 noise shaping for fixed point -* proper output limiting/clamping before feedback ("anti-windup") -* summing junction offset (for PID controller applications) - -Compared to [`fixed-filters`](https://crates.io/crates/fixed-filters) this crate: - -* Supports unified floating point and fixed point API -* decoupled [`iir::Biquad`] configuration and flat `[T; N]` state -* [`iir::Pid`] builder -* additional [`iir::Filter`] builders (I/HO) -* noise shaping for fixed point -* summing junction offset (for PID controller applications) +### Comparison + +This is a rough feature comparison of several available `biquad` crates, with no claim for completeness, accuracy, or even fairness. +TL;DR: `idsp` is slower but offers more features. + +| Feature\Crate | [`biquad-rs`](https://crates.io/crates/biquad) | [`fixed-filters`](https://crates.io/crates/fixed-filters) | `idsp::iir` | +|---|---|---|---| +| Floating point `f32`/`f64` | ✅ | ❌ | ✅ | +| Fixed point `i32` | ❌ | ✅ | ✅ | +| Parametric fixed point `i32` | ❌ | ✅ | ❌ | +| Fixed point `i8`/`i16`/`i64`/`i128` | ❌ | ❌ | ✅ | +| DF2T | ✅ | ❌ | ✅ | +| Limiting/Clamping | ❌ | ✅ | ✅ | +| Fixed point accumulator guard bits | ❌ | ❌ | ✅ | +| Summing junction offset | ❌ | ❌ | ✅ | +| Fixed point noise shaping | ❌ | ❌ | ✅ | +| Configuration/state decoupling/multi-channel | ❌ | ❌ | ✅ | +| `f32` parameter audio filter builder | ✅ | ✅ | ✅ | +| `f64` parameter audio filter builder | ✅ | ❌ | ✅ | +| Additional filters (I/HO) | ❌ | ❌ | ✅ | +| `f32` PI builder | ❌ | ✅ | ✅ | +| `f32/f64` PI²D² builder | ❌ | ❌ | ✅ | +| PI²D² builder limits | ❌ | ❌ | ✅ | +| Support for fixed point `a1=-2` | ❌ | ❌ | ✅ | + +Three crates have been compared when processing 4x1M samples (4 channels) with a biquad lowpass. +Hardware was `thumbv7em-none-eabihf`, `cortex-m7`, code in ITCM, data in DTCM, caches enabled. + +| Crate | Type, features | Cycles per sample | +|---|---|---| +| [`biquad-rs`](https://crates.io/crates/biquad) | `f32` | 11.4 | +| `idsp::iir` | `f32`, limits, offset | 15.5 | +| [`fixed-filters`](https://crates.io/crates/fixed-filters) | `i32`, limits | 20.3 | +| `idsp::iir` | `i32`, limits, offset | 23.5 | +| `idsp::iir` | `i32`, limits, offset, noise shaping | 30.0 | ## State variable filter