KaliVeda
Toolkit for HIC analysis
Loading...
Searching...
No Matches
KVDigitalFilter.cpp
1/*
2 $Header: /home/CVS/luigi/FClasses/KVDigitalFilter.cxx,v 1.6 2013-05-23 19:40:20 garfield Exp $
3 $Log: KVDigitalFilter.cxx,v $
4 Revision 1.6 2013-05-23 19:40:20 garfield
5 KVDigitalFilter: added methods to apply a FIR filter (no b coefficients)
6 added methods to read/write FIR coefficients for MatLab
7
8 Revision 1.5 2013-05-23 11:29:09 garfield
9 Added method to draw the step response of the filter.
10
11 Revision 1.4 2010-01-26 09:48:12 bini
12 Ottimizzazioni in KVSignal
13 Patch per interpolazione cubica in KVDigitalFilterWizard
14
15 Revision 1.3 2009/10/12 12:57:31 bardelli
16 Aggiunto il metodo static KVDigitalFilter BuildInverse(KVDigitalFilter *filter)
17
18 Revision 1.2 2006/05/05 13:10:26 bardelli
19 Aggiunto Header e Log in testa a tutti i files
20
21*/
22
23
24//=======================
25// Author: G.Pasquali & L.Bardelli
26// @(#) 17 11 2003
27//=======================
29// //
30// KVDigitalFilter //
31// //
32// Class for digital filtering //
33// //
34// No limits on number of a[] and b[] coeffs. //
35// //
36// b[0] = 0. (this is different from the sources of KVSignal!) //
37// //
38// INTERNAL COMPUTATIONS DONE USING "DspGuide notation". //
39// //
40// Example: //
41// //
42// // Low pass RC with tau=2 us, coeff. for a 10ns sampling period //
43//KVDigitalFilter rc=KVDigitalFilter::BuildRCLowPass( 2, 10 ); //
44// //
45// // Apply it to a signal: intermediate results are "double" //
46// // type (i.e. 64 bit). //
47// KVSignal s; //
48// rc.ApplyTo( &s ); //
49// //
50// // Apply it to generic data: intermediate results are "double" //
51// // type (i.e. 64 bit). //
52// void ApplyTo(double *data, const int N, int reverse=0) const; //
53// void ApplyTo(float *data, const int N, int reverse=0) const; //
54// void ApplyTo(int *data, const int N, int reverse=0) const; //
55// //
56// // Combine two filters //
57//KVDigitalFilter rc=KVDigitalFilter::BuildRCLowPass( 2, 10 ); //
58//KVDigitalFilter cr=KVDigitalFilter::BuildRCHighPass( 2, 10 ); //
59//KVDigitalFilter total=KVDigitalFilter::CombineStages( &rc, &cr ); //
60// //
61// // Combine Many series (parallel=0) //
62//KVDigitalFilter total4=KVDigitalFilter::CombineStagesMany(&cr,&rc,&rc,&rc,&rc);//
63// //
64// // Write Coeffs //
65// total.PrintCoeffs() //
66// Coefficients valid with tau_clk=10.000 ns //
67// a[ 0]= 4.975083e-03 - //
68// a[ 1]= -4.975083e-03 b[ 1]= 1.990025e+00 //
69// a[ 2]= 0.000000e+00 b[ 2]= -9.900498e-01 //
70// TOTAL: 4 coefficients (a+b). //
71// //
72// - Various "Build*" methods... //
73// - ComputeGainDC(); // gain at 0 frequency //
74// - ComputeGainNy(); // gain at Nyquist frequency (0.5) //
75// //
76// //
78#include "KVDigitalFilter.h"
79#define DUEPI 6.28318530717958623
80#define PI (DUEPI/2.)
81
82#define LINE() // printf("Now %s line %d...\n",__PRETTY_FUNCTION__, __LINE__);
83
85
86//============================================
87
89
90KVDigitalFilter KVDigitalFilter::BuildRCLowPass(const double& tau_usec, const double& tau_clk)
91{
92
93 double f = 1. / (DUEPI * tau_usec * 1000.) * tau_clk;
94 double x = exp(-DUEPI * f);
96 filter.Alloc(2);
97 filter.a[0] = 1 - x;
98 filter.b[1] = x;
99
100 return filter;
101}
102
103//============================================
104
106
107KVDigitalFilter KVDigitalFilter::BuildRCLowPassDeconv(const double& tau_usec, const double& tau_clk)
108{
109
110 double f = 1. / (DUEPI * tau_usec * 1000.) * tau_clk;
111 double x = exp(-DUEPI * f);
112 KVDigitalFilter filter(tau_clk);
113 filter.Alloc(2);
114 filter.a[0] = 1 / (1 - x);
115 filter.a[1] = -x / (1 - x);
116
117 return filter;
118}
119
120//============================================
121
123
124KVDigitalFilter KVDigitalFilter::BuildRCHighPassWithPZ(const double& tau_usec, const double& preamp_decay_usec, const double& tau_clk)
125{
127 double ycoef[] = {0};
128 double xcoef[] = {tau_usec / (preamp_decay_usec + tau_usec)};
129 KVDigitalFilter polez(tau_clk, 1, xcoef, ycoef);
130 return KVDigitalFilter::CombineStages(&highpass, &polez, 1);
131}
132
133//============================================
134
136
137KVDigitalFilter KVDigitalFilter::BuildRCHighPass(const double& tau_usec, const double& tau_clk)
138{
139
140 double f = 1. / (DUEPI * tau_usec * 1000.) * tau_clk;
141 double x = exp(-DUEPI * f);
142 KVDigitalFilter filter(tau_clk);
143 filter.Alloc(2);
144 filter.a[0] = (1 + x) / 2.;
145 filter.a[1] = -(1 + x) / 2.;
146 filter.b[1] = x;
147
148 return filter;
149}
150
151//============================================
152
154
155KVDigitalFilter KVDigitalFilter::BuildChebyshev(const double& freq_cutoff_mhz, int is_highpass,
156 const double& percent_ripple, int npoles,
157 const double& tau_clk)
158{
159 KVDigitalFilter filter(tau_clk);
160 filter.Alloc(30);
161 ComputeChebyshevCoeffs(freq_cutoff_mhz * (tau_clk / 1e3),
162 is_highpass, percent_ripple, npoles, filter.a, filter.b);
163 filter.Compress();
164 return filter;
165}
166
167//============================================
168
169
171
172void KVDigitalFilter::ComputeChebyshevCoeffs_serv(const double& fc, const double& lh,
173 const double& pr, const double& np,
174 int p,
175 double* a0, double* a1, double* a2,
176 double* b1, double* b2)
177{
178 double rp, ip, es, vx, kx, t, w, m, d, k = 0;
179 double x0, x1, x2, y1, y2;
180
181 // printf("%e %e\n",cos(PI/(np*2.)+(p-1)*PI/np),PI/(np*2.)+(p-1)*PI/np);
182
183 /* calculate the pole location on the unit circle */
184 rp = -cos(PI / (np * 2.) + (p - 1) * PI / np);
185 ip = sin(PI / (np * 2.) + (p - 1) * PI / np);
186
188
189 /* warp from a circle to an ellipse */
190
191 if (pr == 0) goto pr_zero;
192
193 // printf("pr=%e\n",pr);
194
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;
201
202 /* s-domain to z-domain conversion */
204
205pr_zero:
206 t = 2 * tan(1 / 2.);
207 w = 2.*PI * fc;
208 m = rp * rp + ip * ip;
209 d = 4 - 4 * rp * t + m * t * t;
210
211 x0 = t * t / d;
212 x1 = 2.*t * t / d;
213 x2 = t * t / d;
214
215 y1 = (8 - 2.*m * t * t) / d;
216 y2 = (-4. - 4.*rp * t - m * t * t) / d;
217
218 // printf("t=%e w=%e m=%e d=%e\n",t,w,m,d);
219 // printf("x0=%e x1=%e x2=%e y1=%e y2=%e\n",x0,x1,x2,y1,y2);
220 /* LP to LP or LP to HP transform */
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);
231
232
233 return;
234}
235
236
237//============================================
238
239
241
242void KVDigitalFilter::ComputeChebyshevCoeffs(const double& freq_cutoff,
243 int is_highpass, const double& percent_ripple, int npoles,
244 double* a, double* b)
245{
246 double fc = freq_cutoff;
247 double lh = is_highpass;
248 double pr = percent_ripple;
249 double np = npoles;
250
251 double a0, a1, a2, b1, b2, gain;
252 double ta[22], tb[22];
253
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);
256 CHECK(lh, 0, 1);
257 CHECK(pr, 0, 29);
258 CHECK(np, 2, 20);
259#undef CHECK
260
261 for (int i = 0; i < 22; i++) {
262 a[i] = 0.;
263 b[i] = 0.;
264 }
265 a[2] = 1.;
266 b[2] = 1.;
267
268 for (int p = 1; p <= np / 2; p++) {
269 ComputeChebyshevCoeffs_serv(fc, lh, pr, np, p, &a0, &a1, &a2, &b1, &b2);
270 for (int i = 0; i < 22; i++) {
271 ta[i] = a[i];
272 tb[i] = b[i];
273 }
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];
277 }
278 }
279 b[2] = 0.;
280 for (int i = 0; i < 20; i++) {
281 a[i] = a[i + 2];
282 b[i] = -b[i + 2];
283 }
284 double sa = 0;
285 double sb = 0;
286
287 /***** ORIGINALE DI Gab.*********/
288#define AAA
289#ifdef AAA
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);
295 }
296#else
297 /**********************/
298 if (lh == 0) {
299 for (int i = 0; i < 20; i++) {
300 sa = sa + a[i];
301 sb = sb + b[i];
302 }
303 }//end of lh==0
304 else {
305 int sign = 1;
306 for (int i = 0; i < 20; i++) {
307 sa = sa + a[i] * sign;
308 sb = sb + b[i] * sign;
309 sign *= -1;
310 }
311 }
312#endif
314 gain = sa / (1. - sb);
315 for (int i = 0; i < 20; i++) a[i] = a[i] / gain;
317 return;
318}
319
320//============================================
321
323
325{
326
327 a = b = NULL;
328 tau_clk = tau;
329 Ncoeff = 0;
330
331}
332
333//============================================
334
336
337KVDigitalFilter::KVDigitalFilter(const double& tau, int N, const double* Xcoeffs, const double* Ycoeffs)
338{
339
340 a = b = NULL;
341 tau_clk = tau;
342 Ncoeff = 0;
343 Alloc(N);
344 memcpy(a, Xcoeffs, sizeof(double)*N);
345 memcpy(b, Ycoeffs, sizeof(double)*N);
346
347}
348
349//=============================================
350
352
354{
355
356 if (a != NULL) {
357 delete [] a;
358 delete [] b;
359 }
360
361}
362
363//=============================================
364
367
368void KVDigitalFilter::Alloc(const int N)
369{
370
371 // printf("a=%p, N=%d, Ncoeff=%d\n", a, N, Ncoeff);
372 Ncoeff = N;
373 if (a != NULL) {
374 delete [] a;
375 delete [] b;
376 a = NULL;
377 b = NULL;
378 };
379 if (N > 0) {
380 a = new double [N];
381 b = new double [N];
382 Azzera();
383 }
384
385}
386
387//=============================================
388
390
392{
393
394 memset(a, 0, sizeof(double)*Ncoeff);
395 memset(b, 0, sizeof(double)*Ncoeff);
396
397}
398
399//=============================================
400
402
404 : KVBase()
405{
406
407 Ncoeff = 0;
408 tau_clk = orig.tau_clk;
409 a = b = NULL;
410 if (orig.Ncoeff > 0) {
411 Alloc(orig.Ncoeff);
412 memcpy(a, orig.a, sizeof(double)*Ncoeff);
413 memcpy(b, orig.b, sizeof(double)*Ncoeff);
414 }
415
416}
417
418//=============================================
419
421
423{
424
425 tau_clk = orig.tau_clk;
426 Alloc(orig.Ncoeff);
427 if (orig.Ncoeff > 0) {
428 memcpy(a, orig.a, sizeof(double)*Ncoeff);
429 memcpy(b, orig.b, sizeof(double)*Ncoeff);
430 }
431
432 return *this;
433}
434
435//=============================================
436
440
441void KVDigitalFilter::ApplyTo(double* datax, const int NSamples, int reverse) const
442{
443 // Copiato +- da KVSignal.cxx
444 // Diversa la convenzione per a0 b0!
445
446 std::vector<long double> datay(NSamples);
447 int i = 0, k = 0;
448 switch (reverse) {
449 case 0:// direct
450 for (i = 0; i < Ncoeff; i++) { // primo loop su Npunti
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];
454 }
455 for (i = Ncoeff; i < NSamples; i++) { //secondo loop. cambia l'indice interno.
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];
459 }
460 break; // end of direct
461 case 1: //reverse: cut & paste from direct and NSamples-1-
462 for (i = 0; i < Ncoeff; i++) { // primo loop su Npunti
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)];
467 }
468 for (i = Ncoeff; i < NSamples; i++) { //secondo loop. cambia l'indice interno.
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)];
473 }
474 break;
475 case -1: // bidirectional
476 ApplyTo(datax, NSamples, 0);
477 ApplyTo(datax, NSamples, 1);
478 return;
479 default:
480 printf("ERROR in %s: reverse=%d not supported\n", __PRETTY_FUNCTION__, reverse);
481 }// end of reverse switch.
482 /*----------------------------------------------*/
483 // void *memcpy(void *dest, const void *src, size_t n);
484 //memcpy(datax, datay, NSamples * sizeof(double));
485 // can't use memcpy: datax & datay have different type sizes!
486 for (int i = 0; i < NSamples; i++)
487 datax[i] = (double)datay[i];
488}
489
490//=============================================
491
495
496void KVDigitalFilter::ApplyTo(float* datax, const int NSamples, int reverse) const
497{
498 // Copiato +- da KVSignal.cxx
499 // Diversa la convenzione per a0 b0!
500
501 std::vector<long double> datay(NSamples);
502 int i = 0, k = 0;
503 switch (reverse) {
504 case 0:// direct
505 for (i = 0; i < Ncoeff; i++) { // primo loop su Npunti
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];
509 }
510 for (i = Ncoeff; i < NSamples; i++) { //secondo loop. cambia l'indice interno.
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];
514 }
515 break; // end of direct
516 case 1: //reverse: cut & paste from direct and NSamples-1-
517 for (i = 0; i < Ncoeff; i++) { // primo loop su Npunti
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)];
522 }
523 for (i = Ncoeff; i < NSamples; i++) { //secondo loop. cambia l'indice interno.
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)];
528 }
529 break;
530 case -1: // bidirectional
531 ApplyTo(datax, NSamples, 0);
532 ApplyTo(datax, NSamples, 1);
533 return;
534 default:
535 printf("ERROR in %s: reverse=%d not supported\n", __PRETTY_FUNCTION__, reverse);
536 }// end of reverse switch.
537 /*----------------------------------------------*/
538 // void *memcpy(void *dest, const void *src, size_t n);
539 for (int i = 0; i < NSamples; i++)
540 datax[i] = (float)datay[i];
541}
542
543//=============================================
544
548
549void KVDigitalFilter::ApplyTo(int* datax, const int NSamples, int reverse) const
550{
551 // Copiato +- da KVSignal.cxx
552 // Diversa la convenzione per a0 b0!
553
554 std::vector<long double> datay(NSamples);
555 int i = 0, k = 0;
556 switch (reverse) {
557 case 0:// direct
558 for (i = 0; i < Ncoeff; i++) { // primo loop su Npunti
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];
562 }
563 for (i = Ncoeff; i < NSamples; i++) { //secondo loop. cambia l'indice interno.
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];
567 }
568 break; // end of direct
569 case 1: //reverse: cut & paste from direct and NSamples-1-
570 for (i = 0; i < Ncoeff; i++) { // primo loop su Npunti
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)];
575 }
576 for (i = Ncoeff; i < NSamples; i++) { //secondo loop. cambia l'indice interno.
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)];
581 }
582 break;
583 case -1: // bidirectional
584 ApplyTo(datax, NSamples, 0);
585 ApplyTo(datax, NSamples, 1);
586 return;
587 default:
588 printf("ERROR in %s: reverse=%d not supported\n", __PRETTY_FUNCTION__, reverse);
589 }// end of reverse switch.
590 /*----------------------------------------------*/
591 // void *memcpy(void *dest, const void *src, size_t n);
592 for (int i = 0; i < NSamples; i++)
593 datax[i] = (int)datay[i];
594}
595
596/**********************************************/
597
600
602{
603 // shorten filter. No deallocation of memory.
604
605 int M = Ncoeff;
606 // printf("COmpress: %d\n", M);
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)
610 break;
611 }
613
614 for (int i = 0; i < M; i++) {
615 if (fabs(a[i]) < zero_level)
616 a[i] = 0;
617 if (fabs(b[i]) < zero_level)
618 b[i] = 0;
619 }
620
621 b[0] = 0;
622
623 Ncoeff = M; // le lascio allocate, ma non le usero' nei conti...
624
625}
626
627
628
629/***************************************************/
630/***************************************************/
631
635
637 const KVDigitalFilter* f3, const KVDigitalFilter* f4,
638 const KVDigitalFilter* f5, const KVDigitalFilter* f6,
639 const KVDigitalFilter* f7, const KVDigitalFilter* f8,
640 const KVDigitalFilter* f9, const KVDigitalFilter* f10)
641{
642 // se ne devi combinare + di 1 IN CASCATA!
643 // per fare parallelo usare l'altro...
644 const KVDigitalFilter* filters[10] = {f1, f2, f3, f4, f5, f6, f7, f8, f9, f10};
645 KVDigitalFilter total = BuildUnity(f1->tau_clk);
646 for (int i = 0; i < 10; i++)
647 if (filters[i] != NULL) {
648 total = CombineStages(&total, filters[i]);
649 }
650 total.Compress();
651 return total;
652}
653
654/***************************************************/
655/***************************************************/
656
658
660 const KVDigitalFilter& f2,
661 int parallel)
662{
663 return CombineStages(&f1, &f2, parallel);
664}
665
666/***************************************************/
667
669
671 const KVDigitalFilter* f2,
672 int parallel)
673{
674 if (fabs(f1->tau_clk - f2->tau_clk) > 1e-4) {
675 printf("ERROR in %s: cannot combine with different taus! %e != %e\n",
676 __PRETTY_FUNCTION__, f1->tau_clk, f2->tau_clk);
677 return KVDigitalFilter(f1->tau_clk);
678 }
679 int Nmax = f1->Ncoeff;
680 int __N = f2->Ncoeff;
681 if (__N > Nmax)
682 Nmax = __N;
683
684 Nmax *= 2;
685
686 /******** now N is the max *********/
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++) {
690 a1[i] = f1->GetXcoeff(i);
691 b1[i] = f1->GetYcoeff(i);
692 a2[i] = f2->GetXcoeff(i);
693 b2[i] = f2->GetYcoeff(i);
695 }
696
697 for (int i = 0; i < Nmax; i++) {
698 b2[i] = - b2[i];
699 b1[i] = - b1[i];
700 }
701 b1[0] = 1;
702 b2[0] = 1;
703 for (int i = 0; i < 2 * Nmax; i++) {
704 a3[i] = 0;
705 b3[i] = 0;
706 for (int j = 0; j < Nmax; j++) {
707 if (j > i || (i - j) >= Nmax) continue;
708 if (!parallel)
709 a3[i] = a3[i] + a1[j] * a2[i - j];
710 else
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];
713 }
714 }
715
716 for (int i = 0; i < Nmax; i++) b3[i] = -b3[i];
717 b3[0] = 0;
718 /*
719 for(int i=0; i< Nmax*2; i++){
720 printf("Combine stages, output: a3[%2d]=%15.8e b3[%2d]=%15.8e\n",
721 i,a3[i],i,b3[i]);
722 }
723 */
724 /********** COPY INTO NEW FILTER ****************/
725 KVDigitalFilter filter(f1->tau_clk, 2 * Nmax, a3, b3);
726 filter.Compress();
727 return filter;
728
729}
730
731/***********************************************/
732
736
738{
739 // Gain of filter at f=0
740 // eq. 33.7 del solito libro
741 double num = 0, den = 0;
742 for (int i = 0; i < Ncoeff; i++) {
743 num += a[i];
744 den += b[i];
745 }
746 return num / (1 - den);
747}
748
749/***********************************************/
750
754
756{
757 // Gain of filter at f=Nyquist (0.5)
758 // eq. 33.8 del solito libro
759 double num = 0, den = 0;
760 int sign = 1;
761 for (int i = 0; i < Ncoeff; i++) {
762 num += a[i] * sign;
763 den += b[i] * sign;
764 sign *= -1;
765 }
766 return num / (1 - den);
767}
768
769
771
772KVDigitalFilter KVDigitalFilter ::BuildUnity(const double& tau_clk)
773{
774 double a[2] = {1, 0};
775 double b[2] = {0, 0};
776 KVDigitalFilter filter(tau_clk, 2, a, b);
777 filter.Compress();
778 return filter;
779}
780
781
783
784KVDigitalFilter KVDigitalFilter ::BuildIntegrator(const double& tau_clk)
785{
786 double a[2] = {1, 0};
787 double b[2] = {0, 1};
788 KVDigitalFilter filter(tau_clk, 2, a, b);
789 return filter;
790}
791
792/***********************************************/
793
795
797{
798 printf("------------------------------------------------------\n");
799 printf("Coefficients valid with tau_clk=%.3f ns.\n", tau_clk);
800 int tot = 0;
801 for (int i = 0; i < Ncoeff; i++) {
802 if (i == 0)
803 printf("*** Xcoeff[%2d]= %20.20e -\n", i, a[i]);
804 else
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++;
808 }
809 printf("TOTAL: %d coefficients (a+b).\n", tot);
810 printf("(DspGuide : Xcoeff <-> a\n"
811 " Ycoeff <-> b\n"
812 " Oppenheim: Xcoeff <-> b\n"
813 " Ycoeff <-> a)\n"
814 "------------------------------------------------------\n"
815 );
816}
817
818
820
822{
823 printf("------------------------------------------------------\n");
824 printf("DSP Coefficients valid with tau_clk=%.3f ns.\n", tau_clk);
825 int tot = 0;
826 for (int i = 0; i < Ncoeff; i++) {
827 if (i == 0)
828 printf("*** Xcoeff[%2d]= 0x%6.6x -\n", i, Double2DSP(a[i]));
829 else
830 printf("*** Xcoeff[%2d]= 0x%6.6x Ycoeff[%2d]= 0x%6.6x\n",
831 i, Double2DSP(a[i]), i, Double2DSP(b[i]));
832 if (fabs(a[i]) > 1e-20) tot++;
833 if (fabs(b[i]) > 1e-20) tot++;
834 }
835 printf("TOTAL: %d coefficients (a+b).\n", tot);
836 printf("(DspGuide : Xcoeff <-> a\n"
837 " Ycoeff <-> b\n"
838 " Oppenheim: Xcoeff <-> b\n"
839 " Ycoeff <-> a)\n"
840 "------------------------------------------------------\n"
841 );
842}
843
844/***********************************************/
845
847
849{
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");
860}
861
862/***********************************************/
863
865
866void KVDigitalFilter::Quantize(int nbits, int use_pow2,
867 double* xgain, int* x_out, int* y_out, int* x_scale, int* y_scale)
868{
869
870 if (nbits <= 0) return;
871 Compress();
872 double* x = GetXarray();
873 double* y = GetYarray();
874
875 double Nlevel = pow(2.f, nbits - 1) - 1; // non posso fare con << xe' non funziona se nbits==32 !!
876 // normalizzazione delle X a 1:
877 double factor = fabs(x[0]);
878 // int imax=0;
879 for (int i = 1; i < GetNCoeff(); i++)
880 if (fabs(x[i]) > factor) {
881 factor = fabs(x[i]);
882 // imax=i;
883 }
884 if (factor <= 1e-16) {
885 printf("ERROR in %s: factor=%e?\n", __PRETTY_FUNCTION__, factor);
886 factor = 1;
887 }
888 if (xgain != NULL) {
889 *xgain = factor;
890 }
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));
895
896
898 for (int i = 0; i < GetNCoeff(); i++) {
899 x[i] /= factor;
900
901
902 // quantizziamo il coefficiente N-esimo...
903 int N = 0;
904 if (use_pow2 == 1) {
905 N = (int)ceil(log(fabs(x[i])) / log(2.));
906 N = (1 << N);
907 }
908 else
909 N = (int)ceil(fabs(x[i])); // numero intero.
910 if (N == 0) {
911 x[i] *= factor;
912 continue;
913 }
914 x[i] /= N;
915 //-- adesso i bits: adesso y[i] e' un numero tra -1..1
916 int k = (int)rint(x[i] * Nlevel);
917 if (k >= Nlevel) k = (int)(Nlevel - 1.);
918 if (k < -Nlevel) k = (int)(-Nlevel);
919 // if(stampa && (k&0x00FFFFFF)!=0) printf("x[%d] 0x %6.6Xoo with factor %d %s\n",i,k&0x00FFFFFF,N,imax==i?" << MAX" : "");
920 if (x_out != NULL) {
922 x_out[i] = k;
923 }
924 if (x_scale != NULL) {
925 x_scale[i] = N;
926 }
927
928 x[i] = (double)k / (double)Nlevel;
929 //--- ripristino N
930 x[i] *= N;
931 //--- ripristino factor
932 x[i] *= factor;
933
934 }
935
936 for (int i = 1; i < GetNCoeff(); i++) {
937
938 // printf("Now coeff %d\n",i);
939 // quantizziamo il coefficiente N-esimo...
940
941 int N = 0;
942 if (use_pow2 == 1) {
943 N = (int)ceil(log(fabs(y[i])) / log(2.));
944 N = (1 << N);
945 }
946 else
947 N = (int)ceil(fabs(y[i])); // numero intero.
948 if (N == 0) {
949 y[i] *= factor;
950 continue;
951 }
952 y[i] /= N;
953 //-- adesso i bits: adesso y[i] e' un numero tra -1..1
954 int k = (int)rint(y[i] * Nlevel);
955 if (k >= Nlevel) k = (int)(Nlevel - 1);
956 if (k < -Nlevel) k = (int)(-Nlevel);
958 if (y_out != NULL)
959 y_out[i] = k;
960 if (y_scale != NULL)
961 y_scale[i] = N;
962 y[i] = (double)k / (double)Nlevel;
963 //--- ripristino N
964 y[i] *= N;
965
966 }
967
968}
969
970/*******************************************************/
971
973
975{
976 int Nbins = 10000;
977
978 TH1F* h1 = new TH1F("hKVDigitalFilter", "Filter response", Nbins, 0, 1. / GetTauClk() / 2.*1000.);
980 h1->GetXaxis()->SetTitle("Frequency (MHz)");
981 printf("NCoeffs: %d\n", GetNCoeff());
982 for (int k = 0; k < Nbins; k++) {
983 double angolo = 3.14159265358979312 * k / (Nbins - 1);
984
985 double numRE = 0;
986 double numIM = 0;
987 double denRE = 0;
988 double denIM = 0;
989
990 for (int i = 0; i < GetNCoeff(); i++) {
991 double a_re = cos(angolo * i);
992 double a_im = sin(angolo * i);
993
994 numRE += GetXcoeff(i) * a_re;
995 numIM += GetXcoeff(i) * a_im;
996 if (i == 0) {
997 denRE += 1;
998 }
999 else {
1000 denRE -= GetYcoeff(i) * a_re;
1001 denIM -= GetYcoeff(i) * a_im;
1002 }
1003 }
1004 /*-------------------------*/
1005 double val = sqrt((numRE * numRE + numIM * numIM) / ((denRE * denRE + denIM * denIM)));
1006 h1->SetBinContent(k + 1, val);
1007 /*-------------------------*/
1008 }
1009 h1->Draw(option);
1010}
1011
1012/**************************************/
1013//void KVDigitalFilter::DrawResponse(Option_t* option)
1014//{
1015// KVSignal s(tau_clk);
1016// s.SetMaxT(10000); // 10 us moooolto arbitrariamente...
1017// s.SetData((int)(100/tau_clk), 1);
1018// ApplyTo(&s);
1019// s.Draw(option);
1020//}
1022//void KVDigitalFilter::DrawStepResponse(Option_t* option)
1023//{
1024// KVSignal s;
1025// s.SetChannelWidth(tau_clk);
1026// s.SetMaxT(10000); // 10 us moooolto arbitrariamente...
1027// s.SetData((int)(100/tau_clk), 1);
1028// s.Integra();
1029// ApplyTo(&s);
1030// s.Draw(option);
1031//}
1032/**************************************/
1033
1035
1037{
1038 KVDigitalFilter inv_filter(filter->GetTauClk());
1039 inv_filter.Alloc(filter->GetNCoeff());
1040 inv_filter.SetXcoeff(0, 1. / filter->GetXcoeff(0));
1041 for (int i = 1; i < filter->GetNCoeff(); i ++) {
1042 inv_filter.SetXcoeff(i, - filter->GetYcoeff(i)*inv_filter.GetXcoeff(0));
1043 inv_filter.SetYcoeff(i, - filter->GetXcoeff(i)*inv_filter.GetXcoeff(0));
1044 }
1045 return inv_filter;
1046}
1047
1048
1049
1050/********************************************/
1051
1052// function to load filter coefficients
1053
1056
1058{
1059
1060 // FILE *fin = fopen("notch_coeffs.txt","r");
1061 FILE* fin = fopen(filecoeff, "r");
1062 int i = 0;
1063 int n;
1064 if (fscanf(fin, "%d", &n)) {
1065 ;
1066 }
1067 Alloc(n);
1068 // while(fscanf(fin, "%lg",&b[i])!=EOF){
1069 for (i = 0; i < n; i++) {
1070 if (fscanf(fin, "%lg", &a[i])) {
1071 ;
1072 }
1073 b[i] = 0.0;
1074 printf("%s: i=%d a[%d]=%20.20e\n", __PRETTY_FUNCTION__, i, i, a[i]);
1075 //i++;
1076 }
1077 printf("%s: Read %d coefficients\n", __PRETTY_FUNCTION__, i);
1078 Ncoeff = i ;
1079 fclose(fin);
1080 return Ncoeff;
1081}
1082
1083
1084/********************************************/
1085
1086// function to load filter coefficients
1087
1089
1091{
1092
1093 FILE* fout = fopen(filecoeff, "w");
1094 int i = 0;
1095 fprintf(fout, "%d\n", Ncoeff);
1096 for (i = 0; i < Ncoeff; i++) {
1097 fprintf(fout, "%20.20e\n", a[i]);
1098 }
1099 printf("%s: Written %d coefficients\n", __PRETTY_FUNCTION__, i);
1100 fclose(fout);
1101 return Ncoeff;
1102}
1103
1104
1105
1106// =========================== ApplyTo per filtri FIR:
1107// eliminata ricorsivita' per risparmiare tempo:
1108//=============================================
1109
1113
1114void KVDigitalFilter::FIRApplyTo(float* datax, const int NSamples, int reverse) const
1115{
1116 // Copiato +- da KVSignal.cxx
1117 // Diversa la convenzione per a0 b0!
1118
1119 std::vector<long double> datay(NSamples);
1120 int i = 0, k = 0;
1121 switch (reverse) {
1122 case 0:// direct
1123 for (i = 0; i < Ncoeff; i++) { // primo loop su Npunti
1124 datay[i] = a[0] * datax[i];
1125 for (k = 0; k < i; k++)
1126 datay[i] += a[k + 1] * datax[i - k - 1];
1127 }
1128 for (i = Ncoeff; i < NSamples; i++) { //secondo loop. cambia l'indice interno.
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];
1132 }
1133 break; // end of direct
1134 case 1: //reverse: cut & paste from direct and NSamples-1-
1135 for (i = 0; i < Ncoeff; i++) { // primo loop su Npunti
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)];
1139 }
1140 for (i = Ncoeff; i < NSamples; i++) { //secondo loop. cambia l'indice interno.
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)];
1144 }
1145 break;
1146 case -1: // bidirectional
1147 FIRApplyTo(datax, NSamples, 0);
1148 FIRApplyTo(datax, NSamples, 1);
1149 return;
1150 default:
1151 printf("ERROR in %s: reverse=%d not supported\n", __PRETTY_FUNCTION__, reverse);
1152 }// end of reverse switch.
1153 /*----------------------------------------------*/
1154 // void *memcpy(void *dest, const void *src, size_t n);
1155 for (int i = 0; i < NSamples; i++)
1156 datax[i] = (float)datay[i];
1157}
1158
1159//=============================================
1160
1164
1165void KVDigitalFilter::FIRApplyTo(double* datax, const int NSamples, int reverse) const
1166{
1167 // Copiato +- da KVSignal.cxx
1168 // Diversa la convenzione per a0 b0!
1169
1170 std::vector<long double> datay(NSamples);
1171 int i = 0, k = 0;
1172 switch (reverse) {
1173 case 0:// direct
1174 for (i = 0; i < Ncoeff; i++) { // primo loop su Npunti
1175 datay[i] = a[0] * datax[i];
1176 for (k = 0; k < i; k++)
1177 datay[i] += a[k + 1] * datax[i - k - 1];
1178 }
1179 for (i = Ncoeff; i < NSamples; i++) { //secondo loop. cambia l'indice interno.
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];
1183 }
1184 break; // end of direct
1185 case 1: //reverse: cut & paste from direct and NSamples-1-
1186 for (i = 0; i < Ncoeff; i++) { // primo loop su Npunti
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)];
1190 }
1191 for (i = Ncoeff; i < NSamples; i++) { //secondo loop. cambia l'indice interno.
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)];
1195 }
1196 break;
1197 case -1: // bidirectional
1198 FIRApplyTo(datax, NSamples, 0);
1199 FIRApplyTo(datax, NSamples, 1);
1200 return;
1201 default:
1202 printf("ERROR in %s: reverse=%d not supported\n", __PRETTY_FUNCTION__, reverse);
1203 }// end of reverse switch.
1204 /*----------------------------------------------*/
1205 // void *memcpy(void *dest, const void *src, size_t n);
1206 // can't use memcpy: datax & datay have different sized types
1207 // (double=64bits & long double=128bits)
1208 //memcpy(datax, datay, NSamples * sizeof(double));
1209 for (int i = 0; i < NSamples; i++)
1210 datax[i] = (double)datay[i];
1211}
1212
1213
#define d(i)
#define f(i)
#define e(i)
constexpr Bool_t kFALSE
const char Option_t
const char * filters[]
#define N
winID w
winID h TVirtualViewer3D TVirtualGLPainter p
Option_t Option_t option
Option_t Option_t TPoint TPoint const char GetTextMagnitude GetFillStyle GetLineColor GetLineWidth GetMarkerStyle GetTextAlign GetTextColor GetTextSize void char Point_t Rectangle_t WindowAttributes_t Float_t Float_t Float_t b
Option_t Option_t TPoint TPoint const char GetTextMagnitude GetFillStyle GetLineColor GetLineWidth GetMarkerStyle GetTextAlign GetTextColor GetTextSize void char Point_t Rectangle_t WindowAttributes_t Float_t Float_t Float_t Int_t Int_t UInt_t UInt_t Rectangle_t Int_t Int_t Window_t TString Int_t GCValues_t GetPrimarySelectionOwner GetDisplay GetScreen GetColormap GetNativeEvent const char const char dpyName wid window const char font_name cursor keysym reg const char only_if_exist regb h Point_t np
Option_t Option_t TPoint TPoint const char x2
Option_t Option_t TPoint TPoint const char x1
Option_t Option_t TPoint TPoint const char y2
Option_t Option_t TPoint TPoint const char y1
Base class for KaliVeda framework.
Definition KVBase.h:142
static KVDigitalFilter CombineStages(const KVDigitalFilter &f1, const KVDigitalFilter &f2, int parallel=0)
const double & GetYcoeff(int i) const
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="")
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
const double & GetTauClk()
void SetXcoeff(int i, const double &val)
const double & GetXcoeff(int i) const
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
double * a
coefficients.
KVDigitalFilter operator=(const KVDigitalFilter &)
int ReadMatlabFIR(char *filecoeff)
FILE *fin = fopen("notch_coeffs.txt","r");.
static KVDigitalFilter BuildUnity(const double &tau_clk)
void PrintCoeffs() const
static KVDigitalFilter BuildRCHighPass(const double &tau_usec, const double &tau_clk)
void SetYcoeff(int i, const double &val)
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!
TAxis * GetXaxis()
void Draw(Option_t *option="") override
virtual void SetBinContent(Int_t bin, Double_t content)
virtual void SetStats(Bool_t stats=kTRUE)
virtual void SetTitle(const char *title="")
RVec< PromoteType< T > > tan(const RVec< T > &v)
RVec< PromoteType< T > > cos(const RVec< T > &v)
RVec< PromoteType< T > > ceil(const RVec< T > &v)
RVec< PromoteType< T > > log(const RVec< T > &v)
RVec< PromoteTypes< T0, T1 > > pow(const RVec< T0 > &v, const T1 &y)
RVec< PromoteType< T > > exp(const RVec< T > &v)
RVec< PromoteType< T > > sin(const RVec< T > &v)
Double_t y[n]
Double_t x[n]
const Int_t n
TH1F * h1
TF1 * f1
Expr< UnaryOp< Sqrt< T >, Expr< A, T, D, D2, R >, T >, T, D, D2, R > sqrt(const Expr< A, T, D, D2, R > &rhs)
Expr< UnaryOp< Fabs< T >, Expr< A, T, D, D2, R >, T >, T, D, D2, R > fabs(const Expr< A, T, D, D2, R > &rhs)
TMarker m
TArc a
ClassImp(TPyArg)