Infinite impulse response filter
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?
User | Revision | Line number | New 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 |