biquad.h

/*
 * Bi-quadratic filters
*/

#pragma once

#include <array>
#include <complex>
#include <tuple>

#include "control/system/type.h"

namespace control::filter {

template<typename T = float>
using TC = std::complex<T>;

template<typename T = float>
using TCS = std::pair<TC<T>, TC<T>>;

template<typename T = float, typename S = const T>
class Biquad : public system::SISO<T> {
 public:
  Biquad(T b0, T b1, T b2, T a1, T a2) : B{b0, b1, b2}, A{a1, a2} {};

  Biquad(T b0, T b1, T b2, T a0, T a1, T a2) : B{b0 / a0, b1 / a0, b2 / a0}, A{a1 / a0, a2 / a0} {};

  Biquad(TCS<T> z, TCS<T> p, T k) {
    static_assert(!std::is_const<S>::value, "Storage type S must be non-const to use ZPK.");
    std::tie(B[0], B[1], B[2], A[0], A[1]) = zpk2coef(z, p, k);
  };

  T step(T x) {
    T y;

    /* Direct form II transposed */
    y = x * B[0] + wz[0];
    wz[0] = x * B[1] - A[0] * y + wz[1];
    wz[1] = x * B[2] - A[1] * y;

    return y;
  }

  TCS<T> poles() {
    return solve((T) 1, A[0], A[1]);
  }

  TCS<T> zeros() {
    return solve(B[0], B[1], B[2]);
  }

  bool stable() {
    auto p = poles();

    // Whether the magnitude of both poles is leq unity
    return abs(std::get<0>(p)) <= (T) 1
        && abs(std::get<1>(p)) <= (T) 1;
  }

  void reset() {
    wz[0] = 0;
    wz[1] = 0;
  }

 protected:

  T wz[2] = {0, 0};

  S B[3];

  S A[2];

  std::tuple<T, T, T, T, T> zpk2coef(TCS<T> z, TCS<T> p, T k) {
    auto b = zero2coef(z);
    auto a = zero2coef(p);
    return std::make_tuple(
        k,
        k * std::get<0>(b),
        k * std::get<1>(b),
        std::get<0>(a),
        std::get<1>(a)
    );
  }

  std::tuple<T, T> zero2coef(TCS<T> z) {
    auto z1 = std::get<0>(z);
    auto z2 = std::get<1>(z);

    auto z1r = z1.real();
    auto z1i = z1.imag();
    auto z2r = z2.real();

    // Assume this is a complex conjugate pair if p1 has imag part
    return z1i
           ? std::make_tuple(-2 * z1r, z1r * z1r + z1i * z1i)
           : std::make_tuple(-z1r - z2r, z1r * z2r);
  };

  TCS<T> solve(T a, T b, T c) {
    // Normalize
    b /= a;
    c /= a;

    // sqrt(b^2 - 4*a*c)
    TC<T> ds = std::sqrt(TC<T>(b * b, 0) - 4 * c);

    // (-b±ds)/2a
    return std::make_pair<TC<T>, TC<T>>((-b + ds) / (T) 2, (-b - ds) / (T) 2);
  }

};

template<typename T>
struct inspect_types {
  typedef T arithmetic_type;
  typedef T storage_type;
};

template<template<typename, typename> class B, typename T, typename S>
struct inspect_types<B<T, S>> {
  typedef T arithmetic_type;
  typedef S storage_type;
};

template<typename B=Biquad<>, size_t N = 1>
class BiquadCascade : public system::SISO<typename inspect_types<B>::arithmetic_type> {
  using T = typename inspect_types<B>::arithmetic_type;
  using BS = std::array<B, N>;
 public:

  template<typename... T>
  BiquadCascade(T... bs) : bs{bs...} {}

  T step(T u) {
    for (auto &b : bs)
      u = b.step(u);

    return u;
  }

 protected:

  BS bs;
};

}