James Kiwic / Mbed 2 deprecated RT2_P3_students

Fertig

Dependencies:   mbed

Fork of RT2_P3_students by TeamSurface

IIR_filter.cpp@0:78ca29b4c49e, 2018-04-03 (annotated)

Committer:
altb
Date:
Tue Apr 03 08:47:41 2018 +0000
Revision:
0:78ca29b4c49e
Child:
3:769ce5f06d3e
Cuboid Lab RT2 FS 2018

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 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 }*/