- Main Page
- Classes
- Files
- Examples

- File List

# fastDepukfb.H

    00001 #ifndef FASTDEPUKFB_H_
    00002 #define FASTDEPUKFB_H_
    00003 #include <string.h>
    00004 #include "depukfb.H"
    00005
    00006 class FastDepUKFB : public DepUKFB {
    00007
    00008   double n_l[2], d_l[3]; // Lower filter IIR coeff.
    00009   double n_u[2], d_u[3]; // Upper filter IIR coeff.
    00010
    00011   void findIIRCoeff(double fc, double pl, double pu){
    00012     double c1, c2, c3, c4; // Numerator coefficients
    00013     double d1, d2, d3, d4; // Denominator coefficients
    00014
    00015     // Find the lower filter's IIR coefficients first
    00016     c1=exp(pl/fc);
    00017     c2=exp(pl);
    00018     //c5=c1*c1;
    00019     c4=c1*c2;
    00020     c3=c1*c4;
    00021     d_l[0]=(c2*fc); // Do this first so we can divide by it
    00022     n_l[0]=(fc+fc*pl)/d_l[0]; // Numerator
    00023     n_l[1]=(-c1*fc-c1*pl-c1*fc*pl)/d_l[0];
    00024     d_l[1]=(-2*c4*fc)/d_l[0]; // Denominator
    00025     d_l[2]=(c3*fc)/d_l[0];
    00026     d_l[0]=1.0;
    00027
    00028     // Find the upper filter's IIR coefficients
    00029     d4=exp(pu/fc);
    00030     d3=d4*d4;
    00031     d2=exp(pu+pu/fc);
    00032     d1=d2*d4;
    00033
    00034     d_u[0]=(d3*fc);
    00035     n_u[0]=(d1*fc-d1*fc*pu)/d_u[0]; // Numerator
    00036     n_u[1]=d2*(-fc+pu+pu*fc)/d_u[0];
    00037     d_u[1]=(-2*d4*fc)/d_u[0]; // Denominator
    00038     d_u[2]=(fc)/d_u[0];
    00039     d_u[0]=1.0;
    00040   }
    00041
    00042   void filter(double fc, double *out){
    00043     //Second order impulse response
    00044
    00045     // Reset the state vars and the output
    00046     double z1=0.0, z2=0.0;
    00047     bzero(out, (int)rint(fs/2.0)*sizeof(double));
    00048
    00049     // Upper filter ...
    00050     // First excite with a unit input ....
    00051     out[0] = n_u[0] - d_u[0]*out[0] + z1;
    00052     z1     = n_u[1] - d_u[1]*out[0] + z2;
    00053     z2     =        - d_u[2]*out[0];
    00054
    00055     for (int i=1;i<(int)rint(fs/2.0);i++){ // the input now equals zero
    00056       out[i] = - d_u[0]*out[i] + z1;
    00057       z1 =     - d_u[1]*out[i] + z2;
    00058       z2 =     - d_u[2]*out[i];
    00059     }
    00060
    00061     // Reset the state vars and the lower output
    00062     z1=z2=0.0;
    00063     bzero(out, (int)rint(fc)*sizeof(double));
    00064
    00065     // Lower filter ...
    00066     // First excite with a unit input ....
    00067     out[0] = n_l[0] - d_l[0]*out[0] + z1;
    00068     z1     = n_l[1] - d_l[1]*out[0] + z2;
    00069     z2     =        - d_l[2]*out[0];
    00070
    00071     for (int i=1;i<(int)rint(fc);i++){ // the input now equals zero
    00072       out[i] = - d_l[0]*out[i] + z1;
    00073       z1 =     - d_l[1]*out[i] + z2;
    00074       z2 =     - d_l[2]*out[i];
    00075     }
    00076   }
    00077
    00078   void afZ(double fc, int whichFilter, double pl, double pu){
    00079     double *filt=w[whichFilter];
    00080     findIIRCoeff(fc, pl, pu); // Find the IIR coefficients to filter with
    00081     filter(fc, filt); // Find the lower filter shape
    00082   }
    00083
    00085   virtual void af(double fc, int whichFilter){
    00086     cout<<"FastDepUKFB::af"<<endl;
    00087
    00088     // Produce the filter
    00089     afZ(fc, whichFilter, p_l(fc), p_u(fc));
    00090   }
    00091
    00092 public:
    00093   FastDepUKFB(int sampleFreq, int fCnt=50) {
    00094     init(sampleFreq, fCnt);
    00095   }
    00096 };
    00097
    00098 #endif //FASTDEPUKFB_H_

All Classes Files Functions Variables