v2.0.0
Loading...
Searching...
No Matches
parksmcclellan.cpp
Go to the documentation of this file.
1//=============================================================================================================
12
13//=============================================================================================================
14// INCLUDES
15//=============================================================================================================
16
17#include "parksmcclellan.h"
18
19//=============================================================================================================
20// QT INCLUDES
21//=============================================================================================================
22
23#include <qmath.h>
24
25//=============================================================================================================
26// USED NAMESPACES
27//=============================================================================================================
28
29using namespace UTILSLIB;
30using namespace Eigen;
31
32//=============================================================================================================
33// DEFINES
34//=============================================================================================================
35
36constexpr int BIG = 4096; // Used to define array sizes. Must be somewhat larger than 8 * MaxNumTaps
37constexpr int SMALL = 256;
38constexpr double M_2PI = 6.28318530717958647692;
39constexpr int ITRMAX = 50; // Max Number of Iterations. Some filters require as many as 45 iterations.
40constexpr double MIN_TEST_VAL = 1.0E-6; // Min value used in LeGrangeInterp and GEE
41
42//=============================================================================================================
43// DEFINE MEMBER METHODS
44//=============================================================================================================
45
46ParksMcClellan::ParksMcClellan()
47: HalfTapCount(0)
48, ExchangeIndex(SMALL)
49, LeGrangeD(SMALL)
50, Alpha(SMALL)
51, CosOfGrid(SMALL)
52, DesPlus(SMALL)
53, Coeff(SMALL)
54, Edge(SMALL)
55, BandMag(SMALL)
56, InitWeight(SMALL)
57, DesiredMag(BIG)
58, Grid(BIG)
59, Weight(BIG)
60, InitDone2(false)
61{
62}
63
64//=============================================================================================================
65
66ParksMcClellan::ParksMcClellan(int NumTaps, double OmegaC, double BW, double ParksWidth, TPassType PassType)
67: HalfTapCount(0)
68, ExchangeIndex(SMALL)
69, LeGrangeD(SMALL)
70, Alpha(SMALL)
71, CosOfGrid(SMALL)
72, DesPlus(SMALL)
73, Coeff(SMALL)
74, Edge(SMALL)
75, BandMag(SMALL)
76, InitWeight(SMALL)
77, DesiredMag(BIG)
78, Grid(BIG)
79, Weight(BIG)
80, InitDone2(false)
81{
82 FirCoeff = RowVectorXd::Zero(NumTaps);
83 init(NumTaps, OmegaC, BW, ParksWidth, PassType);
84}
85
86//=============================================================================================================
87
91
92//=============================================================================================================
93
94void ParksMcClellan::init(int NumTaps, double OmegaC, double BW, double ParksWidth, TPassType PassType)
95{
96 // Set per pass type below; an unrecognised type leaves the band table empty
97 // rather than reading an indeterminate count.
98 int j, NumBands = 0;
99
100 if (NumTaps > 256)
101 NumTaps = 256;
102 if (NumTaps < 9)
103 NumTaps = 9;
104 if ((PassType == HPF || PassType == NOTCH) && NumTaps % 2 == 0)
105 NumTaps--;
106
107 // It helps the algorithm a great deal if each band is at least 0.01 wide.
108 // The weights used here came from the orig PM code.
109 if (PassType == LPF) {
110 NumBands = 2;
111 Edge[1] = 0.0; // Omega = 0
112 Edge[2] = OmegaC; // Pass band edge
113 if (Edge[2] < 0.01)
114 Edge[2] = 0.01;
115 if (Edge[2] > 0.98)
116 Edge[2] = 0.98;
117 Edge[3] = Edge[2] + ParksWidth; // Stop band edge
118 if (Edge[3] > 0.99)
119 Edge[3] = 0.99;
120 Edge[4] = 1.0; // Omega = Pi
121
122 BandMag[1] = 1.0;
123 BandMag[2] = 0.0;
124 InitWeight[1] = 1.0;
125 InitWeight[2] = 10.0;
126 }
127
128 if (PassType == HPF) {
129 NumBands = 2;
130 Edge[1] = 0.0; // Omega = 0
131 Edge[3] = OmegaC; // Pass band edge
132 if (Edge[3] > 0.99)
133 Edge[3] = 0.99;
134 if (Edge[3] < 0.02)
135 Edge[3] = 0.02;
136 Edge[2] = Edge[3] - ParksWidth; // Stop band edge
137 if (Edge[2] < 0.01)
138 Edge[2] = 0.01;
139 Edge[4] = 1.0; // Omega = Pi
140
141 BandMag[1] = 0.0;
142 BandMag[2] = 1.0;
143 InitWeight[1] = 10.0;
144 InitWeight[2] = 1.0;
145 }
146
147 if (PassType == BPF) {
148 NumBands = 3;
149 Edge[1] = 0.0; // Omega = 0
150 Edge[3] = OmegaC - BW / 2.0; // Left pass band edge.
151 if (Edge[3] < 0.02)
152 Edge[3] = 0.02;
153 Edge[2] = Edge[3] - ParksWidth; // Left stop band edge
154 if (Edge[2] < 0.01)
155 Edge[2] = 0.01;
156 Edge[4] = OmegaC + BW / 2.0; // Right pass band edge
157 if (Edge[4] > 0.98)
158 Edge[4] = 0.98;
159 Edge[5] = Edge[4] + ParksWidth; // Right stop band edge
160 if (Edge[5] > 0.99)
161 Edge[5] = 0.99;
162 Edge[6] = 1.0; // Omega = Pi
163
164 BandMag[1] = 0.0;
165 BandMag[2] = 1.0;
166 BandMag[3] = 0.0;
167 InitWeight[1] = 10.0;
168 InitWeight[2] = 1.0;
169 InitWeight[3] = 10.0;
170 }
171
172 if (PassType == NOTCH) {
173 NumBands = 3;
174 Edge[1] = 0.0; // Omega = 0
175 Edge[3] = OmegaC - BW / 2.0; // Left stop band edge.
176 if (Edge[3] < 0.02)
177 Edge[3] = 0.02;
178 Edge[2] = Edge[3] - ParksWidth; // Left pass band edge
179 if (Edge[2] < 0.01)
180 Edge[2] = 0.01;
181 Edge[4] = OmegaC + BW / 2.0; // Right stop band edge
182 if (Edge[4] > 0.98)
183 Edge[4] = 0.98;
184 Edge[5] = Edge[4] + ParksWidth; // Right pass band edge
185 if (Edge[5] > 0.99)
186 Edge[5] = 0.99;
187 Edge[6] = 1.0; // Omega = Pi
188
189 BandMag[1] = 1.0;
190 BandMag[2] = 0.0;
191 BandMag[3] = 1.0;
192 InitWeight[1] = 1.0;
193 InitWeight[2] = 10.0;
194 InitWeight[3] = 1.0;
195 }
196
197 // Parks McClellan's edges are based on 2Pi, we are based on Pi.
198 for (j = 1; j <= 2 * NumBands; j++)
199 Edge[j] /= 2.0;
200
201 CalcParkCoeff2(NumBands, NumTaps);
202}
203
204//=============================================================================================================
205
206void ParksMcClellan::CalcParkCoeff2(int NumBands, int TapCount)
207{
208 int j, k, GridCount, GridIndex, BandIndex, NumIterations;
209 double LowFreqEdge, UpperFreq, TempVar, Change;
210 bool OddNumTaps;
211 GridCount = 16; // Grid Density
212
213 if (TapCount % 2)
214 OddNumTaps = true;
215 else
216 OddNumTaps = false;
217
218 HalfTapCount = TapCount / 2;
219 if (OddNumTaps)
220 HalfTapCount++;
221
222 Grid[1] = Edge[1];
223 LowFreqEdge = GridCount * HalfTapCount;
224 LowFreqEdge = 0.5 / LowFreqEdge;
225 j = 1;
226 k = 1;
227 BandIndex = 1;
228 while (BandIndex <= NumBands) {
229 UpperFreq = Edge[k + 1];
230 while (Grid[j] <= UpperFreq) {
231 TempVar = Grid[j];
232 DesiredMag[j] = BandMag[BandIndex];
233 Weight[j] = InitWeight[BandIndex];
234 j++;
235 ;
236 Grid[j] = TempVar + LowFreqEdge;
237 }
238
239 Grid[j - 1] = UpperFreq;
240 DesiredMag[j - 1] = BandMag[BandIndex];
241 Weight[j - 1] = InitWeight[BandIndex];
242 k += 2;
243 BandIndex++;
244 if (BandIndex <= NumBands)
245 Grid[j] = Edge[k];
246 }
247
248 GridIndex = j - 1;
249 if (!OddNumTaps && Grid[GridIndex] > (0.5 - LowFreqEdge))
250 GridIndex--;
251
252 if (!OddNumTaps) {
253 for (j = 1; j <= GridIndex; j++) {
254 Change = cos(M_PI * Grid[j]);
255 DesiredMag[j] = DesiredMag[j] / Change;
256 Weight[j] = Weight[j] * Change;
257 }
258 }
259
260 TempVar = (double)(GridIndex - 1) / (double)HalfTapCount;
261 for (j = 1; j <= HalfTapCount; j++) {
262 ExchangeIndex[j] = (double)(j - 1) * TempVar + 1.0;
263 }
264 ExchangeIndex[HalfTapCount + 1] = GridIndex;
265
266 NumIterations = Remez2(GridIndex);
267 CalcCoefficients();
268
269 // Calculate the impulse response.
270 if (OddNumTaps) {
271 for (j = 1; j <= HalfTapCount - 1; j++) {
272 Coeff[j] = 0.5 * Alpha[HalfTapCount + 1 - j];
273 }
274 Coeff[HalfTapCount] = Alpha[1];
275 } else {
276 Coeff[1] = 0.25 * Alpha[HalfTapCount];
277 for (j = 2; j <= HalfTapCount - 1; j++) {
278 Coeff[j] = 0.25 * (Alpha[HalfTapCount + 1 - j] + Alpha[HalfTapCount + 2 - j]);
279 }
280 Coeff[HalfTapCount] = 0.5 * Alpha[1] + 0.25 * Alpha[2];
281 }
282
283 // Output section.
284 for (j = 1; j <= HalfTapCount; j++)
285 FirCoeff[j - 1] = Coeff[j];
286 if (OddNumTaps)
287 for (j = 1; j < HalfTapCount; j++)
288 FirCoeff[HalfTapCount + j - 1] = Coeff[HalfTapCount - j];
289 else
290 for (j = 1; j <= HalfTapCount; j++)
291 FirCoeff[HalfTapCount + j - 1] = Coeff[HalfTapCount - j + 1];
292
293 FirCoeff.conservativeResize(TapCount);
294
295 // Parks2Label was on my application's main form.
296 // These replace the original Ouch() function
297 if (NumIterations <= 3) {
298 //TopForm->Parks2Label->Font->Color = clRed;
299 //TopForm->Parks2Label->Caption = "Convergence ?"; // Covergence is doubtful, but possible.
300 } else {
301 //TopForm->Parks2Label->Font->Color = clBlack;
302 //TopForm->Parks2Label->Caption = UnicodeString(NumIterations) + " Iterations";
303 }
304}
305
306//=============================================================================================================
307
308int ParksMcClellan::Remez2(int GridIndex)
309{
310 // This routine is a direct port of the original goto-driven Fortran, so the
311 // compiler cannot prove that every path assigns these before use. Give them
312 // defined starting values rather than relying on the control flow.
313 int j, JET, K, k, NU, JCHNGE, K1, KNZ, KLOW, NUT, KUP;
314 int NUT1 = 0, LUCK, KN, NITER;
315 double Deviation, DNUM, DDEN, TempVar;
316 double DEVL, COMP = 0.0, YNZ = 0.0, Y1, ERR;
317
318 Y1 = 1;
319 LUCK = 0;
320 DEVL = -1.0;
321 NITER = 1; // Init this to 1 to be consistent with the orig code.
322
323TOP_LINE: // We come back to here from 3 places at the bottom.
324 ExchangeIndex[HalfTapCount + 2] = GridIndex + 1;
325
326 for (j = 1; j <= HalfTapCount + 1; j++) {
327 TempVar = Grid[ExchangeIndex[j]];
328 CosOfGrid[j] = cos(TempVar * M_2PI);
329 }
330
331 JET = (HalfTapCount - 1) / 15 + 1;
332 for (j = 1; j <= HalfTapCount + 1; j++) {
333 LeGrangeD[j] = LeGrangeInterp2(j, HalfTapCount + 1, JET);
334 }
335
336 DNUM = 0.0;
337 DDEN = 0.0;
338 K = 1;
339 for (j = 1; j <= HalfTapCount + 1; j++) {
340 k = ExchangeIndex[j];
341 DNUM += LeGrangeD[j] * DesiredMag[k];
342 DDEN += (double)K * LeGrangeD[j] / Weight[k];
343 K = -K;
344 }
345 Deviation = DNUM / DDEN;
346
347 NU = 1;
348 if (Deviation > 0.0)
349 NU = -1;
350 Deviation = -(double)NU * Deviation;
351 K = NU;
352 for (j = 1; j <= HalfTapCount + 1; j++) {
353 k = ExchangeIndex[j];
354 TempVar = (double)K * Deviation / Weight[k];
355 DesPlus[j] = DesiredMag[k] + TempVar;
356 K = -K;
357 }
358
359 if (Deviation <= DEVL)
360 return (NITER); // Ouch
361
362 DEVL = Deviation;
363 JCHNGE = 0;
364 K1 = ExchangeIndex[1];
365 KNZ = ExchangeIndex[HalfTapCount + 1];
366 KLOW = 0;
367 NUT = -NU;
368
369 //Search for the extremal frequencies of the best approximation.
370
371 j = 1;
372 while (j < HalfTapCount + 2) {
373 KUP = ExchangeIndex[j + 1];
374 k = ExchangeIndex[j] + 1;
375 NUT = -NUT;
376 if (j == 2)
377 Y1 = COMP;
378 COMP = Deviation;
379
380 if (k < KUP && !ErrTest(k, NUT, COMP, &ERR)) {
381 L210:
382 COMP = (double)NUT * ERR;
383 for (k++; k < KUP; k++) {
384 if (ErrTest(k, NUT, COMP, &ERR))
385 break; // for loop
386 COMP = (double)NUT * ERR;
387 }
388
389 ExchangeIndex[j] = k - 1;
390 j++;
391 KLOW = k - 1;
392 JCHNGE++;
393 continue; // while loop
394 }
395
396 k--;
397
398 L225:
399 k--;
400 if (k <= KLOW) {
401 k = ExchangeIndex[j] + 1;
402 if (JCHNGE > 0) {
403 ExchangeIndex[j] = k - 1;
404 j++;
405 KLOW = k - 1;
406 JCHNGE++;
407 continue; // while loop
408 } else // JCHNGE <= 0
409 {
410 for (k++; k < KUP; k++) {
411 if (ErrTest(k, NUT, COMP, &ERR))
412 continue; // for loop
413 goto L210;
414 }
415
416 KLOW = ExchangeIndex[j];
417 j++;
418 continue; // while loop
419 }
420 }
421 // Can't use a do while loop here, it would outdent the two continue statements.
422 if (ErrTest(k, NUT, COMP, &ERR) && JCHNGE <= 0)
423 goto L225;
424
425 if (ErrTest(k, NUT, COMP, &ERR)) {
426 KLOW = ExchangeIndex[j];
427 j++;
428 continue; // while loop
429 }
430
431 COMP = (double)NUT * ERR;
432
433 L235:
434 for (k--; k > KLOW; k--) {
435 if (ErrTest(k, NUT, COMP, &ERR))
436 break; // for loop
437 COMP = (double)NUT * ERR;
438 }
439
440 KLOW = ExchangeIndex[j];
441 ExchangeIndex[j] = k + 1;
442 j++;
443 JCHNGE++;
444 } // end while(j<HalfTapCount
445
446 if (j == HalfTapCount + 2)
447 YNZ = COMP;
448
449 while (j <= HalfTapCount + 2) {
450 if (K1 > ExchangeIndex[1])
451 K1 = ExchangeIndex[1];
452 if (KNZ < ExchangeIndex[HalfTapCount + 1])
453 KNZ = ExchangeIndex[HalfTapCount + 1];
454 NUT1 = NUT;
455 NUT = -NU;
456 k = 0;
457 KUP = K1;
458 COMP = YNZ * 1.00001;
459 LUCK = 1;
460
461 for (k++; k < KUP; k++) {
462 if (ErrTest(k, NUT, COMP, &ERR))
463 continue; // for loop
464 j = HalfTapCount + 2;
465 goto L210;
466 }
467 LUCK = 2;
468 break; // break while(j <= HalfTapCount+2) loop
469 } // end while(j <= HalfTapCount+2)
470
471 if (LUCK == 1 || LUCK == 2) {
472 if (LUCK == 1) {
473 if (COMP > Y1)
474 Y1 = COMP;
475 K1 = ExchangeIndex[HalfTapCount + 2];
476 }
477
478 k = GridIndex + 1;
479 KLOW = KNZ;
480 NUT = -NUT1;
481 COMP = Y1 * 1.00001;
482
483 for (k--; k > KLOW; k--) {
484 if (ErrTest(k, NUT, COMP, &ERR))
485 continue; // for loop
486 j = HalfTapCount + 2;
487 COMP = (double)NUT * ERR;
488 LUCK = 3; // last time in this if(LUCK == 1 || LUCK == 2)
489 goto L235;
490 }
491
492 if (LUCK == 2) {
493 if (JCHNGE > 0 && NITER++ < ITRMAX)
494 goto TOP_LINE;
495 else
496 return (NITER);
497 }
498
499 for (j = 1; j <= HalfTapCount; j++) {
500 ExchangeIndex[HalfTapCount + 2 - j] = ExchangeIndex[HalfTapCount + 1 - j];
501 }
502 ExchangeIndex[1] = K1;
503 if (NITER++ < ITRMAX)
504 goto TOP_LINE;
505 } // end if(LUCK == 1 || LUCK == 2)
506
507 KN = ExchangeIndex[HalfTapCount + 2];
508 for (j = 1; j <= HalfTapCount; j++) {
509 ExchangeIndex[j] = ExchangeIndex[j + 1];
510 }
511 ExchangeIndex[HalfTapCount + 1] = KN;
512 if (NITER++ < ITRMAX)
513 goto TOP_LINE;
514
515 return (NITER);
516}
517
518//=============================================================================================================
519
520double ParksMcClellan::LeGrangeInterp2(int K, int N, int M) // D
521{
522 int j, k;
523 double Dee, Q;
524 Dee = 1.0;
525 Q = CosOfGrid[K];
526 for (k = 1; k <= M; k++)
527 for (j = k; j <= N; j += M) {
528 if (j != K)
529 Dee = 2.0 * Dee * (Q - CosOfGrid[j]);
530 }
531 if (std::fabs(Dee) < MIN_TEST_VAL) {
532 if (Dee < 0.0)
533 Dee = -MIN_TEST_VAL;
534 else
535 Dee = MIN_TEST_VAL;
536 }
537 return (1.0 / Dee);
538}
539
540//=============================================================================================================
541
542double ParksMcClellan::GEE2(int K, int N)
543{
544 int j;
545 double P, C, Dee, XF;
546 P = 0.0;
547 XF = Grid[K];
548 XF = cos(M_2PI * XF);
549 Dee = 0.0;
550 for (j = 1; j <= N; j++) {
551 C = XF - CosOfGrid[j];
552 if (std::fabs(C) < MIN_TEST_VAL) {
553 if (C < 0.0)
554 C = -MIN_TEST_VAL;
555 else
556 C = MIN_TEST_VAL;
557 }
558 C = LeGrangeD[j] / C;
559 Dee = Dee + C;
560 P = P + C * DesPlus[j];
561 }
562 if (std::fabs(Dee) < MIN_TEST_VAL) {
563 if (Dee < 0.0)
564 Dee = -MIN_TEST_VAL;
565 else
566 Dee = MIN_TEST_VAL;
567 }
568 return (P / Dee);
569}
570
571//=============================================================================================================
572
573bool ParksMcClellan::ErrTest(int k, int Nut, double Comp, double* Err)
574{
575 *Err = GEE2(k, HalfTapCount + 1);
576 *Err = (*Err - DesiredMag[k]) * Weight[k];
577 if ((double)Nut * *Err - Comp <= 0.0)
578 return (true);
579 else
580 return (false);
581}
582
583//=============================================================================================================
584
585void ParksMcClellan::CalcCoefficients()
586{
587 int j, k, n;
588 double GTempVar, OneOverNumTaps;
589 double Omega, TempVar, FreqN, TempX, GridCos;
590 double GeeArray[SMALL];
591
592 GTempVar = Grid[1];
593 CosOfGrid[HalfTapCount + 2] = -2.0;
594 OneOverNumTaps = 1.0 / (double)(2 * HalfTapCount - 1);
595 k = 1;
596
597 for (j = 1; j <= HalfTapCount; j++) {
598 FreqN = (double)(j - 1) * OneOverNumTaps;
599 TempX = cos(M_2PI * FreqN);
600
601 GridCos = CosOfGrid[k];
602 if (TempX <= GridCos) {
603 while (TempX <= GridCos && (GridCos - TempX) >= MIN_TEST_VAL) // MIN_TEST_VAL = 1.0E-6
604 {
605 k++;
606 ;
607 GridCos = CosOfGrid[k];
608 }
609 }
610 if (TempX <= GridCos || (TempX - GridCos) < MIN_TEST_VAL) {
611 GeeArray[j] = DesPlus[k]; // Desired Response
612 } else {
613 Grid[1] = FreqN;
614 GeeArray[j] = GEE2(1, HalfTapCount + 1);
615 }
616 if (k > 1)
617 k--;
618 }
619
620 Grid[1] = GTempVar;
621 for (j = 1; j <= HalfTapCount; j++) {
622 TempVar = 0.0;
623 Omega = (double)(j - 1) * M_2PI * OneOverNumTaps;
624 for (n = 1; n <= HalfTapCount - 1; n++) {
625 TempVar += GeeArray[n + 1] * cos(Omega * (double)n);
626 }
627 TempVar = 2.0 * TempVar + GeeArray[1];
628 Alpha[j] = TempVar;
629 }
630
631 Alpha[1] = Alpha[1] * OneOverNumTaps;
632 for (j = 2; j <= HalfTapCount; j++) {
633 Alpha[j] = 2.0 * Alpha[j] * OneOverNumTaps;
634 }
635}
M_PI
#define M_PI
Definition coregsettingsview.cpp:15
M_2PI
constexpr double M_2PI
Definition parksmcclellan.cpp:38
BIG
constexpr int BIG
Definition parksmcclellan.cpp:36
MIN_TEST_VAL
constexpr double MIN_TEST_VAL
Definition parksmcclellan.cpp:40
SMALL
constexpr int SMALL
Definition parksmcclellan.cpp:37
ITRMAX
constexpr int ITRMAX
Definition parksmcclellan.cpp:39
parksmcclellan.h
Parks–McClellan equiripple FIR design via the Remez exchange algorithm.
UTILSLIB
Shared utilities (I/O helpers, spectral analysis, layout management, warp algorithms).
Definition rtfiffrawview.h:60
UTILSLIB::ParksMcClellan::LeGrangeInterp2
double LeGrangeInterp2(int K, int N, int M)
Definition parksmcclellan.cpp:520
UTILSLIB::ParksMcClellan::Remez2
int Remez2(int GridIndex)
Definition parksmcclellan.cpp:308
UTILSLIB::ParksMcClellan::GEE2
double GEE2(int K, int N)
Definition parksmcclellan.cpp:542
UTILSLIB::ParksMcClellan::FirCoeff
Eigen::RowVectorXd FirCoeff
Definition parksmcclellan.h:190
UTILSLIB::ParksMcClellan::ErrTest
bool ErrTest(int k, int Nut, double Comp, double *Err)
Definition parksmcclellan.cpp:573
UTILSLIB::ParksMcClellan::CalcParkCoeff2
void CalcParkCoeff2(int NBANDS, int NFILT)
Definition parksmcclellan.cpp:206
UTILSLIB::ParksMcClellan::init
void init(int NumTaps, double OmegaC, double BW, double ParksWidth, TPassType PassType)
Definition parksmcclellan.cpp:94
Generated on for MNE-CPP by doxygen 1.16.1