idsp/iir/
biquad.rs

1use miniconf::Tree;
2use num_traits::{AsPrimitive, Float};
3use serde::{Deserialize, Serialize};
4
5use crate::Coefficient;
6
7/// Biquad IIR filter
8///
9/// A biquadratic IIR filter supports up to two zeros and two poles in the transfer function.
10/// It can be used to implement a wide range of responses to input signals.
11///
12/// The Biquad performs the following operation to compute a new output sample `y0` from a new
13/// input sample `x0` given its configuration and previous samples:
14///
15/// `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)`
16///
17/// This implementation here saves storage and improves caching opportunities by decoupling
18/// filter configuration (coefficients, limits and offset) from filter state
19/// and thus supports both (a) sharing a single filter between multiple states ("channels") and (b)
20/// rapid switching of filters (tuning, transfer) for a given state without copying either
21/// state of configuration.
22///
23/// # Filter architecture
24///
25/// Direct Form 1 (DF1) and Direct Form 2 transposed (DF2T) are the only IIR filter
26/// structures with an (effective bin the case of TDF2) single summing junction
27/// this allows clamping of the output before feedback.
28///
29/// DF1 allows atomic coefficient change because only inputs and outputs are pipelined.
30/// The summing junction pipelining of TDF2 would require incremental
31/// coefficient changes and is thus less amenable to online tuning.
32///
33/// DF2T needs less state storage (2 instead of 4). This is in addition to the coefficient
34/// storage (5 plus 2 limits plus 1 offset)
35///
36/// DF2T is less efficient and accurate for fixed-point architectures as quantization
37/// happens at each intermediate summing junction in addition to the output quantization. This is
38/// especially true for common `i64 + i32 * i32 -> i64` MACC architectures.
39/// One could use wide state storage for fixed point DF2T but that would negate the storage
40/// and processing advantages.
41///
42/// # Coefficients
43///
44/// `ba: [T; 5] = [b0, b1, b2, a1, a2]` is the coefficients type.
45/// To represent the IIR coefficients, this contains the feed-forward
46/// coefficients `b0, b1, b2` followed by the feed-back coefficients
47/// `a1, a2`, all five normalized such that `a0 = 1`.
48///
49/// The summing junction of the filter also receives an offset `u`.
50///
51/// The filter applies clamping such that `min <= y <= max`.
52///
53/// See [`crate::iir::Filter`] and [`crate::iir::PidBuilder`] for ways to generate coefficients.
54///
55/// # Fixed point
56///
57/// Coefficient scaling (see [`Coefficient`]) is fixed and optimized such that -2 is exactly
58/// representable. This is tailored to low-passes, PID, II etc, where the integration rule is
59/// [1, -2, 1].
60///
61/// There are two guard bits in the accumulator before clamping/limiting.
62/// While this isn't enough to cover the worst case accumulator, it does catch many real world
63/// overflow cases.
64///
65/// # State
66///
67/// To represent the IIR state (input and output memory) during [`Biquad::update()`]
68/// the DF1 state contains the two previous inputs and output `[x1, x2, y1, y2]`
69/// concatenated. Lower indices correspond to more recent samples.
70///
71/// In the DF2T case the state contains `[b1*x1 + b2*x2 - a1*y1 - a2*y2, b2*x1 - a2*y1]`
72///
73/// In the DF1 case with first order noise shaping, the state contains `[x1, x2, y1, y2, e1]`
74/// where `e0` is the accumulated quantization error.
75///
76/// # PID controller
77///
78/// The IIR coefficients can be mapped to other transfer function
79/// representations, for example PID controllers as described in
80/// <https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw> and
81/// <https://arxiv.org/abs/1508.06319>.
82///
83/// Using a Biquad as a template for a PID controller achieves several important properties:
84///
85/// * Its transfer function is universal in the sense that any biquadratic
86///   transfer function can be implemented (high-passes, gain limits, second
87///   order integrators with inherent anti-windup, notches etc) without code
88///   changes preserving all features.
89/// * It inherits a universal implementation of "integrator anti-windup", also
90///   and especially in the presence of set-point changes and in the presence
91///   of proportional or derivative gain without any back-off that would reduce
92///   steady-state output range.
93/// * It has universal derivative-kick (undesired, unlimited, and un-physical
94///   amplification of set-point changes by the derivative term) avoidance.
95/// * An offset at the input of an IIR filter (a.k.a. "set-point") is
96///   equivalent to an offset at the summing junction (in output units).
97///   They are related by the overall (DC feed-forward) gain of the filter.
98/// * It stores only previous outputs and inputs. These have direct and
99///   invariant interpretation (independent of coefficients and offset).
100///   Therefore it can trivially implement bump-less transfer between any
101///   coefficients/offset sets.
102/// * Cascading multiple IIR filters allows stable and robust
103///   implementation of transfer functions beyond biquadratic terms.
104#[derive(Clone, Debug, Deserialize, Serialize, PartialEq, PartialOrd, Tree)]
105pub struct Biquad<T> {
106    /// Flattened coefficients `[b0, b1, b2, a1, a2]`
107    ///
108    /// The normalization `a0` is determined by the `Coefficient` implementation of the
109    /// underlying type.
110    ba: [T; 5],
111    /// Summing junction offset
112    u: T,
113    /// Summing junction lower limit
114    min: T,
115    /// Summing junction upper limit
116    max: T,
117}
118
119impl<T: Coefficient> Default for Biquad<T> {
120    fn default() -> Self {
121        Self {
122            ba: [T::ZERO; 5],
123            u: T::ZERO,
124            min: T::MIN,
125            max: T::MAX,
126        }
127    }
128}
129
130impl<T: Coefficient> From<[T; 5]> for Biquad<T> {
131    fn from(ba: [T; 5]) -> Self {
132        Self {
133            ba,
134            ..Default::default()
135        }
136    }
137}
138
139impl<T, C> From<&[[C; 3]; 2]> for Biquad<T>
140where
141    T: Coefficient + AsPrimitive<C>,
142    C: Float + AsPrimitive<T>,
143{
144    fn from(ba: &[[C; 3]; 2]) -> Self {
145        let ia0 = ba[1][0].recip();
146        Self::from([
147            T::quantize(ba[0][0] * ia0),
148            T::quantize(ba[0][1] * ia0),
149            T::quantize(ba[0][2] * ia0),
150            // b[3]: a0*ia0
151            T::quantize(ba[1][1] * ia0),
152            T::quantize(ba[1][2] * ia0),
153        ])
154    }
155}
156
157impl<T, C> From<&Biquad<T>> for [[C; 3]; 2]
158where
159    T: Coefficient + AsPrimitive<C>,
160    C: 'static + Copy,
161{
162    fn from(value: &Biquad<T>) -> Self {
163        let ba = value.ba();
164        [
165            [ba[0].as_(), ba[1].as_(), ba[2].as_()],
166            [T::ONE.as_(), ba[3].as_(), ba[4].as_()],
167        ]
168    }
169}
170
171impl<T: Coefficient> Biquad<T> {
172    /// A "hold" filter that ingests input and maintains output
173    ///
174    /// ```
175    /// # use idsp::iir::*;
176    /// let mut xy = [0.0, 1.0, 2.0, 3.0];
177    /// let x0 = 7.0;
178    /// let y0 = Biquad::HOLD.update(&mut xy, x0);
179    /// assert_eq!(y0, 2.0);
180    /// assert_eq!(xy, [x0, 0.0, y0, y0]);
181    /// ```
182    pub const HOLD: Self = Self {
183        ba: [T::ZERO, T::ZERO, T::ZERO, T::NEG_ONE, T::ZERO],
184        u: T::ZERO,
185        min: T::MIN,
186        max: T::MAX,
187    };
188
189    /// A unity gain filter
190    ///
191    /// ```
192    /// # use idsp::iir::*;
193    /// let x0 = 3.0;
194    /// let y0 = Biquad::IDENTITY.update(&mut [0.0; 4], x0);
195    /// assert_eq!(y0, x0);
196    /// ```
197    pub const IDENTITY: Self = Self::proportional(T::ONE);
198
199    /// A filter with the given proportional gain at all frequencies
200    ///
201    /// ```
202    /// # use idsp::iir::*;
203    /// let x0 = 2.0;
204    /// let k = 5.0;
205    /// let y0 = Biquad::proportional(k).update(&mut [0.0; 4], x0);
206    /// assert_eq!(y0, x0 * k);
207    /// ```
208    pub const fn proportional(k: T) -> Self {
209        Self {
210            ba: [k, T::ZERO, T::ZERO, T::ZERO, T::ZERO],
211            u: T::ZERO,
212            min: T::MIN,
213            max: T::MAX,
214        }
215    }
216
217    /// Filter coefficients
218    ///
219    /// IIR filter tap gains (`ba`) are an array `[b0, b1, b2, a1, a2]` such that
220    /// [`Biquad::update()`] returns
221    /// `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)`.
222    ///
223    /// ```
224    /// # use idsp::Coefficient;
225    /// # use idsp::iir::*;
226    /// assert_eq!(Biquad::<i32>::IDENTITY.ba()[0], <i32 as Coefficient>::ONE);
227    /// assert_eq!(Biquad::<i32>::HOLD.ba()[3], -<i32 as Coefficient>::ONE);
228    /// ```
229    pub fn ba(&self) -> &[T; 5] {
230        &self.ba
231    }
232
233    /// Mutable reference to the filter coefficients.
234    ///
235    /// See [`Biquad::ba()`].
236    ///
237    /// ```
238    /// # use idsp::Coefficient;
239    /// # use idsp::iir::*;
240    /// let mut i = Biquad::default();
241    /// i.ba_mut()[0] = <i32 as Coefficient>::ONE;
242    /// assert_eq!(i, Biquad::IDENTITY);
243    /// ```
244    pub fn ba_mut(&mut self) -> &mut [T; 5] {
245        &mut self.ba
246    }
247
248    /// Summing junction offset
249    ///
250    /// This offset is applied to the output `y0` summing junction
251    /// on top of the feed-forward (`b`) and feed-back (`a`) terms.
252    /// The feedback samples are taken at the summing junction and
253    /// thus also include (and feed back) this offset.
254    pub fn u(&self) -> T {
255        self.u
256    }
257
258    /// Set the summing junction offset
259    ///
260    /// See [`Biquad::u()`].
261    ///
262    /// ```
263    /// # use idsp::iir::*;
264    /// let mut i = Biquad::default();
265    /// i.set_u(5);
266    /// assert_eq!(i.update(&mut [0; 4], 0), 5);
267    /// ```
268    pub fn set_u(&mut self, u: T) {
269        self.u = u;
270    }
271
272    /// Lower output limit
273    ///
274    /// Guaranteed minimum output value.
275    /// The value is inclusive.
276    /// The clamping also cleanly affects the feedback terms.
277    ///
278    /// For fixed point types, during the comparison,
279    /// the lowest two bits of value and limit are truncated.
280    ///
281    /// ```
282    /// # use idsp::iir::*;
283    /// assert_eq!(Biquad::<i32>::default().min(), i32::MIN);
284    /// ```
285    pub fn min(&self) -> T {
286        self.min
287    }
288
289    /// Set the lower output limit
290    ///
291    /// See [`Biquad::min()`].
292    ///
293    /// ```
294    /// # use idsp::iir::*;
295    /// let mut i = Biquad::default();
296    /// i.set_min(4);
297    /// assert_eq!(i.update(&mut [0; 4], 0), 4);
298    /// ```
299    pub fn set_min(&mut self, min: T) {
300        self.min = min;
301    }
302
303    /// Upper output limit
304    ///
305    /// Guaranteed maximum output value.
306    /// The value is inclusive.
307    /// The clamping also cleanly affects the feedback terms.
308    ///
309    /// For fixed point types, during the comparison,
310    /// the lowest two bits of value and limit are truncated.
311    /// The behavior is as if those two bits were 0 in the case
312    /// of `min` and one in the case of `max`.
313    ///
314    /// ```
315    /// # use idsp::iir::*;
316    /// assert_eq!(Biquad::<i32>::default().max(), i32::MAX);
317    /// ```
318    pub fn max(&self) -> T {
319        self.max
320    }
321
322    /// Set the upper output limit
323    ///
324    /// See [`Biquad::max()`].
325    ///
326    /// ```
327    /// # use idsp::iir::*;
328    /// let mut i = Biquad::default();
329    /// i.set_max(-5);
330    /// assert_eq!(i.update(&mut [0; 4], 0), -5);
331    /// ```
332    pub fn set_max(&mut self, max: T) {
333        self.max = max;
334    }
335
336    /// Compute the overall (DC/proportional feed-forward) gain.
337    ///
338    /// ```
339    /// # use idsp::iir::*;
340    /// assert_eq!(Biquad::proportional(3.0).forward_gain(), 3.0);
341    /// ```
342    ///
343    /// # Returns
344    /// The sum of the `b` feed-forward coefficients.
345    pub fn forward_gain(&self) -> T {
346        self.ba[0] + self.ba[1] + self.ba[2]
347    }
348
349    /// Compute input-referred (`x`) offset.
350    ///
351    /// ```
352    /// # use idsp::Coefficient;
353    /// # use idsp::iir::*;
354    /// let mut i = Biquad::proportional(3);
355    /// i.set_u(3);
356    /// assert_eq!(i.input_offset(), <i32 as Coefficient>::ONE);
357    /// ```
358    pub fn input_offset(&self) -> T {
359        self.u.div_scaled(self.forward_gain())
360    }
361
362    /// Convert input (`x`) offset to equivalent summing junction offset (`u`) and apply.
363    ///
364    /// In the case of a "PID" controller the response behavior of the controller
365    /// to the offset is "stabilizing", and not "tracking": its frequency response
366    /// is exclusively according to the lowest non-zero [`crate::iir::Action`] gain.
367    /// There is no high order ("faster") response as would be the case for a "tracking"
368    /// controller.
369    ///
370    /// ```
371    /// # use idsp::iir::*;
372    /// let mut i = Biquad::proportional(3.0);
373    /// i.set_input_offset(2.0);
374    /// let x0 = 0.5;
375    /// let y0 = i.update(&mut [0.0; 4], x0);
376    /// assert_eq!(y0, (x0 + i.input_offset()) * i.forward_gain());
377    /// ```
378    ///
379    /// ```
380    /// # use idsp::Coefficient;
381    /// # use idsp::iir::*;
382    /// let mut i = Biquad::proportional(-<i32 as Coefficient>::ONE);
383    /// i.set_input_offset(1);
384    /// assert_eq!(i.u(), -1);
385    /// ```
386    ///
387    /// # Arguments
388    /// * `offset`: Input (`x`) offset.
389    pub fn set_input_offset(&mut self, offset: T) {
390        self.u = offset.mul_scaled(self.forward_gain());
391    }
392
393    /// Direct Form 1 Update
394    ///
395    /// Ingest a new input value into the filter, update the filter state, and
396    /// return the new output. Only the state `xy` is modified.
397    ///
398    /// ## `N=4` Direct Form 1
399    ///
400    /// `xy` contains:
401    /// * On entry: `[x1, x2, y1, y2]`
402    /// * On exit:  `[x0, x1, y0, y1]`
403    ///
404    /// ```
405    /// # use idsp::iir::*;
406    /// let mut xy = [0.0, 1.0, 2.0, 3.0];
407    /// let x0 = 4.0;
408    /// let y0 = Biquad::IDENTITY.update(&mut xy, x0);
409    /// assert_eq!(y0, x0);
410    /// assert_eq!(xy, [x0, 0.0, y0, 2.0]);
411    /// ```
412    ///
413    /// ## `N=5` Direct Form 1 with first order noise shaping
414    ///
415    /// ```
416    /// # use idsp::iir::*;
417    /// let mut xy = [1, 2, 3, 4, 5];
418    /// let x0 = 6;
419    /// let y0 = Biquad::IDENTITY.update(&mut xy, x0);
420    /// assert_eq!(y0, x0);
421    /// assert_eq!(xy, [x0, 1, y0, 3, 5]);
422    /// ```
423    ///
424    /// `xy` contains:
425    /// * On entry: `[x1, x2, y1, y2, e1]`
426    /// * On exit:  `[x0, x1, y0, y1, e0]`
427    ///
428    /// Note: This is only useful for fixed point filters.
429    ///
430    /// ## `N=2` Direct Form 2 transposed
431    ///
432    /// Note: This is only useful for floating point filters.
433    /// Don't use this for fixed point: Quantization happens at each state store operation.
434    /// Ideally the state would be `[T::ACCU; 2]` but then for fixed point it would use equal amount
435    /// of storage compared to DF1 for no gain in performance and loss in functionality.
436    /// There are also no guard bits here.
437    ///
438    /// `xy` contains:
439    /// * On entry: `[b1*x1 + b2*x2 - a1*y1 - a2*y2, b2*x1 - a2*y1]`
440    /// * On exit:  `[b1*x0 + b2*x1 - a1*y0 - a2*y1, b2*x0 - a2*y0]`
441    ///
442    /// ```
443    /// # use idsp::iir::*;
444    /// let mut xy = [0.0, 1.0];
445    /// let x0 = 3.0;
446    /// let y0 = Biquad::IDENTITY.update(&mut xy, x0);
447    /// assert_eq!(y0, x0);
448    /// assert_eq!(xy, [1.0, 0.0]);
449    /// ```
450    ///
451    /// # Arguments
452    /// * `xy` - Current filter state.
453    /// * `x0` - New input.
454    ///
455    /// # Returns
456    /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)`
457    pub fn update<const N: usize>(&self, xy: &mut [T; N], x0: T) -> T {
458        match N {
459            // DF1
460            4 => {
461                let s = self.ba[0].as_() * x0.as_()
462                    + self.ba[1].as_() * xy[0].as_()
463                    + self.ba[2].as_() * xy[1].as_()
464                    - self.ba[3].as_() * xy[2].as_()
465                    - self.ba[4].as_() * xy[3].as_();
466                let (y0, _) = self.u.macc(s, self.min, self.max, T::ZERO);
467                xy[1] = xy[0];
468                xy[0] = x0;
469                xy[3] = xy[2];
470                xy[2] = y0;
471                y0
472            }
473            // DF1 with noise shaping for fixed point
474            5 => {
475                let s = self.ba[0].as_() * x0.as_()
476                    + self.ba[1].as_() * xy[0].as_()
477                    + self.ba[2].as_() * xy[1].as_()
478                    - self.ba[3].as_() * xy[2].as_()
479                    - self.ba[4].as_() * xy[3].as_();
480                let (y0, e0) = self.u.macc(s, self.min, self.max, xy[4]);
481                xy[4] = e0;
482                xy[1] = xy[0];
483                xy[0] = x0;
484                xy[3] = xy[2];
485                xy[2] = y0;
486                y0
487            }
488            // DF2T for floating point
489            2 => {
490                let y0 = (xy[0] + self.ba[0].mul_scaled(x0)).clip(self.min, self.max);
491                xy[0] = xy[1] + self.ba[1].mul_scaled(x0) - self.ba[3].mul_scaled(y0);
492                xy[1] = self.u + self.ba[2].mul_scaled(x0) - self.ba[4].mul_scaled(y0);
493                y0
494            }
495            _ => unimplemented!(),
496        }
497    }
498}
idsp/iir/biquad.rs

idsp/iir/
biquad.rs
