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