filter.h

Go to the documentation of this file.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
#ifndef SP_FILTER_H
#define SP_FILTER_H
namespace sp
{

template <class T1, class T2, class T3> class FIR_filt
{
private:
  // Ordinary FIR filter
  arma::uword M;     
  arma::uword cur_p; 
  arma::Mat<T1> buf; 
  arma::Mat<T2> b;   
  // Adaptive LMS FIR filter
  double mu;           
  arma::uword L;       
  arma::uword blk_ctr; 
  T2 c;                
  arma::Mat<T1> P; 
  arma::Mat<T1> Q; 
  arma::Mat<T1> R; 
  arma::Mat<T1> K; 
  double lmd;      
  arma::Mat<T1> X_tpz;  
  arma::uword do_adapt; 
public:
  FIR_filt() {}

  ~FIR_filt() {}

  void clear(void)
  {
    buf.zeros();
    cur_p = 0;
  }

  void set_coeffs(const arma::Mat<T2> &_b)
  {
    M = _b.n_elem;
    buf.set_size(M, 1);
    this->clear();
    b.set_size(M, 1);
    b = _b;
  }

  void set_coeffs(const arma::Col<T2> &_b_col)
  {
    arma::Mat<T2> b_mat = arma::conv_to<arma::Mat<T2>>::from(_b_col);
    set_coeffs(b_mat);
  }

  arma::Col<T2> get_coeffs() { return arma::conv_to<arma::Col<T2>>::from(b); }

  void update_coeffs(const arma::Mat<T2> &_b) { b = _b; }

  T3 operator()(const T1 &in)
  {
    T3 out        = 0;
    arma::uword p = 0;
    buf[cur_p]    = in; // Insert new sample
    for (arma::uword m = cur_p; m < M; m++)
      out += b[p++] * buf[m]; // Calc upper part
    for (arma::uword m = 0; m < cur_p; m++)
      out += b[p++] * buf[m]; // ... and lower

    // Move insertion point
    if (cur_p == 0)
      cur_p = M - 1;
    else
      cur_p--;

    return out;
  }

  arma::Mat<T3> filter(const arma::Mat<T1> &in)
  {
    arma::uword sz = in.n_elem;
    arma::Mat<T3> out(sz, 1);
    for (arma::uword n = 0; n < sz; n++)
      out[n] = this->operator()(in[n]);
    return out;
  }
  arma::Col<T3> filter(const arma::Col<T1> &in)
  {
    arma::Mat<T1> in_col = arma::conv_to<arma::Mat<T1>>::from(in);
    return arma::conv_to<arma::Col<T3>>::from(filter(in_col));
  }

  void setup_lms(const arma::uword _N, const double _mu,
                 const arma::uword _L = 1)
  {
    M  = _N;
    mu = _mu;
    L  = _L;
    buf.set_size(M, 1);
    buf.zeros();
    b.set_size(M, 1);
    b.zeros();
    K.set_size(M, 1);
    K.zeros();
    cur_p    = 0;
    blk_ctr  = 0;
    do_adapt = 1;
  }

  void lms_adapt(const T3 _err)
  {
    if (do_adapt)
    {
      // Reshape buf
      arma::Mat<T1> buf_tmp(M, 1);
      for (arma::uword m = 0; m < M; m++)
      {
        buf_tmp(m) = buf((cur_p + m + 1) % M);
      }

      // Accumulate
      K += _err * buf_tmp;

      // Block update
      if (blk_ctr++ % L == 0)
      {
        b += 2 * mu * K / double(L);
        K.zeros();
      }
    }
  }

  void setup_nlms(const arma::uword _N, const double _mu, const T2 _c,
                  const arma::uword _L = 1)
  {
    M  = _N;
    mu = _mu;
    L  = _L;
    c  = _c;
    buf.set_size(M, 1);
    buf.zeros();
    b.set_size(M, 1);
    b.zeros();
    K.set_size(M, 1);
    K.zeros();
    cur_p    = 0;
    blk_ctr  = 0;
    do_adapt = 1;
  }

  void nlms_adapt(const T3 _err)
  {
    if (do_adapt)
    {
      // Reshape buf
      arma::Mat<T1> buf_tmp(M, 1);
      for (arma::uword m = 0; m < M; m++)
      {
        buf_tmp(m) = buf((cur_p + m + 1) % M);
      }

      // Accumulate
      T1 S = c + arma::as_scalar(buf_tmp.t() * buf_tmp);
      K += _err * buf_tmp / S;

      // Block update
      if (blk_ctr++ % L == 0)
      {
        b += 2 * mu * K / double(L);
        K.zeros();
      }
    }
  }

  void setup_newt(const arma::uword _N, const double _mu, const T2 _c,
                  const arma::uword _L = 1)
  {
    M  = _N;
    mu = _mu;
    L  = _L;
    c  = _c;
    buf.set_size(M, 1);
    buf.zeros();
    b.set_size(M, 1);
    b.zeros();
    K.set_size(M, 1);
    K.zeros();
    X_tpz.set_size(M, L);
    X_tpz.zeros();
    cur_p    = 0;
    blk_ctr  = 0;
    do_adapt = 1;
  }

  void newt_adapt(const T3 _err)
  {
    if (do_adapt)
    {
      // Reshape buf
      arma::Mat<T1> buf_tmp(M, 1);
      for (arma::uword m = 0; m < M; m++)
      {
        buf_tmp(m) = buf((cur_p + m + 1) % M);
      }

      // Accumulate in buf
      K += _err * buf_tmp;
      X_tpz.col(blk_ctr % L) = buf_tmp;

      // Block update
      if (blk_ctr++ % L == 0)
      {
        // Correlation matrix estimate
        arma::Mat<T1> Rxx = X_tpz * X_tpz.t() / L;
        arma::Mat<T1> I;
        I.eye(M, M);
        b += mu * pinv(Rxx + c * I) * K / L;
        K.zeros();
      }
    }
  }

  void setup_rls(const arma::uword _N, const double _lmd, const double _P0)
  {
    M   = _N;
    lmd = _lmd;
    L   = 1;
    P.eye(M, M);
    P = _P0 * P;
    K.set_size(M, 1);
    K.zeros();
    buf.set_size(M, 1);
    buf.zeros();
    b.set_size(M, 1);
    b.zeros();
    cur_p    = 0;
    do_adapt = 1;
  }

  void rls_adapt(const T3 _err)
  {
    if (do_adapt)
    {
      // Reshape buf
      arma::Mat<T1> buf_tmp(M, 1);
      for (arma::uword m = 0; m < M; m++)
      {
        buf_tmp(m) = buf((cur_p + m + 1) % M);
      }

      // Update P
      T1 S = lmd + arma::as_scalar(buf_tmp.t() * P * buf_tmp);
      K    = P * buf_tmp / S;
      P    = (P - K * buf_tmp.t() * P) / lmd;

      // Update coeffs
      b = b + K * _err;
    }
  }

  void setup_kalman(const arma::uword _N, const double _P0, const double _Q0,
                    const double _R0)
  {
    M = _N;
    L = 1;
    P.eye(M, M);
    P = _P0 * P;
    Q.eye(M, M);
    Q = _Q0 * Q;
    R.ones(1, 1);
    R = _R0 * R;
    K.set_size(M, 1);
    K.zeros();
    buf.set_size(M, 1);
    buf.zeros();
    b.set_size(M, 1);
    b.zeros();
    cur_p    = 0;
    do_adapt = 1;
  }

  void kalman_adapt(const T3 _err)
  {
    if (do_adapt)
    {
      // Reshape buf
      arma::Mat<T1> buf_tmp(M, 1);
      for (arma::uword m = 0; m < M; m++)
      {
        buf_tmp(m) = buf((cur_p + m + 1) % M);
      }

      // Innovation/error covariance
      T1 S = arma::as_scalar(R + buf_tmp.t() * P * buf_tmp);

      // Kalman gain
      K = P * buf_tmp / S;

      // Update coeffs/state
      b = b + K * _err;

      // Update estimate covariance
      P = P - K * buf_tmp.t() * P + Q;
    }
  }

  double get_step_size(void) { return mu; }

  arma::Mat<T1> get_P(void) { return P; }

  arma::Mat<T1> get_K(void) { return K; }

  void set_step_size(const double _mu) { mu = _mu; }

  void adapt_enable(void) { do_adapt = 1; }

  void adapt_disble(void) { do_adapt = 0; }
};

template <class T1, class T2, class T3> class IIR_filt
{
private:
  arma::uword M;       
  arma::uword N;       
  arma::uword b_cur_p; 
  arma::uword a_cur_p; 
  arma::Col<T2> b;     
  arma::Col<T2> a;     
  arma::Col<T1> b_buf; 
  arma::Col<T1> a_buf; 
public:
  IIR_filt() {}

  ~IIR_filt() {}

  void clear(void)
  {
    b_buf.zeros();
    a_buf.zeros();
    b_cur_p = 0;
    a_cur_p = 0;
  }

  void set_coeffs(const arma::Col<T2> &_b, const arma::Col<T2> &_a)
  {
    M = _b.size();
    N = _a.size();
    b_buf.set_size(M);
    a_buf.set_size(N);
    this->clear();
    b = _b / _a[0];
    a = _a / _a[0];
  }

  void update_coeffs(const arma::Col<T2> &_b, const arma::Col<T2> &_a)
  {
    b = _b / _a[0];
    a = _a / _a[0];
  }

  T3 operator()(const T1 &in)
  {
    T3 out        = 0;
    arma::uword p = 0;

    // MA part
    b_buf[b_cur_p] = in; // Insert new sample
    for (arma::uword m = b_cur_p; m < M; m++)
      out += b[p++] * b_buf[m]; // Calc upper part
    for (arma::uword m = 0; m < b_cur_p; m++)
      out += b[p++] * b_buf[m]; // ... and lower

    // Move insertion point
    if (b_cur_p == 0)
      b_cur_p = M - 1;
    else
      b_cur_p--;

    // AR part
    p = 1;
    for (arma::uword n = a_cur_p + 1; n < N; n++)
      out -= a[p++] * a_buf[n]; // Calc upper part
    for (arma::uword n = 0; n < a_cur_p; n++)
      out -= a[p++] * a_buf[n]; // ... and lower

    a_buf[a_cur_p] = out; // Insert output

    // Move insertion point
    if (a_cur_p == 0)
      a_cur_p = N - 1;
    else
      a_cur_p--;

    return out;
  }

  arma::Col<T3> filter(const arma::Col<T1> &in)
  {
    arma::uword sz = in.size();
    arma::Col<T3> out(sz);
    for (arma::uword n = 0; n < sz; n++)
      out[n] = this->operator()(in[n]);
    return out;
  }
};


arma_inline arma::vec fir1(const arma::uword M, const double f0)
{
  arma::vec b(M + 1), h(M + 1);
  h = hamming(M + 1);
  for (arma::uword m = 0; m < M + 1; m++)
  {
    b[m] = f0 * h[m] * sinc(f0 * (m - M / 2.0));
  }
  return b / arma::sum(b);
}

arma_inline arma::vec fir1_hp(const arma::uword M, const double f0)
{
  if (M % 2 != 0)
    err_handler("Filter order must be even");
  arma::vec b(M + 1), h(M + 1);
  h = hamming(M + 1);
  for (arma::uword m = 0; m < M + 1; m++)
  {
    b[m] = h[m] * (sinc(m - M / 2.0) - f0 * sinc(f0 * (m - M / 2.0)));
  }

  // Scale
  std::complex<double> i(0, 1);
  double nrm;
  arma::vec fv = arma::regspace(0, double(M));
  nrm          = abs(arma::sum(exp(-i * fv * PI) % b));

  return b / nrm;
}

arma_inline arma::vec fir1_bp(const arma::uword M, const double f0,
                              const double f1)
{
  if (f1 <= f0)
    err_handler("Frequencies must be [0 < f0 < f1 < 1]");

  arma::vec b(M + 1), h(M + 1);
  h = hamming(M + 1);
  for (arma::uword m = 0; m < M + 1; m++)
  {
    b[m] =
        h[m] * (f1 * sinc(f1 * (m - M / 2.0)) - f0 * sinc(f0 * (m - M / 2.0)));
  }

  // Scale
  double fc = (f0 + f1) / 2;
  std::complex<double> i(0, 1);
  double nrm;
  arma::vec fv = arma::regspace(0, double(M));
  nrm          = abs(arma::sum(exp(-i * fv * PI * fc) % b));

  return b / nrm;
}

arma_inline arma::vec fir1_bs(const arma::uword M, const double f0,
                              const double f1)
{
  if (M % 2 != 0)
    err_handler("Filter order must be even");
  if (f1 <= f0)
    err_handler("Frequencies must be [0 < f0 < f1 < 1]");

  arma::vec b(M + 1), h(M + 1);
  h = hamming(M + 1);
  for (arma::uword m = 0; m < M + 1; m++)
  {
    b[m] = h[m] * (sinc(m - M / 2.0) - f1 * sinc(f1 * (m - M / 2.0)) +
                   f0 * sinc(f0 * (m - M / 2.0)));
  }

  return b / arma::sum(b);
}

arma_inline arma::vec fd_filter(const arma::uword M, double fd)
{
  arma::vec h(M);
  arma::vec w = blackmanharris(M);
  if (M % 2 == 1)
    fd = fd - 0.5; // Offset for odd nr of taps
  for (arma::uword m = 0; m < M; m++)
  {
    h(m) = w(m) * sinc(m - M / 2.0 - fd);
  }
  h = h / arma::sum(h); // Normalize gain

  return h;
}

arma_inline arma::cx_vec freq(const arma::vec b, const arma::vec a,
                              const arma::uword K = 512)
{
  arma::cx_vec h(K);
  arma::uword M = b.size();
  arma::uword N = a.size();
  std::complex<double> b_tmp, a_tmp, i(0, 1);
  for (arma::uword k = 0; k < K; k++)
  {
    b_tmp = std::complex<double>(b(0), 0);
    for (arma::uword m = 1; m < M; m++)
      b_tmp += b(m) * (cos(m * PI * k / K) - i * sin(m * PI * k / K));
    a_tmp = std::complex<double>(a(0), 0);
    for (arma::uword n = 1; n < N; n++)
      a_tmp += a(n) * (cos(n * PI * k / K) - i * sin(n * PI * k / K));
    h(k) = b_tmp / a_tmp;
  }
  return h;
}

arma_inline arma::vec freqz(const arma::vec b, const arma::vec a,
                            const arma::uword K = 512)
{
  arma::cx_vec f = freq(b, a, K);
  return abs(f);
}

arma_inline arma::vec phasez(const arma::vec b, const arma::vec a,
                             const arma::uword K = 512)
{
  arma::cx_vec f = freq(b, a, K);
  return angle(f);
}

} // namespace sp
#endif