.
Fork of Cntrlol_Lib by
Embed:
(wiki syntax)
Show/hide line numbers
IIR_filter.cpp
00001 #include "IIR_filter.h" 00002 #include "mbed.h" 00003 using namespace std; 00004 00005 /* 00006 IIR filter implemention for the following filter types: 00007 init for: first order differentiatior: G(s) = s/(T*s + 1) 00008 first order lowpass with gain G(s) = K/(T*s + 1) 00009 second order lowpass with gain G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0*w0) 00010 nth order, with arbitrary values 00011 the billinear transformation is used for s -> z 00012 reseting the filter only makes sence for static signals, whatch out if you're using the differnetiator 00013 */ 00014 00015 // G(s) = s/(T*s + 1) 00016 IIR_filter::IIR_filter(float T, float Ts){ 00017 00018 // filter orders 00019 nb = 1; // Filter Order 00020 na = 1; // Filter Order 00021 00022 // filter coefficients 00023 B = (double*)malloc((nb+1)*sizeof(double)); 00024 A = (double*)malloc(na*sizeof(double)); 00025 B[0] = 2.0/(2.0*(double)T + (double)Ts); 00026 B[1] = -B[0]; 00027 A[0] = -(2.0*(double)T - (double)Ts)/(2.0*(double)T + (double)Ts); 00028 00029 // signal arrays 00030 uk = (double*)malloc((nb+1)*sizeof(double)); 00031 yk = (double*)malloc(na*sizeof(double)); 00032 uk[0]= uk[1] = 0.0; 00033 yk[0] = 0.0; 00034 00035 // dc-gain 00036 this->K = 0.0; 00037 } 00038 00039 // G(s) = K/(T*s + 1) 00040 IIR_filter::IIR_filter(float T, float Ts, float K){ 00041 00042 // filter orders 00043 nb = 1; // Filter Order 00044 na = 1; // Filter Order 00045 00046 // filter coefficients 00047 B = (double*)malloc((nb+1)*sizeof(double)); 00048 A = (double*)malloc(na*sizeof(double)); 00049 B[0] = (double)Ts/((double)Ts + 2.0*(double)T); 00050 B[1] = B[0]; 00051 A[0] = ((double)Ts - 2.0*(double)T)/((double)Ts + 2.0*(double)T); 00052 00053 // signal arrays 00054 uk = (double*)malloc((nb+1)*sizeof(double)); 00055 yk = (double*)malloc(na*sizeof(double)); 00056 uk[0]= uk[1] = 0.0; 00057 yk[0] = 0.0; 00058 00059 // dc-gain 00060 this->K = (double)K; 00061 } 00062 00063 // G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0^2) 00064 IIR_filter::IIR_filter(float w0, float D, float Ts, float K){ 00065 00066 // filter orders 00067 nb = 2; // Filter Order 00068 na = 2; // Filter Order 00069 00070 // filter coefficients 00071 B = (double*)malloc((nb+1)*sizeof(double)); 00072 A = (double*)malloc(na*sizeof(double)); 00073 double k0 = (double)Ts*(double)Ts*(double)w0*(double)w0; 00074 double k1 = 4.0*(double)D*(double)Ts*(double)w0; 00075 double k2 = k0 + k1 + 4.0; 00076 B[0] = (double)K*k0/k2; 00077 B[1] = 2.0*B[0]; 00078 B[2] = B[0]; 00079 A[0] = (2.0*k0 - 8.0)/k2; 00080 A[1] = (k0 - k1 + 4.0)/k2; 00081 00082 // signal arrays 00083 uk = (double*)malloc((nb+1)*sizeof(double)); 00084 yk = (double*)malloc(na*sizeof(double)); 00085 uk[0]= uk[1] = uk[2] = 0.0; 00086 yk[0] = yk[1] = 0.0; 00087 00088 // dc-gain 00089 this->K = (double)K; 00090 } 00091 00092 IIR_filter::IIR_filter(float *b, float *a, int nb_, int na_){ 00093 00094 // filter orders 00095 this->nb = nb_-1; // Filter Order 00096 this->na = na_; // Filter Order 00097 00098 // filter coefficients 00099 B = (double*)malloc((nb+1)*sizeof(double)); 00100 A = (double*)malloc(na*sizeof(double)); 00101 uk = (double*)malloc((nb+1)*sizeof(double)); 00102 yk = (double*)malloc(na*sizeof(double)); 00103 00104 for(int k=0;k<=nb;k++){ 00105 B[k]=b[k]; 00106 uk[k]=0.0; 00107 } 00108 for(int k=0;k<na;k++){ 00109 A[k] = a[k]; 00110 yk[k] = 0.0; 00111 } 00112 00113 // dc-gain 00114 this->K = 1.0; 00115 } 00116 00117 00118 IIR_filter::~IIR_filter() {} 00119 00120 void IIR_filter::reset(float val) { 00121 for(int k=0;k < nb;k++) 00122 uk[k] = (double)val; 00123 for(int k=0;k < na;k++) 00124 yk[k] = (double)val*K; 00125 00126 } 00127 00128 /* 00129 the filter is operating as follows: 00130 (B[0] + B[1]*z^-1 + ... + B[nb]*z^-nb)*U(z) = (1 + A[0]*z^-1 + ... + A[na-1]*z^-na))*Y(z) 00131 y(n) = B[0]*u(k) + B[1]*u(k-1) + ... + B[nb]*u(k-nb) + ... 00132 - A[0]*y(k-1) - A[1]*y(k-2) - ... - A[na]*y(n-na) 00133 */ 00134 float IIR_filter::filter(double input){ 00135 for(int k = nb;k > 0;k--) // shift input values back 00136 uk[k] = uk[k-1]; 00137 uk[0] = input; 00138 double ret = 0.0; 00139 for(int k = 0;k <= nb;k++) 00140 ret += B[k] * uk[k]; 00141 for(int k = 0;k < na;k++) 00142 ret -= A[k] * yk[k]; 00143 for(int k = na;k > 1;k--) 00144 yk[k-1] = yk[k-2]; 00145 yk[0] = ret; 00146 return (float)ret; 00147 }
Generated on Wed Jul 20 2022 13:09:12 by 1.7.2