Fertig
Dependencies: mbed
Fork of RT2_P3_students by
Diff: IIR_filter.cpp
- Revision:
- 0:78ca29b4c49e
- Child:
- 3:769ce5f06d3e
diff -r 000000000000 -r 78ca29b4c49e IIR_filter.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IIR_filter.cpp Tue Apr 03 08:47:41 2018 +0000 @@ -0,0 +1,171 @@ +#include "IIR_filter.h" +#include "mbed.h" +using namespace std; + +/* + IIR filter implemention for the following filter types: + init for: first order differentiatior: G(s) = s/(T*s + 1) + first order lowpass with gain G(s) = K/(T*s + 1) + second order lowpass with gain G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0*w0) + nth order, with arbitrary values + the billinear transformation is used for s -> z + reseting the filter only makes sence for static signals, whatch out if you're using the differnetiator +*/ + +// G(s) = s/(T*s + 1) +IIR_filter::IIR_filter(float T,float Ts){ + + // filter orders + nb = 1; // Filter Order + na = 1; // Filter Order + + // filter coefficients + B = (float*)malloc((nb+1)*sizeof(float)); + A = (float*)malloc(na*sizeof(float)); + B[0] = 2.0f/(2.0f*T + Ts); + B[1] = -B[0]; + A[0] = -(2.0f*T - Ts)/(2.0f*T + Ts); + + // signal arrays + uk = (float*)malloc((nb+1)*sizeof(float)); + yk = (float*)malloc(na*sizeof(float)); + uk[0]= uk[1] = 0.0f; + yk[0] = 0.0f; + + // dc-gain + this->K = 0.0f; +} + +// G(s) = K/(T*s + 1) +IIR_filter::IIR_filter(float T,float Ts,float K){ + + // filter orders + nb = 1; // Filter Order + na = 1; // Filter Order + + // filter coefficients + B = (float*)malloc((nb+1)*sizeof(float)); + A = (float*)malloc(na*sizeof(float)); + B[0] = Ts/(Ts + 2.0f*T); + B[1] = B[0]; + A[0] = (Ts - 2.0f*T)/(Ts + 2.0f*T); + + // signal arrays + uk = (float*)malloc((nb+1)*sizeof(float)); + yk = (float*)malloc(na*sizeof(float)); + uk[0]= uk[1] = 0.0f; + yk[0] = 0.0f; + + // dc-gain + this->K = K; +} + +// G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0^2) +IIR_filter::IIR_filter(float w0,float D, float Ts, float K){ + + // filter orders + nb = 2; // Filter Order + na = 2; // Filter Order + + // filter coefficients + B = (float*)malloc((nb+1)*sizeof(float)); + A = (float*)malloc(na*sizeof(float)); + float k0 = Ts*Ts*w0*w0; + float k1 = 4.0f*D*Ts*w0; + float k2 = k0 + k1 + 4.0f; + B[0] = K*k0/k2; + B[1] = 2.0f*B[0]; + B[2] = B[0]; + A[0] = (2.0f*k0 - 8.0f)/k2; + A[1] = (k0 - k1 + 4.0f)/k2; + + // signal arrays + uk = (float*)malloc((nb+1)*sizeof(float)); + yk = (float*)malloc(na*sizeof(float)); + uk[0]= uk[1] = uk[2] = 0.0f; + yk[0] = yk[1] = 0.0f; + + // dc-gain + this->K = K; +} + +IIR_filter::IIR_filter(float *b,float *a,int nb_, int na_){ + + // filter orders + this->nb = nb_-1; // Filter Order + this->na = na_; // Filter Order + + // filter coefficients + B = (float*)malloc((nb+1)*sizeof(float)); + A = (float*)malloc(na*sizeof(float)); + uk = (float*)malloc((nb+1)*sizeof(float)); + yk = (float*)malloc(na*sizeof(float)); + + for(int k=0;k<=nb;k++){ + B[k]=b[k]; + uk[k]=0.0f; + } + for(int k=0;k<na;k++){ + A[k] = a[k]; + yk[k] = 0.0f; + } + //B[0] = K*k0/k2; + //B[1] = 2.0f*B[0]; + //B[2] = B[0]; + //A[0] = (2.0f*k0 - 8.0f)/k2; + //A[1] = (k0 - k1 + 4.0f)/k2; + + // dc-gain + this->K = 1.0f; +} + + +IIR_filter::~IIR_filter() {} + +void IIR_filter::reset(float val) { + for(int k=0;k < nb;k++) + uk[k] = val; + for(int k=0;k < na;k++) + yk[k] = val*K; + +} + +/* + the filter is operating as follows: + (B[0] + B[1]*z^-1 + ... + B[nb]*z^-nb)*U(z) = (1 + A[0]*z^-1 + ... + A[na-1]*z^-na))*Y(z) + y(n) = B[0]*u(k) + B[1]*u(k-1) + ... + B[nb]*u(k-nb) + ... + - A[0]*y(k-1) - A[1]*y(k-2) - ... - A[na]*y(n-na) +*/ +float IIR_filter::filter(float input){ + for(int k = nb;k > 0;k--) // shift input values back + uk[k] = uk[k-1]; + uk[0] = input; + float ret = 0.0f; + for(int k = 0;k <= nb;k++) + ret += B[k] * uk[k]; + for(int k = 0;k < na;k++) + ret -= A[k] * yk[k]; + for(int k = na;k > 1;k--) + yk[k-1] = yk[k-2]; + yk[0] = ret; + return ret; +} + +// (B[0] + B[1]*z^-1 + ... + B[nb]*z^-nb)*U(z) = (1 + A[0]*z^-1 + ... + A[na-1]*z^-na))*Y(z) +/* +IIR_filter::IIR_filter(float *a[], float *b[], float K){ + + this->A = A[0]; + this->B = B[0]; + nb = sizeof(B)/sizeof(B[0]); + na = sizeof(A)/sizeof(A[0]); + uk = (float*)malloc((nb+1)*sizeof(float)); + yk = (float*)malloc(na*sizeof(float)); + for(int ii=0; ii<nb; ii++){ + uk[ii] = 0.0f; + } + for(int ii=0; ii<na; ii++){ + yk[ii] = 0.0f; + } + this->K = K; %%% THIS IMPLEMENTATION SUITS NOT THE RESET PROCESS %%% +}*/ \ No newline at end of file