Control Library by altb
Dependents: My_Libraries IndNav_QK3_T265
Revision 15:c70cad2f4e64, committed 2020-01-16
- Comitter:
- pmic
- Date:
- Thu Jan 16 09:12:50 2020 +0000
- Parent:
- 14:9184aa9fdac7
- Commit message:
- Revisit IIR_filter.h and IIR_filter.cpp. Change internal double to float arithmetic.
Changed in this revision
IIR_filter.cpp | Show annotated file Show diff for this revision Revisions of this file |
IIR_filter.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 9184aa9fdac7 -r c70cad2f4e64 IIR_filter.cpp --- a/IIR_filter.cpp Mon Oct 28 07:53:02 2019 +0000 +++ b/IIR_filter.cpp Thu Jan 16 09:12:50 2020 +0000 @@ -1,150 +1,148 @@ #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) + second order lowpass with gain G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0^2) 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 + billinear transformation is used for s -> z + reseting the filter only makes sence for static signals, whatch out if you're using the differnetiator, static corresponds to output null */ // 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 + nb = 1; + na = 1; // filter coefficients - B = (double*)malloc((nb+1)*sizeof(double)); - A = (double*)malloc(na*sizeof(double)); - B[0] = 2.0/(2.0*(double)T + (double)Ts); + 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.0*(double)T - (double)Ts)/(2.0*(double)T + (double)Ts); + A[0] = -(2.0f*T - Ts)/(2.0f*T + Ts); // signal arrays - uk = (double*)malloc((nb+1)*sizeof(double)); - yk = (double*)malloc(na*sizeof(double)); - uk[0]= uk[1] = 0.0; - yk[0] = 0.0; + 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.0; + this->K = 0.0f; } // 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 + nb = 1; + na = 1; // filter coefficients - B = (double*)malloc((nb+1)*sizeof(double)); - A = (double*)malloc(na*sizeof(double)); - B[0] = (double)Ts/((double)Ts + 2.0*(double)T); + 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] = ((double)Ts - 2.0*(double)T)/((double)Ts + 2.0*(double)T); + A[0] = (Ts - 2.0f*T)/(Ts + 2.0f*T); // signal arrays - uk = (double*)malloc((nb+1)*sizeof(double)); - yk = (double*)malloc(na*sizeof(double)); - uk[0]= uk[1] = 0.0; - yk[0] = 0.0; + 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 = (double)K; -} - -void IIR_filter::setup(float T, float Ts, float K){ - - // filter orders - nb = 1; // Filter Order - na = 1; // Filter Order - - // filter coefficients - 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] = ((double)Ts - 2.0*(double)T)/((double)Ts + 2.0*(double)T); - - // signal arrays - 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 = (double)K; + 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){ +IIR_filter::IIR_filter(float w0, float D, float Ts, float K) { // filter orders - nb = 2; // Filter Order - na = 2; // Filter Order + nb = 2; + na = 2; // filter coefficients - 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 = (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.0*k0 - 8.0)/k2; - A[1] = (k0 - k1 + 4.0)/k2; + A[0] = (2.0f*k0 - 8.0f)/k2; + A[1] = (k0 - k1 + 4.0f)/k2; // signal arrays - 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; + 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 = (double)K; + this->K = 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 + this->nb = nb - 1; + this->na = na; // filter coefficients - 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)); + 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.0; - } - for(int k=0;k<na;k++){ + for(uint8_t k=0;k<=nb;k++) { + B[k] = b[k]; + uk[k] = 0.0f; + } + for(uint8_t k=0;k<na;k++) { A[k] = a[k]; - yk[k] = 0.0; - } + yk[k] = 0.0f; + } // dc-gain - this->K = 1.0; + this->K = 1.0f; } IIR_filter::~IIR_filter() {} + +void IIR_filter::setup(float T, float Ts, float K) { + // filter orders + nb = 1; + na = 1; + + // 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; +} + void IIR_filter::reset(float val) { - for(int k=0;k < nb;k++) - uk[k] = (double)val; - for(int k=0;k < na;k++) - yk[k] = (double)val*K; + for(uint8_t k=0;k < nb;k++) + uk[k] = val; + for(uint8_t k=0;k < na;k++) + yk[k] = val*K; } @@ -154,17 +152,18 @@ 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(double input){ - for(int k = nb;k > 0;k--) // shift input values back +float IIR_filter::filter(float input) { + + for(uint8_t k = nb;k > 0;k--) // shift input values back uk[k] = uk[k-1]; uk[0] = input; - double ret = 0.0; - for(int k = 0;k <= nb;k++) + float ret = 0.0f; + for(uint8_t k = 0;k <= nb;k++) ret += B[k] * uk[k]; - for(int k = 0;k < na;k++) + for(uint8_t k = 0;k < na;k++) ret -= A[k] * yk[k]; - for(int k = na;k > 1;k--) + for(uint8_t k = na;k > 1;k--) yk[k-1] = yk[k-2]; yk[0] = ret; - return (float)ret; + return ret; }
diff -r 9184aa9fdac7 -r c70cad2f4e64 IIR_filter.h --- a/IIR_filter.h Mon Oct 28 07:53:02 2019 +0000 +++ b/IIR_filter.h Thu Jan 16 09:12:50 2020 +0000 @@ -1,29 +1,36 @@ #ifndef IIR_FILTER_H_ #define IIR_FILTER_H_ + +#include "mbed.h" + class IIR_filter{ + public: - IIR_filter(){}; IIR_filter(float T, float Ts); IIR_filter(float T, float Ts, float K); IIR_filter(float w0, float D, float Ts, float K); - IIR_filter(float *b, float *a, int nb_, int na_); + IIR_filter(float *b, float *a, int nb, int na); + IIR_filter() {}; float operator()(float u){ - return filter((double)u); + return filter(u); } - virtual ~IIR_filter(); - void reset(float); - float filter(double); - void setup(float,float,float); + + virtual ~IIR_filter(); + + void setup(float T, float Ts, float K); + void reset(float val); + float filter(float input); + private: - unsigned int nb; - unsigned int na; - double *B; - double *A; - double *uk; - double *yk; - double K; + uint8_t nb; + uint8_t na; + float *B; + float *A; + float *uk; + float *yk; + float K; }; #endif \ No newline at end of file