idsp/iir/
normal.rs

1//! Normal form second order section
2
3use core::ops::{Add, Mul, Neg};
4use dsp_process::{SplitInplace, SplitProcess};
5
6#[cfg(not(feature = "std"))]
7#[allow(unused_imports)]
8use num_traits::float::Float as _;
9
10use crate::Complex;
11
12use super::DirectForm1;
13
14/// Normal form second order section
15///
16/// Also known as Rader Gold oscillator, or Chamberlain form IIR.
17/// A direct form implementation has bad pole resolution near the real axis.
18/// The normal form has constant pole resolution in the plane.
19///
20/// This implementation includes a second order all-zeros before the all-poles section.
21///
22/// The `y0`/`y1` fields of [`DirectForm1`] hold the in-phase and quadrature
23/// components of the current output.
24#[derive(Debug, Clone, Default, serde::Serialize, serde::Deserialize)]
25pub struct Normal<C> {
26    /// Feed forward coefficients
27    pub b: [C; 3],
28    /// Pole
29    ///
30    /// Conjugate pole pair at: `p.re() +- 1j*p.im()`
31    pub p: Complex<C>,
32}
33
34/// The y1, y2 aren't DF1 but y1.re() and y1.im()
35impl<C: Copy + Mul<T, Output = A> + Neg<Output = C>, A: Add<Output = A> + Into<T>, T: Copy>
36    SplitProcess<T, T, DirectForm1<T>> for Normal<C>
37{
38    fn process(&self, state: &mut DirectForm1<T>, x0: T) -> T {
39        let b = &self.b;
40        let p = &self.p;
41        let xy = &mut state.xy;
42        let y1: T =
43            (b[0] * x0 + b[1] * xy[0] + b[2] * xy[1] + p.re() * xy[3] + (-p.im() * xy[2])).into();
44        let y0: T = (p.im() * xy[3] + p.re() * xy[2]).into();
45        *xy = [x0, xy[0], y0, y1];
46        y0
47    }
48}
49
50impl<C, T: Copy> SplitInplace<T, DirectForm1<T>> for Normal<C> where
51    Self: SplitProcess<T, T, DirectForm1<T>>
52{
53}
54
55impl<C: Neg<Output = C> + From<f64>> From<&[[f64; 3]; 2]> for Normal<C> {
56    fn from(ba: &[[f64; 3]; 2]) -> Self {
57        let a0 = 1.0 / ba[1][0];
58        let b = [ba[0][0] * a0, ba[0][1] * a0, ba[0][2] * a0];
59        // Roots of a0 * z * z + a1 * z + a2
60        let p2 = -0.5 * ba[1][1];
61        let pq = ba[1][0] * ba[1][2] - p2.powi(2);
62        assert!(pq >= 0.0);
63        let p = [p2 * a0, pq.sqrt() * a0];
64        Self {
65            b: b.map(Into::into),
66            p: Complex(p.map(Into::into)),
67        }
68    }
69}
idsp/iir/normal.rs

idsp/iir/
normal.rs
