IIR.cpp

00001 #include "IIR.h"
00002 /*-----------------------------*/
00003 //   Order of the numerator: m
00004 //   Orser of the denominator: n
00005 //--------------------------------//
00006 //
00007 //    y     gain*( sum(i from 0 to m) b[i]*x[k-i] )
00008 //   --- = --------------------------------------------
00009 //    u     1 + sum( i from 1 to n) a[i]*y[k-i]
00010 //
00011 //   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]
00012 //
00013 //   Compatible with the coefficient generated by Matlab fdatool
00014 //
00015 //   by C.F. Huang from LDSC
00016 /*-----------------------------*/
00017 
00018 Circular_buffer::Circular_buffer(){
00019     n = 0;
00020     ind = 0;
00021 }
00022 Circular_buffer::Circular_buffer(int n_in){
00023     n = n_in;
00024     A.resize(n);
00025     for (size_t i=0; i< A.size(); i++)
00026         A[i] = 0;
00027 
00028     ind = 0;
00029 }
00030 //
00031 void Circular_buffer::Init(int n_in){ // Input: buffer size
00032     n = n_in;
00033     A.resize(n);
00034     for (size_t i=0; i< A.size(); i++)
00035         A[i] = 0;
00036     ind = 0;
00037 }
00038 //
00039 float Circular_buffer::Get(int i){ // Get the value of the (t-i)th element, where t stands for the current sample
00040     int ind_e = ind-i;
00041     while (ind_e < 0){
00042         ind_e += A.size();
00043     }
00044     return A[ind_e];
00045 }
00046 void Circular_buffer::Insert(float x_new){ // Pop the oldest element and push a new element
00047     ind++;
00048     while (ind >= A.size()){
00049         ind -= A.size();
00050     }
00051     A[ind] = x_new;
00052 }
00053 void Circular_buffer::reset(float val){ // Reset all elements to val
00054     for (size_t i = 0; i< A.size(); i++)
00055         A[i] = val;
00056     ind = 0;
00057 }
00058 
00059 // IIR
00060 //---------------------------------------//
00061 IIR::IIR(int m, int n): m(m),n(n) {
00062     x.Init(m+1);
00063     y.Init(n);
00064 }
00065 //
00066 void IIR::Assign_parameters(float b_in[], float a_in[], float gain_in){
00067     //b.assign(b_in,b_in+m+1);
00068     // a.assign(a_in,a_in+n);
00069     b.resize(m+1);
00070     a.resize(n);
00071     for (size_t i = 0; i < n; i++){
00072         a[i] = a_in[i+1];
00073     }
00074     gain = gain_in;
00075     for (size_t i = 0; i < b.size(); i++){
00076         b[i] = gain*b_in[i];
00077     }
00078 }
00079 //
00080 float IIR::Iterate_once(float x_in){
00081     x.Insert(x_in);
00082     float y_new = 0;
00083     // Numerator
00084     for (int i = 0; i <= m; i++ ){
00085         y_new += b[i]*x.Get(i);
00086     }
00087 
00088     // Denominator
00089     for (int i = 0; i < n; i++){
00090         y_new -= a[i]*y.Get(i);
00091     }
00092     y.Insert(y_new);
00093     return y_new;
00094 }
00095 void IIR::Reset(float val){ // Rest all elements to val
00096     x.reset(val);
00097     y.reset(val);
00098 }