enhanced functionality in V01 vs. V00, V02 finished, conversion to double precsision in V03

Dependencies:   mbed

Committer:
pmic
Date:
Mon Apr 09 17:50:45 2018 +0000
Revision:
22:c895fa4d7319
Parent:
18:44760408b75e
all in float!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rtlabor 0:15be70d21d7c 1 #include "IIR_filter.h"
rtlabor 0:15be70d21d7c 2 #include "mbed.h"
rtlabor 0:15be70d21d7c 3 using namespace std;
pmic 7:b3c5116e9fab 4
pmic 7:b3c5116e9fab 5 /*
pmic 7:b3c5116e9fab 6 IIR filter implemention for the following filter types:
pmic 7:b3c5116e9fab 7 init for: first order differentiatior: G(s) = s/(T*s + 1)
pmic 7:b3c5116e9fab 8 first order lowpass with gain G(s) = K/(T*s + 1)
pmic 7:b3c5116e9fab 9 second order lowpass with gain G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0*w0)
pmic 12:e54941459353 10 nth order, with arbitrary values
pmic 7:b3c5116e9fab 11 the billinear transformation is used for s -> z
pmic 7:b3c5116e9fab 12 reseting the filter only makes sence for static signals, whatch out if you're using the differnetiator
pmic 7:b3c5116e9fab 13 */
pmic 7:b3c5116e9fab 14
pmic 7:b3c5116e9fab 15 // G(s) = s/(T*s + 1)
pmic 18:44760408b75e 16 IIR_filter::IIR_filter(float T, float Ts){
pmic 7:b3c5116e9fab 17
pmic 7:b3c5116e9fab 18 // filter orders
pmic 7:b3c5116e9fab 19 nb = 1; // Filter Order
pmic 7:b3c5116e9fab 20 na = 1; // Filter Order
rtlabor 0:15be70d21d7c 21
pmic 7:b3c5116e9fab 22 // filter coefficients
pmic 18:44760408b75e 23 B = (double*)malloc((nb+1)*sizeof(double));
pmic 18:44760408b75e 24 A = (double*)malloc(na*sizeof(double));
pmic 18:44760408b75e 25 B[0] = 2.0/(2.0*(double)T + (double)Ts);
pmic 7:b3c5116e9fab 26 B[1] = -B[0];
pmic 18:44760408b75e 27 A[0] = -(2.0*(double)T - (double)Ts)/(2.0*(double)T + (double)Ts);
pmic 7:b3c5116e9fab 28
pmic 7:b3c5116e9fab 29 // signal arrays
pmic 18:44760408b75e 30 uk = (double*)malloc((nb+1)*sizeof(double));
pmic 18:44760408b75e 31 yk = (double*)malloc(na*sizeof(double));
pmic 18:44760408b75e 32 uk[0]= uk[1] = 0.0;
pmic 18:44760408b75e 33 yk[0] = 0.0;
pmic 7:b3c5116e9fab 34
pmic 7:b3c5116e9fab 35 // dc-gain
pmic 18:44760408b75e 36 this->K = 0.0;
pmic 7:b3c5116e9fab 37 }
pmic 7:b3c5116e9fab 38
pmic 7:b3c5116e9fab 39 // G(s) = K/(T*s + 1)
pmic 18:44760408b75e 40 IIR_filter::IIR_filter(float T, float Ts, float K){
pmic 7:b3c5116e9fab 41
pmic 7:b3c5116e9fab 42 // filter orders
rtlabor 0:15be70d21d7c 43 nb = 1; // Filter Order
rtlabor 0:15be70d21d7c 44 na = 1; // Filter Order
pmic 7:b3c5116e9fab 45
pmic 7:b3c5116e9fab 46 // filter coefficients
pmic 18:44760408b75e 47 B = (double*)malloc((nb+1)*sizeof(double));
pmic 18:44760408b75e 48 A = (double*)malloc(na*sizeof(double));
pmic 18:44760408b75e 49 B[0] = (double)Ts/((double)Ts + 2.0*(double)T);
pmic 7:b3c5116e9fab 50 B[1] = B[0];
pmic 18:44760408b75e 51 A[0] = ((double)Ts - 2.0*(double)T)/((double)Ts + 2.0*(double)T);
pmic 7:b3c5116e9fab 52
pmic 7:b3c5116e9fab 53 // signal arrays
pmic 18:44760408b75e 54 uk = (double*)malloc((nb+1)*sizeof(double));
pmic 18:44760408b75e 55 yk = (double*)malloc(na*sizeof(double));
pmic 18:44760408b75e 56 uk[0]= uk[1] = 0.0;
pmic 18:44760408b75e 57 yk[0] = 0.0;
pmic 7:b3c5116e9fab 58
pmic 7:b3c5116e9fab 59 // dc-gain
pmic 18:44760408b75e 60 this->K = (double)K;
pmic 7:b3c5116e9fab 61 }
pmic 7:b3c5116e9fab 62
pmic 7:b3c5116e9fab 63 // G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0^2)
pmic 18:44760408b75e 64 IIR_filter::IIR_filter(float w0, float D, float Ts, float K){
pmic 7:b3c5116e9fab 65
pmic 7:b3c5116e9fab 66 // filter orders
pmic 7:b3c5116e9fab 67 nb = 2; // Filter Order
pmic 7:b3c5116e9fab 68 na = 2; // Filter Order
pmic 7:b3c5116e9fab 69
pmic 7:b3c5116e9fab 70 // filter coefficients
pmic 18:44760408b75e 71 B = (double*)malloc((nb+1)*sizeof(double));
pmic 18:44760408b75e 72 A = (double*)malloc(na*sizeof(double));
pmic 18:44760408b75e 73 double k0 = (double)Ts*(double)Ts*(double)w0*(double)w0;
pmic 18:44760408b75e 74 double k1 = 4.0*(double)D*(double)Ts*(double)w0;
pmic 18:44760408b75e 75 double k2 = k0 + k1 + 4.0;
pmic 18:44760408b75e 76 B[0] = (double)K*k0/k2;
pmic 18:44760408b75e 77 B[1] = 2.0*B[0];
pmic 7:b3c5116e9fab 78 B[2] = B[0];
pmic 18:44760408b75e 79 A[0] = (2.0*k0 - 8.0)/k2;
pmic 18:44760408b75e 80 A[1] = (k0 - k1 + 4.0)/k2;
pmic 7:b3c5116e9fab 81
pmic 7:b3c5116e9fab 82 // signal arrays
pmic 18:44760408b75e 83 uk = (double*)malloc((nb+1)*sizeof(double));
pmic 18:44760408b75e 84 yk = (double*)malloc(na*sizeof(double));
pmic 18:44760408b75e 85 uk[0]= uk[1] = uk[2] = 0.0;
pmic 18:44760408b75e 86 yk[0] = yk[1] = 0.0;
pmic 7:b3c5116e9fab 87
pmic 7:b3c5116e9fab 88 // dc-gain
pmic 18:44760408b75e 89 this->K = (double)K;
pmic 7:b3c5116e9fab 90 }
pmic 12:e54941459353 91
pmic 18:44760408b75e 92 IIR_filter::IIR_filter(float *b, float *a, int nb_, int na_){
pmic 12:e54941459353 93
pmic 12:e54941459353 94 // filter orders
pmic 12:e54941459353 95 this->nb = nb_-1; // Filter Order
pmic 12:e54941459353 96 this->na = na_; // Filter Order
pmic 12:e54941459353 97
pmic 12:e54941459353 98 // filter coefficients
pmic 18:44760408b75e 99 B = (double*)malloc((nb+1)*sizeof(double));
pmic 18:44760408b75e 100 A = (double*)malloc(na*sizeof(double));
pmic 18:44760408b75e 101 uk = (double*)malloc((nb+1)*sizeof(double));
pmic 18:44760408b75e 102 yk = (double*)malloc(na*sizeof(double));
pmic 12:e54941459353 103
pmic 12:e54941459353 104 for(int k=0;k<=nb;k++){
pmic 12:e54941459353 105 B[k]=b[k];
pmic 18:44760408b75e 106 uk[k]=0.0;
pmic 12:e54941459353 107 }
pmic 12:e54941459353 108 for(int k=0;k<na;k++){
pmic 12:e54941459353 109 A[k] = a[k];
pmic 18:44760408b75e 110 yk[k] = 0.0;
pmic 12:e54941459353 111 }
pmic 12:e54941459353 112
pmic 12:e54941459353 113 // dc-gain
pmic 18:44760408b75e 114 this->K = 1.0;
pmic 12:e54941459353 115 }
pmic 12:e54941459353 116
rtlabor 0:15be70d21d7c 117
rtlabor 0:15be70d21d7c 118 IIR_filter::~IIR_filter() {}
rtlabor 0:15be70d21d7c 119
rtlabor 0:15be70d21d7c 120 void IIR_filter::reset(float val) {
pmic 7:b3c5116e9fab 121 for(int k=0;k < nb;k++)
pmic 18:44760408b75e 122 uk[k] = (double)val;
pmic 7:b3c5116e9fab 123 for(int k=0;k < na;k++)
pmic 18:44760408b75e 124 yk[k] = (double)val*K;
rtlabor 0:15be70d21d7c 125
rtlabor 0:15be70d21d7c 126 }
pmic 7:b3c5116e9fab 127
pmic 7:b3c5116e9fab 128 /*
pmic 7:b3c5116e9fab 129 the filter is operating as follows:
pmic 7:b3c5116e9fab 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)
pmic 7:b3c5116e9fab 131 y(n) = B[0]*u(k) + B[1]*u(k-1) + ... + B[nb]*u(k-nb) + ...
pmic 7:b3c5116e9fab 132 - A[0]*y(k-1) - A[1]*y(k-2) - ... - A[na]*y(n-na)
rtlabor 0:15be70d21d7c 133 */
pmic 18:44760408b75e 134 float IIR_filter::filter(double input){
pmic 7:b3c5116e9fab 135 for(int k = nb;k > 0;k--) // shift input values back
rtlabor 0:15be70d21d7c 136 uk[k] = uk[k-1];
rtlabor 0:15be70d21d7c 137 uk[0] = input;
pmic 18:44760408b75e 138 double ret = 0.0;
pmic 7:b3c5116e9fab 139 for(int k = 0;k <= nb;k++)
pmic 7:b3c5116e9fab 140 ret += B[k] * uk[k];
pmic 7:b3c5116e9fab 141 for(int k = 0;k < na;k++)
pmic 7:b3c5116e9fab 142 ret -= A[k] * yk[k];
pmic 7:b3c5116e9fab 143 for(int k = na;k > 1;k--)
rtlabor 0:15be70d21d7c 144 yk[k-1] = yk[k-2];
rtlabor 0:15be70d21d7c 145 yk[0] = ret;
pmic 18:44760408b75e 146 return (float)ret;
pmic 7:b3c5116e9fab 147 }