Ruprecht Altenburger / Mbed 2 deprecated RT2_P3_students_G4

Template for group 4

Dependencies:   mbed

Fork of RT2_P3_students by RT2_P3_students

IIR_filter.cpp@10:85840c065e00, 2018-04-27 (annotated)

Committer:
altb
Date:
Fri Apr 27 06:34:29 2018 +0000
Revision:
10:85840c065e00
Parent:
8:72f260c467ad 
group 4, 1

Who changed what in which revision?

UserRevisionLine numberNew 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 7:01a7363583b2 15 // G(s) = 1/(T*s + 1)
altb 2:769ce5f06d3e 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 2:769ce5f06d3e 23 B = (double*)malloc((nb+1)*sizeof(double));
altb 2:769ce5f06d3e 24 A = (double*)malloc(na*sizeof(double));
altb 8:72f260c467ad 25 B[0] = (double)Ts/((double)Ts + 2.0*(double)T);
altb 8:72f260c467ad 26 B[1] = B[0];
altb 8:72f260c467ad 27 A[0] = ((double)Ts - 2.0*(double)T)/((double)Ts + 2.0*(double)T);
altb 8:72f260c467ad 28
altb 0:78ca29b4c49e 29 // signal arrays
altb 2:769ce5f06d3e 30 uk = (double*)malloc((nb+1)*sizeof(double));
altb 2:769ce5f06d3e 31 yk = (double*)malloc(na*sizeof(double));
altb 2:769ce5f06d3e 32 uk[0]= uk[1] = 0.0;
altb 2:769ce5f06d3e 33 yk[0] = 0.0;
altb 0:78ca29b4c49e 34
altb 0:78ca29b4c49e 35 // dc-gain
altb 8:72f260c467ad 36 this->K = 1.0;
altb 0:78ca29b4c49e 37 }
altb 0:78ca29b4c49e 38
altb 0:78ca29b4c49e 39 // G(s) = K/(T*s + 1)
altb 2:769ce5f06d3e 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 2:769ce5f06d3e 47 B = (double*)malloc((nb+1)*sizeof(double));
altb 2:769ce5f06d3e 48 A = (double*)malloc(na*sizeof(double));
altb 8:72f260c467ad 49 B[0] = (double)K * (double)Ts/((double)Ts + 2.0*(double)T);
altb 0:78ca29b4c49e 50 B[1] = B[0];
altb 2:769ce5f06d3e 51 A[0] = ((double)Ts - 2.0*(double)T)/((double)Ts + 2.0*(double)T);
altb 0:78ca29b4c49e 52
altb 0:78ca29b4c49e 53 // signal arrays
altb 2:769ce5f06d3e 54 uk = (double*)malloc((nb+1)*sizeof(double));
altb 2:769ce5f06d3e 55 yk = (double*)malloc(na*sizeof(double));
altb 2:769ce5f06d3e 56 uk[0]= uk[1] = 0.0;
altb 2:769ce5f06d3e 57 yk[0] = 0.0;
altb 0:78ca29b4c49e 58
altb 0:78ca29b4c49e 59 // dc-gain
altb 2:769ce5f06d3e 60 this->K = (double)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 2:769ce5f06d3e 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 2:769ce5f06d3e 71 B = (double*)malloc((nb+1)*sizeof(double));
altb 2:769ce5f06d3e 72 A = (double*)malloc(na*sizeof(double));
altb 2:769ce5f06d3e 73 double k0 = (double)Ts*(double)Ts*(double)w0*(double)w0;
altb 2:769ce5f06d3e 74 double k1 = 4.0*(double)D*(double)Ts*(double)w0;
altb 2:769ce5f06d3e 75 double k2 = k0 + k1 + 4.0;
altb 2:769ce5f06d3e 76 B[0] = (double)K*k0/k2;
altb 2:769ce5f06d3e 77 B[1] = 2.0*B[0];
altb 0:78ca29b4c49e 78 B[2] = B[0];
altb 2:769ce5f06d3e 79 A[0] = (2.0*k0 - 8.0)/k2;
altb 2:769ce5f06d3e 80 A[1] = (k0 - k1 + 4.0)/k2;
altb 0:78ca29b4c49e 81
altb 0:78ca29b4c49e 82 // signal arrays
altb 2:769ce5f06d3e 83 uk = (double*)malloc((nb+1)*sizeof(double));
altb 2:769ce5f06d3e 84 yk = (double*)malloc(na*sizeof(double));
altb 2:769ce5f06d3e 85 uk[0]= uk[1] = uk[2] = 0.0;
altb 2:769ce5f06d3e 86 yk[0] = yk[1] = 0.0;
altb 0:78ca29b4c49e 87
altb 0:78ca29b4c49e 88 // dc-gain
altb 2:769ce5f06d3e 89 this->K = (double)K;
altb 0:78ca29b4c49e 90 }
altb 0:78ca29b4c49e 91
altb 2:769ce5f06d3e 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 2:769ce5f06d3e 99 B = (double*)malloc((nb+1)*sizeof(double));
altb 2:769ce5f06d3e 100 A = (double*)malloc(na*sizeof(double));
altb 2:769ce5f06d3e 101 uk = (double*)malloc((nb+1)*sizeof(double));
altb 2:769ce5f06d3e 102 yk = (double*)malloc(na*sizeof(double));
altb 0:78ca29b4c49e 103
altb 0:78ca29b4c49e 104 for(int k=0;k<=nb;k++){
altb 0:78ca29b4c49e 105 B[k]=b[k];
altb 2:769ce5f06d3e 106 uk[k]=0.0;
altb 0:78ca29b4c49e 107 }
altb 0:78ca29b4c49e 108 for(int k=0;k<na;k++){
altb 0:78ca29b4c49e 109 A[k] = a[k];
altb 2:769ce5f06d3e 110 yk[k] = 0.0;
altb 0:78ca29b4c49e 111 }
altb 0:78ca29b4c49e 112
altb 0:78ca29b4c49e 113 // dc-gain
altb 2:769ce5f06d3e 114 this->K = 1.0;
altb 0:78ca29b4c49e 115 }
altb 0:78ca29b4c49e 116
altb 0:78ca29b4c49e 117
altb 0:78ca29b4c49e 118 IIR_filter::~IIR_filter() {}
altb 0:78ca29b4c49e 119
altb 0:78ca29b4c49e 120 void IIR_filter::reset(float val) {
altb 0:78ca29b4c49e 121 for(int k=0;k < nb;k++)
altb 2:769ce5f06d3e 122 uk[k] = (double)val;
altb 0:78ca29b4c49e 123 for(int k=0;k < na;k++)
altb 2:769ce5f06d3e 124 yk[k] = (double)val*K;
altb 0:78ca29b4c49e 125
altb 0:78ca29b4c49e 126 }
altb 0:78ca29b4c49e 127
altb 0:78ca29b4c49e 128 /*
altb 0:78ca29b4c49e 129 the filter is operating as follows:
altb 0:78ca29b4c49e 130 (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 131 y(n) = B[0]*u(k) + B[1]*u(k-1) + ... + B[nb]*u(k-nb) + ...
altb 0:78ca29b4c49e 132 - A[0]*y(k-1) - A[1]*y(k-2) - ... - A[na]*y(n-na)
altb 0:78ca29b4c49e 133 */
altb 2:769ce5f06d3e 134 float IIR_filter::filter(double input){
altb 0:78ca29b4c49e 135 for(int k = nb;k > 0;k--) // shift input values back
altb 0:78ca29b4c49e 136 uk[k] = uk[k-1];
altb 0:78ca29b4c49e 137 uk[0] = input;
altb 2:769ce5f06d3e 138 double ret = 0.0;
altb 0:78ca29b4c49e 139 for(int k = 0;k <= nb;k++)
altb 0:78ca29b4c49e 140 ret += B[k] * uk[k];
altb 0:78ca29b4c49e 141 for(int k = 0;k < na;k++)
altb 0:78ca29b4c49e 142 ret -= A[k] * yk[k];
altb 0:78ca29b4c49e 143 for(int k = na;k > 1;k--)
altb 0:78ca29b4c49e 144 yk[k-1] = yk[k-2];
altb 0:78ca29b4c49e 145 yk[0] = ret;
altb 2:769ce5f06d3e 146 return (float)ret;
altb 0:78ca29b4c49e 147 }