File content as of revision 1:3d8226c5abfe:

#include <vector>
using std::vector;
/*-----------------------------*/
//
//    y     gain*( sum(i from 0 to m) b[i]*x[k-i] )
//   --- = --------------------------------------------
//    u     1 + sum( i from 1 to n) a[i]*y[k-i]
//
//   y[k] = gain*( sum(i from 0 to m) b[i]*x[k-i] ) - sum( i from 1 to n) a[i]*y[k-i]
//   
//   Compatible with the coefficient generated by Matlab fdatool 
//
//   by C.F. Huang from LDSC
/*-----------------------------*/

class Circular_buffer{
private:
    vector<float> A;
    // vector<float>::iterator it;
    int ind; // The index of the newest element
    
public:
    Circular_buffer(){
        ind = 0;
    }
    Circular_buffer(int n){
        A.resize(n);
        for (size_t i=0; i< A.size(); i++)
            A[i] = 0;
            
        ind = 0;
    }
    
    void Init(int n){ // Input: buffer size
        A.resize(n);
        for (size_t i=0; i< A.size(); i++)
            A[i] = 0;
        ind = 0;
    }
    
    float Get(int i){ // Get the value of the (t-i)th element, where t stands for the current sample
        int ind_e = ind-i;
        while (ind_e < 0){
            ind_e += A.size();
        }
        return A[ind_e];
    }
    void Insert(float x_new){ // Pop the oldest element and push a new element 
        ind++;
        while (ind >= A.size()){
            ind -= A.size();
        } 
        A[ind] = x_new;
    }
};

class IIR{
private:
    
    int m; // the order of numerator
    int n; // the order of denominator
    Circular_buffer x,y;
    vector<float> b; // parameters of numerator, highest order --> lowest order (newest -> oldest)
    vector<float> a; // parameters of denominator, highest order --> lowest order (newest -> oldest)
    float gain;
    
public:
    IIR(int m, int n): m(m),n(n) {
        x.Init(m+1);
        y.Init(n);      
    }
    void Assign_parameters(float b_in[], float a_in[], float gain_in){
        //b.assign(b_in,b_in+m+1);
        // a.assign(a_in,a_in+n);
        b.resize(m+1);
        a.resize(n);
        for (size_t i = 0; i < n; i++){
            a[i] = a_in[i+1];
        }
        gain = gain_in;
        for (size_t i = 0; i < b.size(); i++){
            b[i] = gain*b_in[i];
        }
    }
    float Iterate_once(float x_in){
        x.Insert(x_in);
        float y_new = 0;
        // Numerator
        for (int i = 0; i <= m; i++ ){
            y_new += b[i]*x.Get(i);
        }
        
        // Denominator
        for (int i = 0; i < n; i++){
            y_new -= a[i]*y.Get(i);
        }
        y.Insert(y_new);
        return y_new;
    }
};