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