Ruprecht Altenburger
/
RT2_P3_students_G4
Template for group 4
Fork of RT2_P3_students by
IIR_filter.cpp@0:78ca29b4c49e, 2018-04-03 (annotated)
- Committer:
- altb
- Date:
- Tue Apr 03 08:47:41 2018 +0000
- Revision:
- 0:78ca29b4c49e
- Child:
- 2:769ce5f06d3e
Cuboid Lab RT2 FS 2018
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb | 0:78ca29b4c49e | 1 | #include "IIR_filter.h" |
altb | 0:78ca29b4c49e | 2 | #include "mbed.h" |
altb | 0:78ca29b4c49e | 3 | using namespace std; |
altb | 0:78ca29b4c49e | 4 | |
altb | 0:78ca29b4c49e | 5 | /* |
altb | 0:78ca29b4c49e | 6 | IIR filter implemention for the following filter types: |
altb | 0:78ca29b4c49e | 7 | init for: first order differentiatior: G(s) = s/(T*s + 1) |
altb | 0:78ca29b4c49e | 8 | first order lowpass with gain G(s) = K/(T*s + 1) |
altb | 0:78ca29b4c49e | 9 | second order lowpass with gain G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0*w0) |
altb | 0:78ca29b4c49e | 10 | nth order, with arbitrary values |
altb | 0:78ca29b4c49e | 11 | the billinear transformation is used for s -> z |
altb | 0:78ca29b4c49e | 12 | reseting the filter only makes sence for static signals, whatch out if you're using the differnetiator |
altb | 0:78ca29b4c49e | 13 | */ |
altb | 0:78ca29b4c49e | 14 | |
altb | 0:78ca29b4c49e | 15 | // G(s) = s/(T*s + 1) |
altb | 0:78ca29b4c49e | 16 | IIR_filter::IIR_filter(float T,float Ts){ |
altb | 0:78ca29b4c49e | 17 | |
altb | 0:78ca29b4c49e | 18 | // filter orders |
altb | 0:78ca29b4c49e | 19 | nb = 1; // Filter Order |
altb | 0:78ca29b4c49e | 20 | na = 1; // Filter Order |
altb | 0:78ca29b4c49e | 21 | |
altb | 0:78ca29b4c49e | 22 | // filter coefficients |
altb | 0:78ca29b4c49e | 23 | B = (float*)malloc((nb+1)*sizeof(float)); |
altb | 0:78ca29b4c49e | 24 | A = (float*)malloc(na*sizeof(float)); |
altb | 0:78ca29b4c49e | 25 | B[0] = 2.0f/(2.0f*T + Ts); |
altb | 0:78ca29b4c49e | 26 | B[1] = -B[0]; |
altb | 0:78ca29b4c49e | 27 | A[0] = -(2.0f*T - Ts)/(2.0f*T + Ts); |
altb | 0:78ca29b4c49e | 28 | |
altb | 0:78ca29b4c49e | 29 | // signal arrays |
altb | 0:78ca29b4c49e | 30 | uk = (float*)malloc((nb+1)*sizeof(float)); |
altb | 0:78ca29b4c49e | 31 | yk = (float*)malloc(na*sizeof(float)); |
altb | 0:78ca29b4c49e | 32 | uk[0]= uk[1] = 0.0f; |
altb | 0:78ca29b4c49e | 33 | yk[0] = 0.0f; |
altb | 0:78ca29b4c49e | 34 | |
altb | 0:78ca29b4c49e | 35 | // dc-gain |
altb | 0:78ca29b4c49e | 36 | this->K = 0.0f; |
altb | 0:78ca29b4c49e | 37 | } |
altb | 0:78ca29b4c49e | 38 | |
altb | 0:78ca29b4c49e | 39 | // G(s) = K/(T*s + 1) |
altb | 0:78ca29b4c49e | 40 | IIR_filter::IIR_filter(float T,float Ts,float K){ |
altb | 0:78ca29b4c49e | 41 | |
altb | 0:78ca29b4c49e | 42 | // filter orders |
altb | 0:78ca29b4c49e | 43 | nb = 1; // Filter Order |
altb | 0:78ca29b4c49e | 44 | na = 1; // Filter Order |
altb | 0:78ca29b4c49e | 45 | |
altb | 0:78ca29b4c49e | 46 | // filter coefficients |
altb | 0:78ca29b4c49e | 47 | B = (float*)malloc((nb+1)*sizeof(float)); |
altb | 0:78ca29b4c49e | 48 | A = (float*)malloc(na*sizeof(float)); |
altb | 0:78ca29b4c49e | 49 | B[0] = Ts/(Ts + 2.0f*T); |
altb | 0:78ca29b4c49e | 50 | B[1] = B[0]; |
altb | 0:78ca29b4c49e | 51 | A[0] = (Ts - 2.0f*T)/(Ts + 2.0f*T); |
altb | 0:78ca29b4c49e | 52 | |
altb | 0:78ca29b4c49e | 53 | // signal arrays |
altb | 0:78ca29b4c49e | 54 | uk = (float*)malloc((nb+1)*sizeof(float)); |
altb | 0:78ca29b4c49e | 55 | yk = (float*)malloc(na*sizeof(float)); |
altb | 0:78ca29b4c49e | 56 | uk[0]= uk[1] = 0.0f; |
altb | 0:78ca29b4c49e | 57 | yk[0] = 0.0f; |
altb | 0:78ca29b4c49e | 58 | |
altb | 0:78ca29b4c49e | 59 | // dc-gain |
altb | 0:78ca29b4c49e | 60 | this->K = K; |
altb | 0:78ca29b4c49e | 61 | } |
altb | 0:78ca29b4c49e | 62 | |
altb | 0:78ca29b4c49e | 63 | // G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0^2) |
altb | 0:78ca29b4c49e | 64 | IIR_filter::IIR_filter(float w0,float D, float Ts, float K){ |
altb | 0:78ca29b4c49e | 65 | |
altb | 0:78ca29b4c49e | 66 | // filter orders |
altb | 0:78ca29b4c49e | 67 | nb = 2; // Filter Order |
altb | 0:78ca29b4c49e | 68 | na = 2; // Filter Order |
altb | 0:78ca29b4c49e | 69 | |
altb | 0:78ca29b4c49e | 70 | // filter coefficients |
altb | 0:78ca29b4c49e | 71 | B = (float*)malloc((nb+1)*sizeof(float)); |
altb | 0:78ca29b4c49e | 72 | A = (float*)malloc(na*sizeof(float)); |
altb | 0:78ca29b4c49e | 73 | float k0 = Ts*Ts*w0*w0; |
altb | 0:78ca29b4c49e | 74 | float k1 = 4.0f*D*Ts*w0; |
altb | 0:78ca29b4c49e | 75 | float k2 = k0 + k1 + 4.0f; |
altb | 0:78ca29b4c49e | 76 | B[0] = K*k0/k2; |
altb | 0:78ca29b4c49e | 77 | B[1] = 2.0f*B[0]; |
altb | 0:78ca29b4c49e | 78 | B[2] = B[0]; |
altb | 0:78ca29b4c49e | 79 | A[0] = (2.0f*k0 - 8.0f)/k2; |
altb | 0:78ca29b4c49e | 80 | A[1] = (k0 - k1 + 4.0f)/k2; |
altb | 0:78ca29b4c49e | 81 | |
altb | 0:78ca29b4c49e | 82 | // signal arrays |
altb | 0:78ca29b4c49e | 83 | uk = (float*)malloc((nb+1)*sizeof(float)); |
altb | 0:78ca29b4c49e | 84 | yk = (float*)malloc(na*sizeof(float)); |
altb | 0:78ca29b4c49e | 85 | uk[0]= uk[1] = uk[2] = 0.0f; |
altb | 0:78ca29b4c49e | 86 | yk[0] = yk[1] = 0.0f; |
altb | 0:78ca29b4c49e | 87 | |
altb | 0:78ca29b4c49e | 88 | // dc-gain |
altb | 0:78ca29b4c49e | 89 | this->K = K; |
altb | 0:78ca29b4c49e | 90 | } |
altb | 0:78ca29b4c49e | 91 | |
altb | 0:78ca29b4c49e | 92 | IIR_filter::IIR_filter(float *b,float *a,int nb_, int na_){ |
altb | 0:78ca29b4c49e | 93 | |
altb | 0:78ca29b4c49e | 94 | // filter orders |
altb | 0:78ca29b4c49e | 95 | this->nb = nb_-1; // Filter Order |
altb | 0:78ca29b4c49e | 96 | this->na = na_; // Filter Order |
altb | 0:78ca29b4c49e | 97 | |
altb | 0:78ca29b4c49e | 98 | // filter coefficients |
altb | 0:78ca29b4c49e | 99 | B = (float*)malloc((nb+1)*sizeof(float)); |
altb | 0:78ca29b4c49e | 100 | A = (float*)malloc(na*sizeof(float)); |
altb | 0:78ca29b4c49e | 101 | uk = (float*)malloc((nb+1)*sizeof(float)); |
altb | 0:78ca29b4c49e | 102 | yk = (float*)malloc(na*sizeof(float)); |
altb | 0:78ca29b4c49e | 103 | |
altb | 0:78ca29b4c49e | 104 | for(int k=0;k<=nb;k++){ |
altb | 0:78ca29b4c49e | 105 | B[k]=b[k]; |
altb | 0:78ca29b4c49e | 106 | uk[k]=0.0f; |
altb | 0:78ca29b4c49e | 107 | } |
altb | 0:78ca29b4c49e | 108 | for(int k=0;k<na;k++){ |
altb | 0:78ca29b4c49e | 109 | A[k] = a[k]; |
altb | 0:78ca29b4c49e | 110 | yk[k] = 0.0f; |
altb | 0:78ca29b4c49e | 111 | } |
altb | 0:78ca29b4c49e | 112 | //B[0] = K*k0/k2; |
altb | 0:78ca29b4c49e | 113 | //B[1] = 2.0f*B[0]; |
altb | 0:78ca29b4c49e | 114 | //B[2] = B[0]; |
altb | 0:78ca29b4c49e | 115 | //A[0] = (2.0f*k0 - 8.0f)/k2; |
altb | 0:78ca29b4c49e | 116 | //A[1] = (k0 - k1 + 4.0f)/k2; |
altb | 0:78ca29b4c49e | 117 | |
altb | 0:78ca29b4c49e | 118 | // dc-gain |
altb | 0:78ca29b4c49e | 119 | this->K = 1.0f; |
altb | 0:78ca29b4c49e | 120 | } |
altb | 0:78ca29b4c49e | 121 | |
altb | 0:78ca29b4c49e | 122 | |
altb | 0:78ca29b4c49e | 123 | IIR_filter::~IIR_filter() {} |
altb | 0:78ca29b4c49e | 124 | |
altb | 0:78ca29b4c49e | 125 | void IIR_filter::reset(float val) { |
altb | 0:78ca29b4c49e | 126 | for(int k=0;k < nb;k++) |
altb | 0:78ca29b4c49e | 127 | uk[k] = val; |
altb | 0:78ca29b4c49e | 128 | for(int k=0;k < na;k++) |
altb | 0:78ca29b4c49e | 129 | yk[k] = val*K; |
altb | 0:78ca29b4c49e | 130 | |
altb | 0:78ca29b4c49e | 131 | } |
altb | 0:78ca29b4c49e | 132 | |
altb | 0:78ca29b4c49e | 133 | /* |
altb | 0:78ca29b4c49e | 134 | the filter is operating as follows: |
altb | 0:78ca29b4c49e | 135 | (B[0] + B[1]*z^-1 + ... + B[nb]*z^-nb)*U(z) = (1 + A[0]*z^-1 + ... + A[na-1]*z^-na))*Y(z) |
altb | 0:78ca29b4c49e | 136 | y(n) = B[0]*u(k) + B[1]*u(k-1) + ... + B[nb]*u(k-nb) + ... |
altb | 0:78ca29b4c49e | 137 | - A[0]*y(k-1) - A[1]*y(k-2) - ... - A[na]*y(n-na) |
altb | 0:78ca29b4c49e | 138 | */ |
altb | 0:78ca29b4c49e | 139 | float IIR_filter::filter(float input){ |
altb | 0:78ca29b4c49e | 140 | for(int k = nb;k > 0;k--) // shift input values back |
altb | 0:78ca29b4c49e | 141 | uk[k] = uk[k-1]; |
altb | 0:78ca29b4c49e | 142 | uk[0] = input; |
altb | 0:78ca29b4c49e | 143 | float ret = 0.0f; |
altb | 0:78ca29b4c49e | 144 | for(int k = 0;k <= nb;k++) |
altb | 0:78ca29b4c49e | 145 | ret += B[k] * uk[k]; |
altb | 0:78ca29b4c49e | 146 | for(int k = 0;k < na;k++) |
altb | 0:78ca29b4c49e | 147 | ret -= A[k] * yk[k]; |
altb | 0:78ca29b4c49e | 148 | for(int k = na;k > 1;k--) |
altb | 0:78ca29b4c49e | 149 | yk[k-1] = yk[k-2]; |
altb | 0:78ca29b4c49e | 150 | yk[0] = ret; |
altb | 0:78ca29b4c49e | 151 | return ret; |
altb | 0:78ca29b4c49e | 152 | } |
altb | 0:78ca29b4c49e | 153 | |
altb | 0:78ca29b4c49e | 154 | // (B[0] + B[1]*z^-1 + ... + B[nb]*z^-nb)*U(z) = (1 + A[0]*z^-1 + ... + A[na-1]*z^-na))*Y(z) |
altb | 0:78ca29b4c49e | 155 | /* |
altb | 0:78ca29b4c49e | 156 | IIR_filter::IIR_filter(float *a[], float *b[], float K){ |
altb | 0:78ca29b4c49e | 157 | |
altb | 0:78ca29b4c49e | 158 | this->A = A[0]; |
altb | 0:78ca29b4c49e | 159 | this->B = B[0]; |
altb | 0:78ca29b4c49e | 160 | nb = sizeof(B)/sizeof(B[0]); |
altb | 0:78ca29b4c49e | 161 | na = sizeof(A)/sizeof(A[0]); |
altb | 0:78ca29b4c49e | 162 | uk = (float*)malloc((nb+1)*sizeof(float)); |
altb | 0:78ca29b4c49e | 163 | yk = (float*)malloc(na*sizeof(float)); |
altb | 0:78ca29b4c49e | 164 | for(int ii=0; ii<nb; ii++){ |
altb | 0:78ca29b4c49e | 165 | uk[ii] = 0.0f; |
altb | 0:78ca29b4c49e | 166 | } |
altb | 0:78ca29b4c49e | 167 | for(int ii=0; ii<na; ii++){ |
altb | 0:78ca29b4c49e | 168 | yk[ii] = 0.0f; |
altb | 0:78ca29b4c49e | 169 | } |
altb | 0:78ca29b4c49e | 170 | this->K = K; %%% THIS IMPLEMENTATION SUITS NOT THE RESET PROCESS %%% |
altb | 0:78ca29b4c49e | 171 | }*/ |