home *** CD-ROM | disk | FTP | other *** search
/ Enigma Amiga Life 109 / EnigmaAmiga109CD.iso / software / musica / resample / src / remez.c < prev    next >
C/C++ Source or Header  |  1999-10-31  |  11KB  |  352 lines
  1. /*************************************************************************\
  2.   The following implementation of the Remez Exchange Algorithm (for design
  3.   of FIR linear phase filters) was converted from a Fortran program by
  4.   James H. McClellan,  Thomas W. Parks  and  Lawrence R. Rabiner,
  5.   published in:
  6.  
  7.   Lawrence R. Rabiner/Bernard Gold:  Theory and Application of Digital
  8.   Signal Processing.  London: Prentice-Hall, 1975.
  9.  
  10.   Equation numbers (where given) refer to that book.
  11. \*************************************************************************/
  12.  
  13. #include <math.h>
  14. #include <stdio.h>
  15. #include "remez.h"
  16.  
  17. #define NCOEFF  (NFMAX/2 + 2)   /* number of coefficient values */
  18. #define MAXGRID (8*NFMAX + 32)  /* frequency discretization */
  19.  
  20. double des[ MAXGRID ], grid[ MAXGRID ], wt[ MAXGRID ];
  21. int    iext[ NCOEFF ], iext0[ NCOEFF ];
  22. double eext[ NCOEFF ];
  23. double ad[ NCOEFF ], alpha[ NCOEFF ], x[ NCOEFF ], y[ NCOEFF ];
  24. float  h[ NFMAX ];
  25.  
  26. double pi, pi2;
  27.  
  28. int nfcns;          /* number of cosine functions */
  29. int ngrid;          /* frequency discretization points */
  30. int lgrid = 16;     /* grid density, <= 16 */
  31. int nfilt;          /* filter length */
  32. int remez_talk = 0; /* debugging output? */
  33. int remez_dot = 0;  /* print one '.' for each iteration? */
  34.  
  35. int remez();
  36. double d(int k, int n, int m);
  37. double gee(double f);
  38.  
  39. int makefilter( int length )
  40.     {
  41.     int nodd, j, nm1, nz, rval;
  42.     double delf, fr;
  43.  
  44.     /* Important initialization: */
  45.     pi = 4*atan(1);
  46.     pi2 = 2*pi;
  47.  
  48.     if( (nfilt = length) > NFMAX )
  49.         {
  50.         if (remez_talk)
  51.             printf("illegal filter length: %d\n", length);
  52.         return FILTER_TOO_LONG;
  53.         }
  54.     if (nfilt < 3)
  55.         {   /* You call that a filter? */
  56.         for (j = 0; j<nfilt; j++)
  57.             h[j] = 1 / (float)nfilt;
  58.         return REMEZ_OK;
  59.         }
  60.     nodd = nfilt % 2;
  61.     nfcns = nfilt / 2 + nodd;
  62.     /* Set up the frequency grid.
  63.      * Note that it isn't "complete", i. e. frequencies from transition
  64.      * bands (where the weight function returns 0) are simply left out.
  65.      */
  66.     delf = 0.5 / (nfcns * lgrid);
  67.     ngrid = 0; fr = 0;
  68.     while (fr <= (0.5 - delf))
  69.         {
  70.         if ((wt[ngrid] = weight(fr)) != 0)
  71.             {
  72.             des[ngrid] = desire(fr);
  73.             grid[ngrid] = fr;
  74.             ngrid++;
  75.             }
  76.         fr += delf;
  77.         }
  78.     /* For even <nfilt>: Set up a new approximation problem which is
  79.      * equivalent to the original problem.
  80.      */
  81.     if (nodd == 0)
  82.         for (j = 0; j<ngrid; j++)
  83.             {
  84.             des[j] /= cos(pi * grid[j]);
  85.             wt[j] *= cos(pi * grid[j]);
  86.             }
  87.     /* Initial guess for the extremal frequencies - equally spaced along
  88.      * the grid:
  89.      */
  90.     for (j = 0; j<nfcns; j++)
  91.         iext[j] = ((ngrid-1) * j) / nfcns;
  92.     iext[ nfcns ] = ngrid-1;
  93.  
  94.     rval = remez();   /* OK, let's go !!! */
  95.     if (rval != 0)
  96.         {
  97.         /* Algorithm failed to converge. There seem to be two conditions under
  98.          * which this happens:
  99.          * (1) The initial guess for the extremal frequencies is so poor that
  100.          *     the exchange iteration cannot get started, or
  101.          * (2) Near the termination of a correct design, the deviation decreases
  102.          *     due to rounding errors and the program stops. In this latter case
  103.          *     the filter design is probably acceptable, but should be checked
  104.          *     by computing a frequency response.
  105.          */
  106.         if (remez_talk)
  107.             {
  108.             printf("*** NO CONVERGENCE AFTER %d ITERATIONS ***\n", rval);
  109.             printf("Probable cause is machine rounding error.\n");
  110.             if (rval>3)
  111.                 printf("Design may be correct, but should be verified.\n");
  112.             }
  113.         if (rval>3)
  114.             rval = REMEZ_UNSURE;
  115.         else
  116.             rval = REMEZ_FAIL;
  117.         }
  118.  
  119.     /* Calculate the impulse response */
  120.     nm1 = nfcns - 1;
  121.     nz = nfcns + 1;
  122.     if (nodd)
  123.         {
  124.         for (j = 0; j <= nm1; j++)
  125.             h[j] = alpha[nm1-j];
  126.         }
  127.     else
  128.         {
  129.         h[0] = alpha[nm1];
  130.         for( j = 1; j <= nm1; j++ )
  131.             h[j] = 0.5 * (alpha[nm1-j] + alpha[nfcns-j]);
  132.         }
  133.     for( j = 0; j < nfcns; j++ )
  134.         h[ nfilt-1-j ] = h[ j ];
  135.  
  136.     return rval;
  137.     }
  138.  
  139.  
  140. float extreme( float freq )
  141. /* Returns one extremal value of the frequency response that is
  142.  * closest to the specified frequency.
  143.  */
  144.     {
  145.     int i, j;
  146.     float err, sum = 0;
  147.  
  148.     /* find an appropriate extremal frequency */
  149.     if( nfcns > 2 )  /* have we calculated extremal frequencies at all? */
  150.         {
  151.         for( j = 0; j <= nfcns; j++ )
  152.             if( j == 0 || fabs(freq - grid[iext[j]]) < err )
  153.                 {
  154.                 i = j;
  155.                 err = fabs( freq - grid[ iext[ j ] ] );
  156.                 }
  157.         freq = grid[ iext[ i ] ];
  158.         }
  159.     else
  160.         freq = 0.25;    /* not necessarily true, but who cares */
  161.     /* calculate the response */
  162.     for( j = 0; j<nfilt; j++ )
  163.         sum += h[j] * cos(pi*freq * (nfilt-1-2*j));
  164.     return sum;
  165.     }
  166.  
  167.  
  168. int remez()
  169. /* This function implements the Remez Exchange Algorithm for the weighted
  170.  * Chebyshev approximation of a continuous function with a sum of cosines.
  171.  * Inputs to the function are a dense grid which represents the frequency
  172.  * axis (grid[ngrid]), the desired function on the grid (des[], wt[]),
  173.  * the number of cosines (nfcns), and an initial guess of the extremal
  174.  * frequencies (iext[nfcns+1]).
  175.  * The program minimizes the Chebyshev error by determining the best
  176.  * location of the extremal frequencies (points of maximum error) and then
  177.  * calculates the coefficients of the best approximation.
  178.  */
  179. #define MAXITER 25
  180. /* The return value is 0 if OK, else the number of iterations attempted */
  181.     {
  182.     int iter = MAXITER;
  183.     int nz = nfcns + 1;
  184.     int j, jchnge = 0, k, l, minpos;
  185.     double dtemp, dnum, dden, dev, minval;
  186.     double devl = -1;
  187.     double err0, err1, errm1;
  188.     double cn, ft;
  189.  
  190.     while( --iter )
  191.         {
  192.         /* Calculate the Lagrange coefficients for use with funtion gee() */
  193.         for( j = 0; j<nz; j++ )
  194.             x[ j ] = cos( pi2 * grid[ iext[ j ] ] );    /* eq. (3.123) */
  195.         for (j = 0; j<nz; j++)
  196.             {
  197.             dtemp = 1;
  198.             for( l = 0; l < nz; l++ )
  199.                 if( l != j )
  200.                     dtemp *= 2 * (x[j] - x[l]);
  201.             ad[j] = 1/dtemp;    /* eq. (3.122) */
  202.             }
  203.         dnum = 0;
  204.         dden = 0;
  205.         k = 1;
  206.         for (j = 0; j<nz; j++)
  207.             {
  208.             dnum += ad[j] * des[iext[j]];
  209.             dden += k * ad[j] / wt[iext[j]];
  210.             k = -k;
  211.             }
  212.         dev = dnum / dden;      /* eq. (3.121) */
  213.         if( remez_talk )
  214.             printf("deviation = %f\n", dev);
  215.         k = 1;
  216.         if( dev>0 )
  217.             k = -1;
  218.         dev = -k*dev;
  219.         /* set new interpolation points: */
  220.         for (j = 0; j<nz; j++)
  221.             {
  222.             y[j] = des[iext[j]] + k * dev / wt[iext[j]];   /* eq. (3.124) */
  223.             k = -k;
  224.             }
  225.         if( (devl-dev)/(fabs(devl)+fabs(dev)) > 0.001 )
  226.             break;  /* convergence failure !!! */
  227.         devl = dev;
  228.  
  229.         if (remez_dot)
  230.             {
  231.             printf( "." );
  232.             fflush( stdout );
  233.             }
  234.  
  235.         /* Search for the extremal frequencies of the best approximation */
  236.         /* remember the old frequencies first: */
  237.         for (j = 0; j<nz; j++)
  238.             iext0[j] = iext[j];
  239.         j = 0;
  240.         err0 = 0;
  241.         err1 = (gee( grid[ 0 ] ) - des[ 0 ]) * wt[0];
  242.         for( l = 1; l<ngrid; l++ )
  243.             {
  244.             errm1 = err0;
  245.             err0 = err1;
  246.             err1 = (gee(grid[l]) - des[l]) * wt[l];
  247.             if ((err0 - errm1)*(err0 - err1) > 0 || (err0 == err1))
  248.                 {
  249.                 if( j == nz )
  250.                     {   /* do we have to kick out less important extremes? */
  251.                     minval = fabs(err0 - eext[j-1]);
  252.                     minpos = 0;
  253.                     for( k = 1; k<j; k++ )
  254.                         if (fabs(eext[k] - eext[k-1]) < minval)
  255.                             {
  256.                             minpos = k;
  257.                             minval = fabs( eext[k] - eext[k-1] );
  258.                             }
  259.                     if( minpos > 0 )
  260.                         {       /* yes, we can */
  261.                         if( remez_talk )
  262.                         printf( "%d & %d dropped\n", iext[minpos-1], iext[minpos] );
  263.                         for (k = minpos; k<j-1; k++)
  264.                             {
  265.                             iext[k-1] = iext[k+1];
  266.                             eext[k-1] = eext[k+1];
  267.                             }
  268.                         j -= 2;
  269.                         }
  270.                     }
  271.                 if( j < nz )
  272.                     {
  273.                     iext[ j++ ] = l-1;
  274.                     eext[ j ] = err0;
  275.                     if( remez_talk )
  276.                         printf( "%3d: %10.5f\n", l-1, err0 );
  277.                     }
  278.                 }
  279.             }
  280.         /* Add the upper edge, if we need one more extreme, OR if it may */
  281.         /* come in instead of the lower edge */
  282.         if( j < nz )
  283.             {
  284.             iext[ j++ ] = ngrid-1;
  285.             if( remez_talk )
  286.                 printf("%3d: %10.5f <\n", ngrid-1, err1);
  287.             }
  288.         else if( iext[0] == 0 && fabs(err1-eext[j-1]) > fabs( eext[1]-eext[0] ) )
  289.             {
  290.             for( k = 1; k<j; k++ )
  291.                 iext[ k-1 ] = iext[ k ];
  292.             iext[ j-1 ] = ngrid-1;
  293.             if( remez_talk )
  294.                 printf("%3d: %10.5f forced\n", ngrid-1, err1);
  295.             }
  296.         /* if still (j<nz), we've got a problem ;-) */
  297.  
  298.         /* Have there been any changes? */
  299.         jchnge = 0;
  300.         for (j = 0; j<nz; j++)
  301.             if (iext0[j] != iext[j])
  302.                 jchnge++;
  303.         if( jchnge == 0 )
  304.             break;      /* normal way of termination */
  305.         }
  306.  
  307.     /* Calculation of the coefficients of the best approximation using the
  308.      * inverse Fourier transform. This will work, because a polynom of
  309.      * cosines like gee(f) may as well be represented as a sum of cosines,
  310.      * cos(0·f) to cos((nfcns-1)·f).
  311.      */
  312.     cn = 2*nfcns;
  313.     for( j = 0; j<nfcns; j++ )
  314.         alpha[ j ] = 0;
  315.     for( k = 0; k<2*nfcns; k++ )
  316.         {
  317.         ft = k/cn;
  318.         dtemp = gee(ft)/cn;
  319.         for( j = 0; j<nfcns; j++ )
  320.             alpha[j] += dtemp * cos(pi2*ft*j);
  321.         }
  322.  
  323.     if (jchnge == 0)
  324.         return 0;
  325.     else
  326.         return MAXITER - iter;
  327.     }
  328.  
  329.  
  330. double gee( double f )
  331. /* Evaluates the frequency response using the Lagrange interpolation
  332.  * formula in the barycentric form, from ad[], x[] and y[].
  333.  */
  334.     {
  335.     double p = 0, d = 0;
  336.     double c, xf;
  337.     double fsh = 1e-6;
  338.     int j;
  339.  
  340.     xf = cos(pi2 * f);
  341.     for (j=0; j<=nfcns; j++)
  342.         {
  343.         if (fabs(xf-x[j]) < fsh)
  344.             return y[j];
  345.         c = ad[j] / (xf-x[j]);
  346.         p += c * y[j];
  347.         d += c;
  348.         }
  349.     return p/d;     /* eq. (3.125) */
  350.     }
  351.  
  352.