78 #include "KVDigitalFilter.h"
79 #define DUEPI 6.28318530717958623
93 double f = 1. / (DUEPI * tau_usec * 1000.) *
tau_clk;
94 double x = exp(-DUEPI * f);
110 double f = 1. / (DUEPI * tau_usec * 1000.) *
tau_clk;
111 double x = exp(-DUEPI * f);
114 filter.
a[0] = 1 / (1 - x);
115 filter.
a[1] = -x / (1 - x);
127 double ycoef[] = {0};
128 double xcoef[] = {tau_usec / (preamp_decay_usec + tau_usec)};
140 double f = 1. / (DUEPI * tau_usec * 1000.) *
tau_clk;
141 double x = exp(-DUEPI * f);
144 filter.
a[0] = (1 + x) / 2.;
145 filter.
a[1] = -(1 + x) / 2.;
156 const double& percent_ripple,
int npoles,
157 const double& tau_clk)
162 is_highpass, percent_ripple, npoles, filter.
a, filter.
b);
173 const double& pr,
const double& np,
175 double* a0,
double* a1,
double* a2,
176 double* b1,
double* b2)
178 double rp, ip, es, vx, kx, t, w, m, d, k = 0;
179 double x0, x1, x2, y1, y2;
184 rp = -cos(PI / (np * 2.) + (p - 1) * PI / np);
185 ip = sin(PI / (np * 2.) + (p - 1) * PI / np);
191 if (pr == 0)
goto pr_zero;
195 es = sqrt((100. / (100. - pr)) * (100. / (100. - pr)) - 1);
196 vx = (1 / np) * log((1. / es) + sqrt(1. / es / es + 1.));
197 kx = (1 / np) * log((1. / es) + sqrt(1. / es / es - 1.));
198 kx = (exp(kx) + exp(-kx)) / 2.;
199 rp = rp * ((exp(vx) - exp(-vx)) / 2.) / kx;
200 ip = ip * ((exp(vx) + exp(-vx)) / 2.) / kx;
208 m = rp * rp + ip * ip;
209 d = 4 - 4 * rp * t + m * t * t;
215 y1 = (8 - 2.*m * t * t) / d;
216 y2 = (-4. - 4.*rp * t - m * t * t) / d;
221 if (lh == 1) k = -cos(w / 2. + 1 / 2.) / cos(w / 2. - 1 / 2.);
222 if (lh == 0) k = sin(-w / 2. + 1 / 2.) / sin(w / 2. + 1 / 2.);
223 d = 1 + y1 * k - y2 * k * k ;
224 *a0 = (x0 - x1 * k + x2 * k * k) / d;
225 *a1 = (-2.*x0 * k + x1 + x1 * k * k - 2.*x2 * k) / d;
226 *a2 = (x0 * k * k - x1 * k + x2) / d;
227 *b1 = (2.*k + y1 + y1 * k * k - 2.*y2 * k) / d;
228 *b2 = (-k * k - y1 * k + y2) / d;
229 if (lh == 1) *a1 = -(*a1);
230 if (lh == 1) *b1 = -(*b1);
243 int is_highpass,
const double& percent_ripple,
int npoles,
244 double* a,
double* b)
246 double fc = freq_cutoff;
247 double lh = is_highpass;
248 double pr = percent_ripple;
251 double a0, a1, a2, b1, b2, gain;
252 double ta[22], tb[22];
254 #define CHECK(A,MIN,MAX) if(A<MIN || A>MAX) printf("ERROR in %s: %s=%e! (ok is %e..%e)\n",__PRETTY_FUNCTION__,#A,(double)A,(double)MIN,(double)MAX);
255 CHECK(freq_cutoff, 0, 0.5);
261 for (
int i = 0; i < 22; i++) {
268 for (
int p = 1; p <= np / 2; p++) {
270 for (
int i = 0; i < 22; i++) {
274 for (
int i = 2; i < 22; i++) {
275 a[i] = a0 * ta[i] + a1 * ta[i - 1] + a2 * ta[i - 2];
276 b[i] = tb[i] - b1 * tb[i - 1] - b2 * tb[i - 2];
280 for (
int i = 0; i < 20; i++) {
290 for (
int i = 0; i < 20; i++) {
291 if (lh == 0) sa = sa +
a[i];
292 if (lh == 0) sb = sb +
b[i];
293 if (lh == 1) sa = sa +
a[i] * pow(-1., i);
294 if (lh == 1) sb = sb +
b[i] * pow(-1., i);
299 for (
int i = 0; i < 20; i++) {
306 for (
int i = 0; i < 20; i++) {
307 sa = sa +
a[i] * sign;
308 sb = sb +
b[i] * sign;
314 gain = sa / (1. - sb);
315 for (
int i = 0; i < 20; i++)
a[i] =
a[i] / gain;
344 memcpy(
a, Xcoeffs,
sizeof(
double)*N);
345 memcpy(
b, Ycoeffs,
sizeof(
double)*N);
353 KVDigitalFilter::~KVDigitalFilter()
394 memset(
a, 0,
sizeof(
double)*
Ncoeff);
395 memset(
b, 0,
sizeof(
double)*
Ncoeff);
412 memcpy(
a, orig.
a,
sizeof(
double)*
Ncoeff);
413 memcpy(
b, orig.
b,
sizeof(
double)*
Ncoeff);
428 memcpy(
a, orig.
a,
sizeof(
double)*
Ncoeff);
429 memcpy(
b, orig.
b,
sizeof(
double)*
Ncoeff);
446 std::vector<long double> datay(NSamples);
450 for (i = 0; i <
Ncoeff; i++) {
451 datay[i] =
a[0] * datax[i];
452 for (k = 0; k < i; k++)
453 datay[i] +=
a[k + 1] * datax[i - k - 1] +
b[k + 1] * datay[i - k - 1];
455 for (i =
Ncoeff; i < NSamples; i++) {
456 datay[i] =
a[0] * datax[i];
457 for (k = 0; k <
Ncoeff - 1; k++)
458 datay[i] +=
a[k + 1] * datax[i - k - 1] +
b[k + 1] * datay[i - k - 1];
462 for (i = 0; i <
Ncoeff; i++) {
463 datay[NSamples - 1 - i] =
a[0] * datax[NSamples - 1 - i];
464 for (k = 0; k < i; k++)
465 datay[NSamples - 1 - i] +=
a[k + 1] * datax[NSamples - 1 - (i - k - 1)]
466 +
b[k + 1] * datay[NSamples - 1 - (i - k - 1)];
468 for (i =
Ncoeff; i < NSamples; i++) {
469 datay[NSamples - 1 - i] =
a[0] * datax[NSamples - 1 - i];
470 for (k = 0; k <
Ncoeff - 1; k++)
471 datay[NSamples - 1 - i] +=
a[k + 1] * datax[NSamples - 1 - (i - k - 1)]
472 +
b[k + 1] * datay[NSamples - 1 - (i - k - 1)];
480 printf(
"ERROR in %s: reverse=%d not supported\n", __PRETTY_FUNCTION__, reverse);
486 for (
int i = 0; i < NSamples; i++)
487 datax[i] = (
double)datay[i];
501 std::vector<long double> datay(NSamples);
505 for (i = 0; i <
Ncoeff; i++) {
506 datay[i] =
a[0] * datax[i];
507 for (k = 0; k < i; k++)
508 datay[i] +=
a[k + 1] * datax[i - k - 1] +
b[k + 1] * datay[i - k - 1];
510 for (i =
Ncoeff; i < NSamples; i++) {
511 datay[i] =
a[0] * datax[i];
512 for (k = 0; k <
Ncoeff - 1; k++)
513 datay[i] +=
a[k + 1] * datax[i - k - 1] +
b[k + 1] * datay[i - k - 1];
517 for (i = 0; i <
Ncoeff; i++) {
518 datay[NSamples - 1 - i] =
a[0] * datax[NSamples - 1 - i];
519 for (k = 0; k < i; k++)
520 datay[NSamples - 1 - i] +=
a[k + 1] * datax[NSamples - 1 - (i - k - 1)]
521 +
b[k + 1] * datay[NSamples - 1 - (i - k - 1)];
523 for (i =
Ncoeff; i < NSamples; i++) {
524 datay[NSamples - 1 - i] =
a[0] * datax[NSamples - 1 - i];
525 for (k = 0; k <
Ncoeff - 1; k++)
526 datay[NSamples - 1 - i] +=
a[k + 1] * datax[NSamples - 1 - (i - k - 1)]
527 +
b[k + 1] * datay[NSamples - 1 - (i - k - 1)];
535 printf(
"ERROR in %s: reverse=%d not supported\n", __PRETTY_FUNCTION__, reverse);
539 for (
int i = 0; i < NSamples; i++)
540 datax[i] = (
float)datay[i];
554 std::vector<long double> datay(NSamples);
558 for (i = 0; i <
Ncoeff; i++) {
559 datay[i] =
a[0] * datax[i];
560 for (k = 0; k < i; k++)
561 datay[i] +=
a[k + 1] * datax[i - k - 1] +
b[k + 1] * datay[i - k - 1];
563 for (i =
Ncoeff; i < NSamples; i++) {
564 datay[i] =
a[0] * datax[i];
565 for (k = 0; k <
Ncoeff - 1; k++)
566 datay[i] +=
a[k + 1] * datax[i - k - 1] +
b[k + 1] * datay[i - k - 1];
570 for (i = 0; i <
Ncoeff; i++) {
571 datay[NSamples - 1 - i] =
a[0] * datax[NSamples - 1 - i];
572 for (k = 0; k < i; k++)
573 datay[NSamples - 1 - i] +=
a[k + 1] * datax[NSamples - 1 - (i - k - 1)]
574 +
b[k + 1] * datay[NSamples - 1 - (i - k - 1)];
576 for (i =
Ncoeff; i < NSamples; i++) {
577 datay[NSamples - 1 - i] =
a[0] * datax[NSamples - 1 - i];
578 for (k = 0; k <
Ncoeff - 1; k++)
579 datay[NSamples - 1 - i] +=
a[k + 1] * datax[NSamples - 1 - (i - k - 1)]
580 +
b[k + 1] * datay[NSamples - 1 - (i - k - 1)];
588 printf(
"ERROR in %s: reverse=%d not supported\n", __PRETTY_FUNCTION__, reverse);
592 for (
int i = 0; i < NSamples; i++)
593 datax[i] = (
int)datay[i];
607 const double zero_level = 1e-20;
608 for (; M >= 1; M--) {
609 if (fabs(
a[M - 1]) > zero_level || fabs(
b[M - 1]) > zero_level)
614 for (
int i = 0; i < M; i++) {
615 if (fabs(
a[i]) < zero_level)
617 if (fabs(
b[i]) < zero_level)
644 const KVDigitalFilter* filters[10] = {f1, f2, f3, f4, f5, f6, f7, f8, f9, f10};
646 for (
int i = 0; i < 10; i++)
647 if (filters[i] != NULL) {
675 printf(
"ERROR in %s: cannot combine with different taus! %e != %e\n",
687 double a1[Nmax], a2[Nmax], a3[2 * Nmax];
688 double b1[Nmax], b2[Nmax], b3[2 * Nmax];
689 for (
int i = 0; i < Nmax; i++) {
697 for (
int i = 0; i < Nmax; i++) {
703 for (
int i = 0; i < 2 * Nmax; i++) {
706 for (
int j = 0; j < Nmax; j++) {
707 if (j > i || (i - j) >= Nmax)
continue;
709 a3[i] = a3[i] + a1[j] * a2[i - j];
711 a3[i] = a3[i] + a1[j] * b2[i - j] + a2[j] * b1[i - j];
712 b3[i] = b3[i] + b1[j] * b2[i - j];
716 for (
int i = 0; i < Nmax; i++) b3[i] = -b3[i];
741 double num = 0, den = 0;
742 for (
int i = 0; i <
Ncoeff; i++) {
746 return num / (1 - den);
759 double num = 0, den = 0;
761 for (
int i = 0; i <
Ncoeff; i++) {
766 return num / (1 - den);
774 double a[2] = {1, 0};
775 double b[2] = {0, 0};
786 double a[2] = {1, 0};
787 double b[2] = {0, 1};
798 printf(
"------------------------------------------------------\n");
799 printf(
"Coefficients valid with tau_clk=%.3f ns.\n",
tau_clk);
801 for (
int i = 0; i <
Ncoeff; i++) {
803 printf(
"*** Xcoeff[%2d]= %20.20e -\n", i,
a[i]);
805 printf(
"*** Xcoeff[%2d]= %20.20e Ycoeff[%2d]= %20.20e\n", i,
a[i], i,
b[i]);
806 if (fabs(
a[i]) > 1e-20) tot++;
807 if (fabs(
b[i]) > 1e-20) tot++;
809 printf(
"TOTAL: %d coefficients (a+b).\n", tot);
810 printf(
"(DspGuide : Xcoeff <-> a\n"
812 " Oppenheim: Xcoeff <-> b\n"
814 "------------------------------------------------------\n"
823 printf(
"------------------------------------------------------\n");
824 printf(
"DSP Coefficients valid with tau_clk=%.3f ns.\n",
tau_clk);
826 for (
int i = 0; i <
Ncoeff; i++) {
828 printf(
"*** Xcoeff[%2d]= 0x%6.6x -\n", i,
Double2DSP(
a[i]));
830 printf(
"*** Xcoeff[%2d]= 0x%6.6x Ycoeff[%2d]= 0x%6.6x\n",
832 if (fabs(
a[i]) > 1e-20) tot++;
833 if (fabs(
b[i]) > 1e-20) tot++;
835 printf(
"TOTAL: %d coefficients (a+b).\n", tot);
836 printf(
"(DspGuide : Xcoeff <-> a\n"
838 " Oppenheim: Xcoeff <-> b\n"
840 "------------------------------------------------------\n"
850 printf(
"/****** filter valid for %.1f tau_clk *********/\n",
tau_clk);
851 printf(
" int Ncoeff=%d;\n",
Ncoeff);
852 printf(
" double Xcoeffs[%d]={\n",
Ncoeff);
853 for (
int i = 0; i <
Ncoeff; i++)
854 printf(
"\t%20.20e%s\n",
a[i], i ==
Ncoeff - 1 ?
"};" :
",");
855 printf(
" double Ycoeffs[%d]={\n",
Ncoeff);
856 for (
int i = 0; i <
Ncoeff; i++)
857 printf(
"\t%20.20e%s\n",
b[i], i ==
Ncoeff - 1 ?
"};" :
",");
858 printf(
" KVDigitalFilter filter(%d, Xcoeffs, Ycoeffs, %f);\n",
Ncoeff,
tau_clk);
859 printf(
"/**********************************************/\n");
867 double* xgain,
int* x_out,
int* y_out,
int* x_scale,
int* y_scale)
870 if (nbits <= 0)
return;
875 double Nlevel = pow(2.f, nbits - 1) - 1;
877 double factor = fabs(x[0]);
880 if (fabs(x[i]) > factor) {
884 if (factor <= 1e-16) {
885 printf(
"ERROR in %s: factor=%e?\n", __PRETTY_FUNCTION__, factor);
891 if (x_out != NULL) memset(x_out, 0,
GetNCoeff()*
sizeof(
int));
892 if (y_out != NULL) memset(y_out, 0,
GetNCoeff()*
sizeof(
int));
893 if (x_scale != NULL) memset(x_scale, 0,
GetNCoeff()*
sizeof(
int));
894 if (y_scale != NULL) memset(y_scale, 0,
GetNCoeff()*
sizeof(
int));
905 N = (int)ceil(log(fabs(x[i])) / log(2.));
909 N = (int)ceil(fabs(x[i]));
916 int k = (int)rint(x[i] * Nlevel);
917 if (k >= Nlevel) k = (int)(Nlevel - 1.);
918 if (k < -Nlevel) k = (int)(-Nlevel);
924 if (x_scale != NULL) {
928 x[i] = (double)k / (
double)Nlevel;
943 N = (int)ceil(log(fabs(y[i])) / log(2.));
947 N = (int)ceil(fabs(y[i]));
954 int k = (int)rint(y[i] * Nlevel);
955 if (k >= Nlevel) k = (int)(Nlevel - 1);
956 if (k < -Nlevel) k = (int)(-Nlevel);
962 y[i] = (double)k / (
double)Nlevel;
978 TH1F* h1 =
new TH1F(
"hKVDigitalFilter",
"Filter response", Nbins, 0, 1. /
GetTauClk() / 2.*1000.);
979 h1->SetStats(kFALSE);
980 h1->GetXaxis()->SetTitle(
"Frequency (MHz)");
982 for (
int k = 0; k < Nbins; k++) {
983 double angolo = 3.14159265358979312 * k / (Nbins - 1);
991 double a_re = cos(angolo * i);
992 double a_im = sin(angolo * i);
1005 double val = sqrt((numRE * numRE + numIM * numIM) / ((denRE * denRE + denIM * denIM)));
1006 h1->SetBinContent(k + 1, val);
1041 for (
int i = 1; i < filter->
GetNCoeff(); i ++) {
1061 FILE* fin = fopen(filecoeff,
"r");
1064 if (fscanf(fin,
"%d", &n)) {
1069 for (i = 0; i < n; i++) {
1070 if (fscanf(fin,
"%lg", &
a[i])) {
1074 printf(
"%s: i=%d a[%d]=%20.20e\n", __PRETTY_FUNCTION__, i, i,
a[i]);
1077 printf(
"%s: Read %d coefficients\n", __PRETTY_FUNCTION__, i);
1093 FILE* fout = fopen(filecoeff,
"w");
1095 fprintf(fout,
"%d\n",
Ncoeff);
1096 for (i = 0; i <
Ncoeff; i++) {
1097 fprintf(fout,
"%20.20e\n",
a[i]);
1099 printf(
"%s: Written %d coefficients\n", __PRETTY_FUNCTION__, i);
1119 std::vector<long double> datay(NSamples);
1123 for (i = 0; i <
Ncoeff; i++) {
1124 datay[i] =
a[0] * datax[i];
1125 for (k = 0; k < i; k++)
1126 datay[i] +=
a[k + 1] * datax[i - k - 1];
1128 for (i =
Ncoeff; i < NSamples; i++) {
1129 datay[i] =
a[0] * datax[i];
1130 for (k = 0; k <
Ncoeff - 1; k++)
1131 datay[i] +=
a[k + 1] * datax[i - k - 1];
1135 for (i = 0; i <
Ncoeff; i++) {
1136 datay[NSamples - 1 - i] =
a[0] * datax[NSamples - 1 - i];
1137 for (k = 0; k < i; k++)
1138 datay[NSamples - 1 - i] +=
a[k + 1] * datax[NSamples - 1 - (i - k - 1)];
1140 for (i =
Ncoeff; i < NSamples; i++) {
1141 datay[NSamples - 1 - i] =
a[0] * datax[NSamples - 1 - i];
1142 for (k = 0; k <
Ncoeff - 1; k++)
1143 datay[NSamples - 1 - i] +=
a[k + 1] * datax[NSamples - 1 - (i - k - 1)];
1151 printf(
"ERROR in %s: reverse=%d not supported\n", __PRETTY_FUNCTION__, reverse);
1155 for (
int i = 0; i < NSamples; i++)
1156 datax[i] = (
float)datay[i];
1170 std::vector<long double> datay(NSamples);
1174 for (i = 0; i <
Ncoeff; i++) {
1175 datay[i] =
a[0] * datax[i];
1176 for (k = 0; k < i; k++)
1177 datay[i] +=
a[k + 1] * datax[i - k - 1];
1179 for (i =
Ncoeff; i < NSamples; i++) {
1180 datay[i] =
a[0] * datax[i];
1181 for (k = 0; k <
Ncoeff - 1; k++)
1182 datay[i] +=
a[k + 1] * datax[i - k - 1];
1186 for (i = 0; i <
Ncoeff; i++) {
1187 datay[NSamples - 1 - i] =
a[0] * datax[NSamples - 1 - i];
1188 for (k = 0; k < i; k++)
1189 datay[NSamples - 1 - i] +=
a[k + 1] * datax[NSamples - 1 - (i - k - 1)];
1191 for (i =
Ncoeff; i < NSamples; i++) {
1192 datay[NSamples - 1 - i] =
a[0] * datax[NSamples - 1 - i];
1193 for (k = 0; k <
Ncoeff - 1; k++)
1194 datay[NSamples - 1 - i] +=
a[k + 1] * datax[NSamples - 1 - (i - k - 1)];
1202 printf(
"ERROR in %s: reverse=%d not supported\n", __PRETTY_FUNCTION__, reverse);
1209 for (
int i = 0; i < NSamples; i++)
1210 datax[i] = (
double)datay[i];
Base class for KaliVeda framework.
static KVDigitalFilter CombineStages(const KVDigitalFilter &f1, const KVDigitalFilter &f2, int parallel=0)
static KVDigitalFilter BuildRCHighPassWithPZ(const double &tau_usec, const double &preamp_decay_usec, const double &tau_clk)
void Compress()
shorten filter. No deallocation of memory.
virtual void Draw(Option_t *option="")
const double & GetYcoeff(int i) const
static void ComputeChebyshevCoeffs_serv(const double &fc, const double &lh, const double &pr, const double &np, int p, double *a0, double *a1, double *a2, double *b1, double *b2)
static int Double2DSP(const double &val)
– conversion to/from DSP 1.15? notation
void SetXcoeff(int i, const double &val)
static KVDigitalFilter BuildRCLowPassDeconv(const double &tau_usec, const double &tau_clk)
void Quantize(int nbits, int use_pow2=0, double *xgain=NULL, int *x_out=NULL, int *y_out=NULL, int *x_scale=NULL, int *y_scale=NULL)
static KVDigitalFilter BuildRCLowPass(const double &tau_usec, const double &tau_clk)
void PrintCoeffsDSP() const
static void ComputeChebyshevCoeffs(const double &freq_cutoff, int is_highpass, const double &percent_ripple, int npoles, double *a, double *b)
static KVDigitalFilter BuildChebyshev(const double &freq_cutoff_mhz, int is_highpass, const double &percent_ripple, int npoles, const double &tau_clk)
void ApplyTo(double *data, const int N, int reverse=0) const
KVDigitalFilter operator=(const KVDigitalFilter &)
int ReadMatlabFIR(char *filecoeff)
FILE *fin = fopen("notch_coeffs.txt","r");.
static KVDigitalFilter BuildUnity(const double &tau_clk)
const double & GetXcoeff(int i) const
static KVDigitalFilter BuildRCHighPass(const double &tau_usec, const double &tau_clk)
void SetYcoeff(int i, const double &val)
const double & GetTauClk()
static KVDigitalFilter BuildIntegrator(const double &tau_clk)
KVDigitalFilter(const double &tau=10)
int WriteMatlabFIR(char *filecoeff)
void PrintCoeffs_AsC() const
void Alloc(const int Ncoeff)
printf("a=%p, N=%d, Ncoeff=%d\n", a, N, Ncoeff);
void FIRApplyTo(double *datax, const int NSamples, int reverse) const
static KVDigitalFilter BuildInverse(KVDigitalFilter *filter)
**************************************/
static KVDigitalFilter CombineStagesMany(const KVDigitalFilter *f1, const KVDigitalFilter *f2, const KVDigitalFilter *f3=NULL, const KVDigitalFilter *f4=NULL, const KVDigitalFilter *f5=NULL, const KVDigitalFilter *f6=NULL, const KVDigitalFilter *f7=NULL, const KVDigitalFilter *f8=NULL, const KVDigitalFilter *f9=NULL, const KVDigitalFilter *f10=NULL)
se ne devi combinare + di 1 IN CASCATA!