Min phase filter computation using QR decomposition

main.cpp

#include <cmath>
#include <iostream>
#include <numeric>
#include <valarray>
#include <vector>

// Motivated by
// Computing the Minimum-Phase filter using the QL-Factorization
// Hansen, Morten; Christensen, Lars P.b.; Winther, Ole
// https://backend.orbit.dtu.dk/ws/portalfiles/portal/5161145/Hansen.pdf

// Given a vector of filter coefficients return a vector of coefficients
// for a minimum phase filter with the same absolute value spectrum.
//
// Equivalently, given the coefficients (a, b, …) of a polynomial
// p(z) = axⁿ + bxⁿ⁻¹ + … with roots {z_i} returns the coefficients of a
// polynomial whose roots are {w_i} where
// w_i = z_i if |z_i| <= 1, or 1 / z_i otherwise.
// Traditionally implemented using cepstral methods but this has the virtue
// of being a small self-contained piece of code.
//
// Or equivalently, given the coefficients of the polynomial p(z), returns the
// coefficients of another polynomial q(z) such that when |w| = 1,
// |p(w)| = |q(w)| and such that the sum of the squares of the first m
// coefficients of q(z) take the largest possible value among all such
// polynomials - informally the highest coefficients are packed towards the
// front.
//
// It's probably slower than cepstral methods except maybe for small filters
// but can probably be implemented easily on small embedded systems.
// Convergence is extra slow when any zeros of the polynomial are close
// to unit circle.
template <typename Real>
std::valarray<Real> min_phase(const std::valarray<Real> &filter,
                              int max_iterations, Real RMS = 1e-6) {
  size_t dim = filter.size();

  std::valarray<Real> matrix(dim * dim);
  std::valarray<Real> v(dim), w(dim);
  std::valarray<Real> previous(filter), current(dim);

  // Create Toeplitz matrix
  for (int j = 0; j < dim; ++j) {
    for (int i = 0; i < dim; ++i) {
      int k = i - j;
      matrix[i + dim * j] = k >= 0 && k < dim ? filter[k] : 0;
    }
  }

  for (int T = 0;; ++T) {
    // Construct Householder reflection axis...
    std::copy(std::begin(matrix), std::begin(matrix) + dim, std::begin(v));

    // ...adding something with same sign we avoid catastrophic cancellations
    v[0] += (v[0] > 0 ? 1 : -1) *
            sqrt(std::inner_product(std::begin(v), std::end(v), std::begin(v),
                                    Real(0)));

    Real s = 2 / std::inner_product(std::begin(v), std::end(v), std::begin(v),
                                    Real(0));
    for (int i = 0; i < dim; ++i) {
      w[i] = s * std::inner_product(std::begin(v), std::end(v),
                                    std::begin(matrix) + dim * i, Real(0));
    }

    // Apply Householder reflection to one row.
    for (int i = 0; i < dim; ++i) {
      current[i] = matrix[dim * i] - v[0] * w[i];
    }
    //     current = matrix[std::slice(0, dim, dim)] - v[0] * w;

    if (T >= max_iterations) {
      std::cout << "Returning because #iterations = " << T << std::endl;
      return current;
    }

    Real MSE = std::inner_product(
        std::begin(previous), std::end(previous), std::begin(current), Real(0),
        std::plus(), [](Real a, Real b) { return (a - b) * (a - b); });

    if (MSE < RMS * RMS) {
      std::cout << "Returning because RMS = " << sqrt(MSE) << std::endl;
      std::cout << "#iterations = " << T << std::endl;
      return current;
    }

    std::swap(previous, current);

    // Combine rest of Householder reflection and shift
    for (int j = 1; j < dim; ++j) {
      for (int i = 1; i < dim; ++i) {
        int k = i + dim * j;
        matrix[k - dim - 1] = matrix[k] - v[i] * w[j];
      }
    }

    // Zero out bottom row
    std::fill(std::begin(matrix) + dim * (dim - 1), std::end(matrix) - 1, 0);
    // Place filter in final column
    matrix[std::slice(dim - 1, dim, dim)] = filter;

    std::copy(std::begin(matrix), std::end(matrix),
              std::ostream_iterator<float>(std::cout, " "));
    std::cout << std::endl << "--" << std::endl;
  }
}

int main() {
  int n = 4;
  std::valarray<float> filter(n);

  // Note that random filters tend to have many zeros near the
  // unit circle so this is likely a challenging example.
  for (int i = 0; i < n; ++i) {
    filter[i] = drand48() - 0.5;
  }
  // filter[0] = 1;
  // filter[1] = 2;
  // filter[2] = 3;
  // filter[3] = 4;
  std::copy(std::begin(filter), std::end(filter),
            std::ostream_iterator<float>(std::cout, " "));
  std::cout << std::endl;

  auto result = min_phase(filter, 10000, float(1e-4));

  std::copy(std::begin(result), std::end(result),
            std::ostream_iterator<float>(std::cout, " "));
  std::cout << std::endl;
}