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