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) = 1/(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] = (double)Ts/((double)Ts + 2.0*(double)T);
00026     B[1] = B[0];
00027     A[0] = ((double)Ts - 2.0*(double)T)/((double)Ts + 2.0*(double)T); 
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 = 1.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)K * (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 }