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