idsp/iir/
biquad.rs

1//! Biquad IIR
2
3use crate::Clamp;
4use core::ops::{Add, Div, Mul};
5use dsp_fixedpoint::Q;
6use dsp_process::{SplitInplace, SplitProcess};
7
8#[cfg(not(feature = "std"))]
9#[allow(unused_imports)]
10use num_traits::float::FloatCore as _;
11use num_traits::{AsPrimitive, clamp};
12
13/// Biquad IIR (second order section)
14///
15/// A biquadratic IIR filter supports up to two zeros and two poles in the transfer function.
16///
17/// The Biquad performs the following operation to compute a new output sample `y0` from a new
18/// input sample `x0` given its configuration and previous samples:
19///
20/// `y0 = b0*x0 + b1*x1 + b2*x2 + a1*y1 + a2*y2`
21///
22/// This implementation here saves storage and improves caching opportunities by decoupling
23/// filter configuration (coefficients, limits and offset) from filter state
24/// and thus supports both (a) sharing a single filter between multiple states ("channels") and (b)
25/// rapid switching of filters (tuning, transfer) for a given state without copying either
26/// state of configuration.
27///
28/// # Filter architecture
29///
30/// Direct Form 1 (DF1) and Direct Form 2 transposed (DF2T) are the only IIR filter
31/// structures with an (effective in the case of TDF2) single summing junction
32/// this allows clamping of the output before feedback.
33///
34/// DF1 allows atomic coefficient change because only inputs and outputs are stored.
35/// The summing junction pipelining of TDF2 would require incremental
36/// coefficient changes and is thus less amenable to online tuning.
37///
38/// DF2T needs less state storage (2 instead of 4). This is in addition to the coefficient
39/// storage (5 plus 2 limits plus 1 offset)
40///
41/// DF2T is less efficient and less accurate for fixed-point architectures as quantization
42/// happens at each intermediate summing junction in addition to the output quantization. This is
43/// especially true for common `i64 + i32 * i32 -> i64` MACC architectures.
44/// One could use wide state storage for fixed point DF2T but that would negate the storage
45/// and processing advantages.
46///
47/// # Coefficients
48///
49/// `ba: [T; 5] = [b0, b1, b2, a1, a2]` is the coefficients type.
50/// To represent the IIR coefficients, this contains the feed-forward
51/// coefficients `b0, b1, b2` followed by the feed-back coefficients
52/// `a1, a2`, all five normalized such that `a0 = 1`.
53///
54/// The summing junction of the [`BiquadClamp`] filter also receives an offset `u`
55/// and applies clamping such that `min <= y <= max`.
56///
57/// See [`crate::iir::coefficients::Filter`] and [`crate::iir::pid::Builder`] for ways to generate coefficients.
58///
59/// # Fixed point
60///
61/// Coefficient scaling for fixed point (i.e. integer) processing relies on [`dsp_fixedpoint::Q`].
62///
63/// Choose the number of fractional bits to meet coefficient range (e.g. potentially `a1 = 2`
64/// for a double integrator) and guard bits.
65///
66/// # PID controller
67///
68/// The IIR coefficients can be mapped to other transfer function
69/// representations, for example PID controllers as described in
70/// <https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw> and
71/// <https://arxiv.org/abs/1508.06319>.
72///
73/// Using a Biquad as a template for a PID controller achieves several important properties:
74///
75/// * Its transfer function is universal in the sense that any biquadratic
76///   transfer function can be implemented (high-passes, gain limits, second
77///   order integrators with inherent anti-windup, notches etc) without code
78///   changes preserving all features.
79/// * It inherits a universal implementation of "integrator anti-windup", also
80///   and especially in the presence of set-point changes and in the presence
81///   of proportional or derivative gain without any back-off that would reduce
82///   steady-state output range.
83/// * It has universal derivative-kick (undesired, unlimited, and un-physical
84///   amplification of set-point changes by the derivative term) avoidance.
85/// * An offset at the input of an IIR filter (a.k.a. "set-point") is
86///   equivalent to an offset at the summing junction (in output units).
87///   They are related by the overall (DC feed-forward) gain of the filter.
88/// * It stores only previous outputs and inputs. These have direct and
89///   invariant interpretation (independent of coefficients and offset).
90///   Therefore it can trivially implement bump-less transfer between any
91///   coefficients/offset sets.
92/// * Cascading multiple IIR filters allows stable and robust
93///   implementation of transfer functions beyond biquadratic terms.
94#[derive(Clone, Debug, Default, PartialEq, PartialOrd, serde::Serialize, serde::Deserialize)]
95pub struct Biquad<C> {
96    /// Coefficients
97    ///
98    /// `[b0, b1, b2, a1, a2]`
99    ///
100    /// Such that
101    /// `y0 = (b0*x0 + b1*x1 + b2*x2 + a1*y1 + a2*y2)/(1 << F)`
102    /// where `x0, x1, x2` are current, delayed, and doubly delayed inputs and
103    /// `y0, y1, y2` are current, delayed, and doubly delayed outputs.
104    ///
105    /// Note the a1, a2 sign. The transfer function is:
106    /// `H(z) = (b0 + b1*z^-1 + b2*z^-2)/((1 << F) - a2*z^-1 - a2*z^-2)`
107    pub ba: [C; 5],
108}
109
110/// Second-order-section with offset and clamp
111#[derive(Clone, Debug, PartialEq, PartialOrd, serde::Serialize, serde::Deserialize)]
112pub struct BiquadClamp<C, T = C> {
113    /// Coefficients
114    pub coeff: Biquad<C>,
115
116    /// Summing junction offset
117    ///
118    /// ```
119    /// # use idsp::iir::*;
120    /// # use dsp_process::SplitProcess;
121    /// let mut i = BiquadClamp::<f32>::default();
122    /// i.u = 5.0;
123    /// assert_eq!(i.process(&mut DirectForm1::default(), 0.0), 5.0);
124    /// ```
125    pub u: T,
126
127    /// Summing junction lower limit
128    ///
129    /// ```
130    /// # use idsp::iir::*;
131    /// # use dsp_process::SplitProcess;
132    /// let mut i = BiquadClamp::<f32>::default();
133    /// i.min = 5.0;
134    /// assert_eq!(i.process(&mut DirectForm1::default(), 0.0), 5.0);
135    /// ```
136    pub min: T,
137
138    /// Summing junction upper limit
139    ///
140    /// ```
141    /// # use idsp::iir::*;
142    /// # use dsp_process::SplitProcess;
143    /// let mut i = BiquadClamp::<f32>::default();
144    /// i.max = -5.0;
145    /// assert_eq!(i.process(&mut DirectForm1::default(), 0.0), -5.0);
146    /// ```
147    pub max: T,
148}
149
150impl<C, T: Clamp> Default for BiquadClamp<C, T>
151where
152    Biquad<C>: Default,
153{
154    fn default() -> Self {
155        Self {
156            coeff: Biquad::default(),
157            u: T::ZERO,
158            min: T::MIN,
159            max: T::MAX,
160        }
161    }
162}
163
164impl<C: Clamp + Copy> Biquad<C> {
165    /// A unity gain filter
166    ///
167    /// ```
168    /// # use idsp::iir::*;
169    /// # use dsp_process::SplitProcess;
170    /// let x0 = 3.0;
171    /// let y0 = Biquad::<f32>::IDENTITY.process(&mut DirectForm1::default(), x0);
172    /// assert_eq!(y0, x0);
173    /// ```
174    pub const IDENTITY: Self = Self::proportional(C::ONE);
175
176    /// A filter with the given proportional gain at all frequencies
177    ///
178    /// ```
179    /// # use idsp::iir::*;
180    /// # use dsp_process::SplitProcess;
181    /// let x0 = 3.0;
182    /// let y0 = Biquad::<f32>::proportional(2.0).process(&mut DirectForm1::default(), x0);
183    /// assert_eq!(y0, 2.0 * x0);
184    /// ```
185    pub const fn proportional(k: C) -> Self {
186        Self {
187            ba: [k, C::ZERO, C::ZERO, C::ZERO, C::ZERO],
188        }
189    }
190    /// A "hold" filter that ingests input and maintains output
191    ///
192    /// ```
193    /// # use idsp::iir::*;
194    /// # use dsp_process::SplitProcess;
195    /// let mut state = DirectForm1::<f32>::default();
196    /// state.xy[2] = 2.0;
197    /// let x0 = 7.0;
198    /// let y0 = Biquad::<f32>::HOLD.process(&mut state, x0);
199    /// assert_eq!(y0, 2.0);
200    /// assert_eq!(state.xy, [x0, 0.0, y0, y0]);
201    /// ```
202    pub const HOLD: Self = Self {
203        ba: [C::ZERO, C::ZERO, C::ZERO, C::ONE, C::ZERO],
204    };
205}
206
207impl<C: Copy + Add<Output = C>> Biquad<C> {
208    /// DC forward gain fro input to summing junction
209    ///
210    /// ```
211    /// # use idsp::iir::*;
212    /// assert_eq!(Biquad::proportional(3.0).forward_gain(), 3.0);
213    /// ```
214    pub fn forward_gain(&self) -> C {
215        self.ba[0] + self.ba[1] + self.ba[2]
216    }
217}
218
219impl<C: Copy + Add<Output = C>, T: Copy + Div<C, Output = T> + Mul<C, Output = T>>
220    BiquadClamp<C, T>
221{
222    /// Summing junction offset referred to input
223    ///
224    /// ```
225    /// # use idsp::iir::*;
226    /// let mut i = BiquadClamp::from(Biquad::proportional(3.0));
227    /// i.u = 6.0;
228    /// assert_eq!(i.input_offset(), 2.0);
229    /// ```
230    pub fn input_offset(&self) -> T {
231        self.u / self.coeff.forward_gain()
232    }
233
234    /// Summing junction offset referred to input
235    ///
236    /// ```
237    /// # use idsp::iir::*;
238    /// let mut i = BiquadClamp::from(Biquad::proportional(3.0));
239    /// i.set_input_offset(2.0);
240    /// assert_eq!(i.u, 6.0);
241    /// ```
242    pub fn set_input_offset(&mut self, i: T) {
243        self.u = i * self.coeff.forward_gain();
244    }
245}
246
247/// SOS state
248#[derive(Clone, Debug, Default, serde::Deserialize, serde::Serialize)]
249pub struct DirectForm1<T> {
250    /// X,Y state
251    ///
252    /// Contents before `process()`: `[x1, x2, y1, y2]`
253    pub xy: [T; 4],
254}
255
256/// ```
257/// # use dsp_process::SplitProcess;
258/// # use idsp::iir::*;
259/// let mut state = DirectForm1 {
260///     xy: [0.0, 1.0, 2.0, 3.0],
261/// };
262/// let x0 = 4.0;
263/// let y0 = Biquad::<f32>::IDENTITY.process(&mut state, x0);
264/// assert_eq!(y0, x0);
265/// assert_eq!(state.xy, [x0, 0.0, y0, 2.0]);
266/// ```
267impl<T: 'static + Copy, C: Copy + Mul<T, Output = A>, A: Add<Output = A> + AsPrimitive<T>>
268    SplitProcess<T, T, DirectForm1<T>> for Biquad<C>
269{
270    fn process(&self, state: &mut DirectForm1<T>, x0: T) -> T {
271        let xy = &mut state.xy;
272        let ba = &self.ba;
273        let y0 = (ba[0] * x0 + ba[1] * xy[0] + ba[2] * xy[1] + ba[3] * xy[2] + ba[4] * xy[3]).as_();
274        *xy = [x0, xy[0], y0, xy[2]];
275        y0
276    }
277}
278
279/// ```
280/// use dsp_process::SplitProcess;
281/// use idsp::iir::*;
282/// let biquad = BiquadClamp::<f32, f32>::from(Biquad::IDENTITY);
283/// let mut state = DirectForm2Transposed::default();
284/// let x = 3.0f32;
285/// let y = biquad.process(&mut state, x);
286/// assert_eq!(x, y);
287/// ```
288impl<T: Copy + Add<Output = T> + PartialOrd, C> SplitProcess<T, T, DirectForm1<T>>
289    for BiquadClamp<C, T>
290where
291    Biquad<C>: SplitProcess<T, T, DirectForm1<T>>,
292{
293    fn process(&self, state: &mut DirectForm1<T>, x0: T) -> T {
294        let y0 = clamp(self.coeff.process(state, x0) + self.u, self.min, self.max);
295        state.xy[2] = y0; // overwrite
296        y0
297    }
298}
299
300/// Direct form 2 transposed SOS state
301#[derive(Clone, Debug, Default, serde::Deserialize, serde::Serialize)]
302pub struct DirectForm2Transposed<T> {
303    /// internal state
304    ///
305    /// `[u1, u2]`
306    pub u: [T; 2],
307}
308
309/// ```
310/// use dsp_process::SplitProcess;
311/// use idsp::iir::*;
312/// let biquad = Biquad::<f32>::IDENTITY;
313/// let mut state = DirectForm2Transposed::default();
314/// let x = 3.0f32;
315/// let y = biquad.process(&mut state, x);
316/// assert_eq!(x, y);
317/// ```
318impl<T: Copy + Mul<Output = T> + Add<Output = T>> SplitProcess<T, T, DirectForm2Transposed<T>>
319    for Biquad<T>
320{
321    fn process(&self, state: &mut DirectForm2Transposed<T>, x0: T) -> T {
322        let u = &mut state.u;
323        let ba = &self.ba;
324        let y0 = u[0] + ba[0] * x0;
325        u[0] = u[1] + ba[1] * x0 + ba[3] * y0;
326        u[1] = ba[2] * x0 + ba[4] * y0;
327        y0
328    }
329}
330
331impl<T: Copy + Add<Output = T> + Mul<Output = T> + PartialOrd>
332    SplitProcess<T, T, DirectForm2Transposed<T>> for BiquadClamp<T, T>
333{
334    fn process(&self, state: &mut DirectForm2Transposed<T>, x0: T) -> T {
335        let u = &mut state.u;
336        let ba = &self.coeff.ba;
337        let y0 = clamp(u[0] + ba[0] * x0 + self.u, self.min, self.max);
338        u[0] = u[1] + ba[1] * x0 + ba[3] * y0;
339        u[1] = ba[2] * x0 + ba[4] * y0;
340        y0
341    }
342}
343
344/// SOS state with wide Y
345#[derive(Clone, Debug, Default, serde::Deserialize, serde::Serialize)]
346pub struct DirectForm1Wide {
347    /// X state
348    ///
349    /// `[x1, x2]`
350    pub x: [i32; 2],
351    /// Y state
352    ///
353    /// `[y1, y2]`
354    pub y: [i64; 2],
355}
356
357impl<const F: i8> SplitProcess<i32, i32, DirectForm1Wide> for Biquad<Q<i32, i64, F>> {
358    fn process(&self, state: &mut DirectForm1Wide, x0: i32) -> i32 {
359        let x = &mut state.x;
360        let y = &mut state.y;
361        let ba = &self.ba;
362        let mut acc = (ba[0] * x0 + ba[1] * x[0] + ba[2] * x[1]).inner;
363        *x = [x0, x[0]];
364        acc += (y[0] as u32 as i64 * ba[3].inner as i64) >> 32;
365        acc += (y[0] >> 32) as i32 as i64 * ba[3].inner as i64;
366        acc += (y[1] as u32 as i64 * ba[4].inner as i64) >> 32;
367        acc += (y[1] >> 32) as i32 as i64 * ba[4].inner as i64;
368        acc <<= 32 - F;
369        *y = [acc, y[0]];
370        (acc >> 32) as _
371    }
372}
373
374impl<const F: i8> SplitProcess<i32, i32, DirectForm1Wide> for BiquadClamp<Q<i32, i64, F>, i32> {
375    fn process(&self, state: &mut DirectForm1Wide, x0: i32) -> i32 {
376        let x = &mut state.x;
377        let y = &mut state.y;
378        let ba = &self.coeff.ba;
379        let mut acc = (ba[0] * x0 + ba[1] * x[0] + ba[2] * x[1]).inner;
380        *x = [x0, x[0]];
381        acc += (y[0] as u32 as i64 * ba[3].inner as i64) >> 32;
382        acc += (y[0] >> 32) as i32 as i64 * ba[3].inner as i64;
383        acc += (y[1] as u32 as i64 * ba[4].inner as i64) >> 32;
384        acc += (y[1] >> 32) as i32 as i64 * ba[4].inner as i64;
385        acc <<= 32 - F;
386        let y0 = Ord::clamp((acc >> 32) as i32 + self.u, self.min, self.max);
387        *y = [((y0 as i64) << 32) | acc as u32 as i64, y[0]];
388        y0
389    }
390}
391
392/// SOS state with first order error feedback
393#[derive(Clone, Debug, Default, serde::Deserialize, serde::Serialize)]
394pub struct DirectForm1Dither {
395    /// X,Y state
396    ///
397    /// `[x1, x2, y1, y2]`
398    pub xy: [i32; 4],
399    /// Error feedback
400    pub e: u32,
401}
402
403/// ```
404/// # use dsp_process::SplitProcess;
405/// # use dsp_fixedpoint::Q32;
406/// # use idsp::iir::*;
407/// let mut state = DirectForm1Dither {
408///     xy: [1, 2, 3, 4],
409///     e: 5,
410/// };
411/// let x0 = 6;
412/// let y0 = Biquad::<Q32<30>>::IDENTITY.process(&mut state, x0);
413/// assert_eq!(y0, x0);
414/// assert_eq!(state.xy, [x0, 1, y0, 3]);
415/// assert_eq!(state.e, 20);
416/// ```
417impl<const F: i8> SplitProcess<i32, i32, DirectForm1Dither> for Biquad<Q<i32, i64, F>> {
418    fn process(&self, state: &mut DirectForm1Dither, x0: i32) -> i32 {
419        let xy = &mut state.xy;
420        let e = &mut state.e;
421        let ba = &self.ba;
422        let mut acc = *e as i64
423            + (ba[0] * x0 + ba[1] * xy[0] + ba[2] * xy[1] + ba[3] * xy[2] + ba[4] * xy[3]).inner;
424        acc <<= 32 - F;
425        *e = acc as _;
426        let y0 = (acc >> 32) as _;
427        *xy = [x0, xy[0], y0, xy[2]];
428        y0
429    }
430}
431
432impl<const F: i8> SplitProcess<i32, i32, DirectForm1Dither> for BiquadClamp<Q<i32, i64, F>, i32> {
433    fn process(&self, state: &mut DirectForm1Dither, x0: i32) -> i32 {
434        let xy = &mut state.xy;
435        let e = &mut state.e;
436        let ba = &self.coeff.ba;
437        let mut acc = *e as i64
438            + (ba[0] * x0 + ba[1] * xy[0] + ba[2] * xy[1] + ba[3] * xy[2] + ba[4] * xy[3]).inner;
439        acc <<= 32 - F;
440        *e = acc as _;
441        let y0 = Ord::clamp((acc >> 32) as i32 + self.u, self.min, self.max);
442        *xy = [x0, xy[0], y0, xy[2]];
443        y0
444    }
445}
446
447impl<C, T: Copy, S> SplitInplace<T, S> for Biquad<C> where Self: SplitProcess<T, T, S> {}
448impl<C, T: Copy, S> SplitInplace<T, S> for BiquadClamp<C, T> where Self: SplitProcess<T, T, S> {}
449
450/// `[[b0, b1, b2], [a0, a1, a2]]` coefficients with the literature sign of a1/a2
451macro_rules! impl_from_float {
452    ($ty:ident) => {
453        impl<C> From<[[$ty; 3]; 2]> for Biquad<C>
454        where
455            [$ty; 5]: Into<Biquad<C>>,
456        {
457            fn from(ba: [[$ty; 3]; 2]) -> Self {
458                let a0 = 1.0 / ba[1][0];
459                [
460                    ba[0][0] * a0,
461                    ba[0][1] * a0,
462                    ba[0][2] * a0,
463                    -ba[1][1] * a0,
464                    -ba[1][2] * a0,
465                ]
466                .into()
467            }
468        }
469    };
470}
471impl_from_float!(f32);
472impl_from_float!(f64);
473
474/// Normalized and sign-flipped coefficients
475/// `[b0, b1, b2, a1, a2]`
476impl<C: Copy + 'static, T> From<[T; 5]> for Biquad<C>
477where
478    T: AsPrimitive<C>,
479{
480    fn from(ba: [T; 5]) -> Self {
481        Self {
482            ba: ba.map(AsPrimitive::as_),
483        }
484    }
485}
486
487impl<C, T, F> From<F> for BiquadClamp<C, T>
488where
489    F: Into<Biquad<C>>,
490    Self: Default,
491{
492    fn from(coeff: F) -> Self {
493        Self {
494            coeff: coeff.into(),
495            ..Default::default()
496        }
497    }
498}
499
500#[cfg(test)]
501mod test {
502    #![allow(dead_code)]
503    use super::*;
504    use dsp_fixedpoint::Q32;
505    use dsp_process::SplitInplace;
506    // No manual tuning needed here.
507    // Compiler knows best how and when:
508    //   unroll loops
509    //   cache on stack
510    //   handle alignment
511    //   register allocate variables
512    //   manage pipeline and insn issue
513
514    // cargo asm idsp::iir::biquad::test::pnm -p idsp --rust --target thumbv7em-none-eabihf --lib --target-cpu cortex-m7 --color --mca -M=-iterations=1 -M=-timeline -M=-skip-unsupported-instructions=lack-sched | less -R
515
516    pub fn pnm(
517        config: &[Biquad<Q32<29>>; 4],
518        state: &mut [DirectForm1<i32>; 4],
519        xy0: &mut [i32; 1 << 3],
520    ) {
521        config.inplace(state, xy0);
522    }
523
524    // ~20 cycles/sample/sos on skylake, >200 MS/s
525    #[test]
526    #[ignore]
527    fn sos_insn() {
528        let cfg = [
529            [[1., 3., 5.], [19., -9., 9.]],
530            [[3., 3., 5.], [21., -11., 11.]],
531            [[1., 3., 5.], [55., -17., 17.]],
532            [[1., 8., 5.], [77., -7., 7.]],
533        ]
534        .map(|c| Biquad::from(c));
535        let mut state = Default::default();
536        let mut x = [977371917; 1 << 7];
537        for _ in 0..1 << 20 {
538            x[9] = x[63];
539            let (x, []) = x.as_chunks_mut() else {
540                unreachable!()
541            };
542            for x in x {
543                pnm(&cfg, &mut state, x);
544            }
545        }
546    }
547}
idsp/iir/biquad.rs

idsp/iir/
biquad.rs
