LDSC_Robotics_TAs / IIR

Infinite impulse response filter

Dependents:   WIPV

IIR.h@0:a7405ea45d27, 2016-07-29 (annotated)

Committer:
benson516
Date:
Fri Jul 29 14:26:54 2016 +0000
Revision:
0:a7405ea45d27
Child:
1:3d8226c5abfe
Infinite impulse response filter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:a7405ea45d27 1 #include <vector>
benson516 0:a7405ea45d27 2 using std::vector;
benson516 0:a7405ea45d27 3
benson516 0:a7405ea45d27 4 class Circular_buffer{
benson516 0:a7405ea45d27 5 private:
benson516 0:a7405ea45d27 6 vector<float> A;
benson516 0:a7405ea45d27 7 // vector<float>::iterator it;
benson516 0:a7405ea45d27 8 int ind; // The index of the newest element
benson516 0:a7405ea45d27 9
benson516 0:a7405ea45d27 10 public:
benson516 0:a7405ea45d27 11 Circular_buffer(){
benson516 0:a7405ea45d27 12 ind = 0;
benson516 0:a7405ea45d27 13 }
benson516 0:a7405ea45d27 14 Circular_buffer(int n){
benson516 0:a7405ea45d27 15 A.resize(n);
benson516 0:a7405ea45d27 16 for (size_t i=0; i< A.size(); i++)
benson516 0:a7405ea45d27 17 A[i] = 0;
benson516 0:a7405ea45d27 18
benson516 0:a7405ea45d27 19 ind = 0;
benson516 0:a7405ea45d27 20 }
benson516 0:a7405ea45d27 21
benson516 0:a7405ea45d27 22 void Init(int n){ // Input: buffer size
benson516 0:a7405ea45d27 23 A.resize(n);
benson516 0:a7405ea45d27 24 for (size_t i=0; i< A.size(); i++)
benson516 0:a7405ea45d27 25 A[i] = 0;
benson516 0:a7405ea45d27 26 ind = 0;
benson516 0:a7405ea45d27 27 }
benson516 0:a7405ea45d27 28
benson516 0:a7405ea45d27 29 float Get(int i){ // Get the value of the (t-i)th element, where t stands for the current sample
benson516 0:a7405ea45d27 30 int ind_e = ind-i;
benson516 0:a7405ea45d27 31 while (ind_e < 0){
benson516 0:a7405ea45d27 32 ind_e += A.size();
benson516 0:a7405ea45d27 33 }
benson516 0:a7405ea45d27 34 return A[ind_e];
benson516 0:a7405ea45d27 35 }
benson516 0:a7405ea45d27 36 void Insert(float x_new){ // Pop the oldest element and push a new element
benson516 0:a7405ea45d27 37 ind++;
benson516 0:a7405ea45d27 38 while (ind >= A.size()){
benson516 0:a7405ea45d27 39 ind -= A.size();
benson516 0:a7405ea45d27 40 }
benson516 0:a7405ea45d27 41 A[ind] = x_new;
benson516 0:a7405ea45d27 42 }
benson516 0:a7405ea45d27 43 };
benson516 0:a7405ea45d27 44
benson516 0:a7405ea45d27 45 class IIR{
benson516 0:a7405ea45d27 46 private:
benson516 0:a7405ea45d27 47
benson516 0:a7405ea45d27 48 int m; // the order of numerator
benson516 0:a7405ea45d27 49 int n; // the order of denominator
benson516 0:a7405ea45d27 50 Circular_buffer x,y;
benson516 0:a7405ea45d27 51 vector<float> b; // parameters of numerator, highest order --> lowest order (newest -> oldest)
benson516 0:a7405ea45d27 52 vector<float> a; // parameters of denominator, highest order --> lowest order (newest -> oldest)
benson516 0:a7405ea45d27 53 float gain;
benson516 0:a7405ea45d27 54
benson516 0:a7405ea45d27 55 public:
benson516 0:a7405ea45d27 56 IIR(int m, int n): m(m),n(n) {
benson516 0:a7405ea45d27 57 x.Init(m+1);
benson516 0:a7405ea45d27 58 y.Init(n);
benson516 0:a7405ea45d27 59 }
benson516 0:a7405ea45d27 60 void Assign_parameters(float b_in[], float a_in[], float gain_in){
benson516 0:a7405ea45d27 61 b.assign(b_in,b_in+m+1);
benson516 0:a7405ea45d27 62 a.assign(a_in,a_in+n);
benson516 0:a7405ea45d27 63 gain = gain_in;
benson516 0:a7405ea45d27 64 for (size_t i = 0; i < b.size(); i++){
benson516 0:a7405ea45d27 65 b[i] *= gain;
benson516 0:a7405ea45d27 66 }
benson516 0:a7405ea45d27 67 }
benson516 0:a7405ea45d27 68 float Iterate_once(float x_in){
benson516 0:a7405ea45d27 69 x.Insert(x_in);
benson516 0:a7405ea45d27 70 float y_new = 0;
benson516 0:a7405ea45d27 71 // Numerator
benson516 0:a7405ea45d27 72 for (int i = 0; i <= m; i++ ){
benson516 0:a7405ea45d27 73 y_new += b[i]*x.Get(i);
benson516 0:a7405ea45d27 74 }
benson516 0:a7405ea45d27 75
benson516 0:a7405ea45d27 76 // Denominator
benson516 0:a7405ea45d27 77 for (int i = 0; i < n; i++){
benson516 0:a7405ea45d27 78 y_new -= a[i]*y.Get(i);
benson516 0:a7405ea45d27 79 }
benson516 0:a7405ea45d27 80 y.Insert(y_new);
benson516 0:a7405ea45d27 81 return y_new;
benson516 0:a7405ea45d27 82 }
benson516 0:a7405ea45d27 83 };