idsp/iir/
biquad.rs

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

idsp/iir/
biquad.rs
