![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Fertig
Dependencies: mbed
Fork of RT2_P3_students by
Diff: IIR_filter.cpp
- Revision:
- 3:769ce5f06d3e
- Parent:
- 0:78ca29b4c49e
diff -r a30512c3ac73 -r 769ce5f06d3e IIR_filter.cpp --- a/IIR_filter.cpp Mon Apr 09 05:50:04 2018 +0000 +++ b/IIR_filter.cpp Mon Apr 09 08:01:29 2018 +0000 @@ -13,110 +13,105 @@ */ // G(s) = s/(T*s + 1) -IIR_filter::IIR_filter(float T,float Ts){ +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 = (double*)malloc((nb+1)*sizeof(double)); + A = (double*)malloc(na*sizeof(double)); + B[0] = 2.0/(2.0*(double)T + (double)Ts); B[1] = -B[0]; - A[0] = -(2.0f*T - Ts)/(2.0f*T + Ts); + A[0] = -(2.0*(double)T - (double)Ts)/(2.0*(double)T + (double)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; + uk = (double*)malloc((nb+1)*sizeof(double)); + yk = (double*)malloc(na*sizeof(double)); + uk[0]= uk[1] = 0.0; + yk[0] = 0.0; // dc-gain - this->K = 0.0f; + this->K = 0.0; } // G(s) = K/(T*s + 1) -IIR_filter::IIR_filter(float T,float Ts,float K){ +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 = (double*)malloc((nb+1)*sizeof(double)); + A = (double*)malloc(na*sizeof(double)); + B[0] = (double)Ts/((double)Ts + 2.0*(double)T); B[1] = B[0]; - A[0] = (Ts - 2.0f*T)/(Ts + 2.0f*T); + A[0] = ((double)Ts - 2.0*(double)T)/((double)Ts + 2.0*(double)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; + uk = (double*)malloc((nb+1)*sizeof(double)); + yk = (double*)malloc(na*sizeof(double)); + uk[0]= uk[1] = 0.0; + yk[0] = 0.0; // dc-gain - this->K = K; + this->K = (double)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){ +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 = (double*)malloc((nb+1)*sizeof(double)); + A = (double*)malloc(na*sizeof(double)); + double k0 = (double)Ts*(double)Ts*(double)w0*(double)w0; + double k1 = 4.0*(double)D*(double)Ts*(double)w0; + double k2 = k0 + k1 + 4.0; + B[0] = (double)K*k0/k2; + B[1] = 2.0*B[0]; B[2] = B[0]; - A[0] = (2.0f*k0 - 8.0f)/k2; - A[1] = (k0 - k1 + 4.0f)/k2; + A[0] = (2.0*k0 - 8.0)/k2; + A[1] = (k0 - k1 + 4.0)/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; + uk = (double*)malloc((nb+1)*sizeof(double)); + yk = (double*)malloc(na*sizeof(double)); + uk[0]= uk[1] = uk[2] = 0.0; + yk[0] = yk[1] = 0.0; // dc-gain - this->K = K; + this->K = (double)K; } -IIR_filter::IIR_filter(float *b,float *a,int nb_, int na_){ +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)); + B = (double*)malloc((nb+1)*sizeof(double)); + A = (double*)malloc(na*sizeof(double)); + uk = (double*)malloc((nb+1)*sizeof(double)); + yk = (double*)malloc(na*sizeof(double)); for(int k=0;k<=nb;k++){ B[k]=b[k]; - uk[k]=0.0f; + uk[k]=0.0; } for(int k=0;k<na;k++){ A[k] = a[k]; - yk[k] = 0.0f; + yk[k] = 0.0; } - //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; + this->K = 1.0; } @@ -124,9 +119,9 @@ void IIR_filter::reset(float val) { for(int k=0;k < nb;k++) - uk[k] = val; + uk[k] = (double)val; for(int k=0;k < na;k++) - yk[k] = val*K; + yk[k] = (double)val*K; } @@ -136,11 +131,11 @@ 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){ +float IIR_filter::filter(double input){ for(int k = nb;k > 0;k--) // shift input values back uk[k] = uk[k-1]; uk[0] = input; - float ret = 0.0f; + double ret = 0.0; for(int k = 0;k <= nb;k++) ret += B[k] * uk[k]; for(int k = 0;k < na;k++) @@ -148,24 +143,5 @@ for(int k = na;k > 1;k--) yk[k-1] = yk[k-2]; yk[0] = ret; - return ret; + return (float)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