Ruprecht Altenburger / Library_Misc_cuboid

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?

UserRevisionLine numberNew 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