LDSC_Robotics_TAs / IIR

Infinite impulse response filter

Dependents:   WIPV

IIR.h@2:0bae37b297f4, 2017-01-10 (annotated)

Committer:
benson516
Date:
Tue Jan 10 19:45:40 2017 +0000
Revision:
2:0bae37b297f4
Parent:
1:3d8226c5abfe
Child:
3:7191d27f4855
Add reset function for resetting all elements without resize the buffer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 2:0bae37b297f4 1 #ifndef IIR_H
benson516 2:0bae37b297f4 2 #define IIR_H
benson516 2:0bae37b297f4 3
benson516 0:a7405ea45d27 4 #include <vector>
benson516 0:a7405ea45d27 5 using std::vector;
benson516 1:3d8226c5abfe 6 /*-----------------------------*/
benson516 2:0bae37b297f4 7 // Order of the numerator: m
benson516 2:0bae37b297f4 8 // Orser of the denominator: n
benson516 2:0bae37b297f4 9 //--------------------------------//
benson516 1:3d8226c5abfe 10 //
benson516 1:3d8226c5abfe 11 // y gain*( sum(i from 0 to m) b[i]*x[k-i] )
benson516 1:3d8226c5abfe 12 // --- = --------------------------------------------
benson516 1:3d8226c5abfe 13 // u 1 + sum( i from 1 to n) a[i]*y[k-i]
benson516 1:3d8226c5abfe 14 //
benson516 1:3d8226c5abfe 15 // 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]
benson516 2:0bae37b297f4 16 //
benson516 2:0bae37b297f4 17 // Compatible with the coefficient generated by Matlab fdatool
benson516 1:3d8226c5abfe 18 //
benson516 1:3d8226c5abfe 19 // by C.F. Huang from LDSC
benson516 1:3d8226c5abfe 20 /*-----------------------------*/
benson516 0:a7405ea45d27 21
benson516 0:a7405ea45d27 22 class Circular_buffer{
benson516 0:a7405ea45d27 23 private:
benson516 2:0bae37b297f4 24 size_t n;
benson516 0:a7405ea45d27 25 vector<float> A;
benson516 0:a7405ea45d27 26 // vector<float>::iterator it;
benson516 0:a7405ea45d27 27 int ind; // The index of the newest element
benson516 2:0bae37b297f4 28
benson516 0:a7405ea45d27 29 public:
benson516 0:a7405ea45d27 30 Circular_buffer(){
benson516 2:0bae37b297f4 31 n = 0;
benson516 0:a7405ea45d27 32 ind = 0;
benson516 0:a7405ea45d27 33 }
benson516 2:0bae37b297f4 34 Circular_buffer(int n_in){
benson516 2:0bae37b297f4 35 n = n_in;
benson516 0:a7405ea45d27 36 A.resize(n);
benson516 0:a7405ea45d27 37 for (size_t i=0; i< A.size(); i++)
benson516 0:a7405ea45d27 38 A[i] = 0;
benson516 2:0bae37b297f4 39
benson516 0:a7405ea45d27 40 ind = 0;
benson516 0:a7405ea45d27 41 }
benson516 2:0bae37b297f4 42
benson516 2:0bae37b297f4 43 void Init(int n_in){ // Input: buffer size
benson516 2:0bae37b297f4 44 n = n_in;
benson516 0:a7405ea45d27 45 A.resize(n);
benson516 0:a7405ea45d27 46 for (size_t i=0; i< A.size(); i++)
benson516 0:a7405ea45d27 47 A[i] = 0;
benson516 0:a7405ea45d27 48 ind = 0;
benson516 0:a7405ea45d27 49 }
benson516 2:0bae37b297f4 50
benson516 0:a7405ea45d27 51 float Get(int i){ // Get the value of the (t-i)th element, where t stands for the current sample
benson516 0:a7405ea45d27 52 int ind_e = ind-i;
benson516 0:a7405ea45d27 53 while (ind_e < 0){
benson516 0:a7405ea45d27 54 ind_e += A.size();
benson516 0:a7405ea45d27 55 }
benson516 0:a7405ea45d27 56 return A[ind_e];
benson516 0:a7405ea45d27 57 }
benson516 2:0bae37b297f4 58 void Insert(float x_new){ // Pop the oldest element and push a new element
benson516 0:a7405ea45d27 59 ind++;
benson516 0:a7405ea45d27 60 while (ind >= A.size()){
benson516 0:a7405ea45d27 61 ind -= A.size();
benson516 2:0bae37b297f4 62 }
benson516 0:a7405ea45d27 63 A[ind] = x_new;
benson516 0:a7405ea45d27 64 }
benson516 2:0bae37b297f4 65 void reset(float val){ // Reset all elements to val
benson516 2:0bae37b297f4 66 for (size_t i = 0; i< A.size(); i++)
benson516 2:0bae37b297f4 67 A[i] = val;
benson516 2:0bae37b297f4 68 ind = 0;
benson516 2:0bae37b297f4 69 }
benson516 0:a7405ea45d27 70 };
benson516 0:a7405ea45d27 71
benson516 0:a7405ea45d27 72 class IIR{
benson516 0:a7405ea45d27 73 private:
benson516 2:0bae37b297f4 74
benson516 0:a7405ea45d27 75 int m; // the order of numerator
benson516 0:a7405ea45d27 76 int n; // the order of denominator
benson516 0:a7405ea45d27 77 Circular_buffer x,y;
benson516 0:a7405ea45d27 78 vector<float> b; // parameters of numerator, highest order --> lowest order (newest -> oldest)
benson516 0:a7405ea45d27 79 vector<float> a; // parameters of denominator, highest order --> lowest order (newest -> oldest)
benson516 0:a7405ea45d27 80 float gain;
benson516 2:0bae37b297f4 81
benson516 0:a7405ea45d27 82 public:
benson516 0:a7405ea45d27 83 IIR(int m, int n): m(m),n(n) {
benson516 0:a7405ea45d27 84 x.Init(m+1);
benson516 2:0bae37b297f4 85 y.Init(n);
benson516 0:a7405ea45d27 86 }
benson516 0:a7405ea45d27 87 void Assign_parameters(float b_in[], float a_in[], float gain_in){
benson516 1:3d8226c5abfe 88 //b.assign(b_in,b_in+m+1);
benson516 1:3d8226c5abfe 89 // a.assign(a_in,a_in+n);
benson516 1:3d8226c5abfe 90 b.resize(m+1);
benson516 1:3d8226c5abfe 91 a.resize(n);
benson516 1:3d8226c5abfe 92 for (size_t i = 0; i < n; i++){
benson516 1:3d8226c5abfe 93 a[i] = a_in[i+1];
benson516 1:3d8226c5abfe 94 }
benson516 0:a7405ea45d27 95 gain = gain_in;
benson516 0:a7405ea45d27 96 for (size_t i = 0; i < b.size(); i++){
benson516 1:3d8226c5abfe 97 b[i] = gain*b_in[i];
benson516 0:a7405ea45d27 98 }
benson516 0:a7405ea45d27 99 }
benson516 0:a7405ea45d27 100 float Iterate_once(float x_in){
benson516 0:a7405ea45d27 101 x.Insert(x_in);
benson516 0:a7405ea45d27 102 float y_new = 0;
benson516 0:a7405ea45d27 103 // Numerator
benson516 0:a7405ea45d27 104 for (int i = 0; i <= m; i++ ){
benson516 0:a7405ea45d27 105 y_new += b[i]*x.Get(i);
benson516 0:a7405ea45d27 106 }
benson516 2:0bae37b297f4 107
benson516 0:a7405ea45d27 108 // Denominator
benson516 0:a7405ea45d27 109 for (int i = 0; i < n; i++){
benson516 0:a7405ea45d27 110 y_new -= a[i]*y.Get(i);
benson516 0:a7405ea45d27 111 }
benson516 0:a7405ea45d27 112 y.Insert(y_new);
benson516 0:a7405ea45d27 113 return y_new;
benson516 0:a7405ea45d27 114 }
benson516 2:0bae37b297f4 115 void Reset(float val){ // Rest all elements to val
benson516 2:0bae37b297f4 116 x.reset(val);
benson516 2:0bae37b297f4 117 y.reset(val);
benson516 2:0bae37b297f4 118 }
benson516 0:a7405ea45d27 119 };
benson516 2:0bae37b297f4 120
benson516 2:0bae37b297f4 121 #endif