Developers_of_anti_slip_compensator / IIR

20160814

Fork of IIR by LDSC_Robotics_TAs

IIR.h@1:3d8226c5abfe, 2016-07-29 (annotated)

Committer:
benson516
Date:
Fri Jul 29 14:55:33 2016 +0000
Revision:
1:3d8226c5abfe
Parent:
0:a7405ea45d27 
Change the definition of the coefficient in denominator

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