branch for cuboid

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers IIR_filter.cpp Source File

IIR_filter.cpp

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