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];
00009 double n_u[2], d_u[3];
00010
00011 void findIIRCoeff(double fc, double pl, double pu){
00012 double c1, c2, c3, c4;
00013 double d1, d2, d3, d4;
00014
00015
00016 c1=exp(pl/fc);
00017 c2=exp(pl);
00018
00019 c4=c1*c2;
00020 c3=c1*c4;
00021 d_l[0]=(c2*fc);
00022 n_l[0]=(fc+fc*pl)/d_l[0];
00023 n_l[1]=(-c1*fc-c1*pl-c1*fc*pl)/d_l[0];
00024 d_l[1]=(-2*c4*fc)/d_l[0];
00025 d_l[2]=(c3*fc)/d_l[0];
00026 d_l[0]=1.0;
00027
00028
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];
00036 n_u[1]=d2*(-fc+pu+pu*fc)/d_u[0];
00037 d_u[1]=(-2*d4*fc)/d_u[0];
00038 d_u[2]=(fc)/d_u[0];
00039 d_u[0]=1.0;
00040 }
00041
00042 void filter(double fc, double *out){
00043
00044
00045
00046 double z1=0.0, z2=0.0;
00047 bzero(out, (int)rint(fs/2.0)*sizeof(double));
00048
00049
00050
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++){
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
00062 z1=z2=0.0;
00063 bzero(out, (int)rint(fc)*sizeof(double));
00064
00065
00066
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++){
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);
00081 filter(fc, filt);
00082 }
00083
00085 virtual void af(double fc, int whichFilter){
00086 cout<<"FastDepUKFB::af"<<endl;
00087
00088
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_