libaf/filter.c

   1 /*=============================================================================
   2 //
   3 //  This software has been released under the terms of the GNU Public
   4 //  license. See http://www.gnu.org/copyleft/gpl.html for details.
   5 //
   6 //  Copyright 2001 Anders Johansson ajh@atri.curtin.edu.au
   7 //
   8 //=============================================================================
   9 */
  10
  11 /* Design and implementation of different types of digital filters
  12
  13 */
  14 #include <string.h>
  15 #include <math.h>
  16 #include "dsp.h"
  17
  18 /******************************************************************************
  19 *  FIR filter implementations
  20 ******************************************************************************/
  21
  22 /* C implementation of FIR filter y=w*x
  23
  24    n number of filter taps, where mod(n,4)==0
  25    w filter taps
  26    x input signal must be a circular buffer which is indexed backwards
  27 */
  28 inline _ftype_t fir(register unsigned int n, _ftype_t* w, _ftype_t* x)
  29 {
  30   register _ftype_t y; // Output
  31   y = 0.0;
  32   do{
  33     n--;
  34     y+=w[n]*x[n];
  35   }while(n != 0);
  36   return y;
  37 }
  38
  39 /* C implementation of parallel FIR filter y(k)=w(k) * x(k) (where * denotes convolution)
  40
  41    n  number of filter taps, where mod(n,4)==0
  42    d  number of filters
  43    xi current index in xq
  44    w  filter taps k by n big
  45    x  input signal must be a circular buffers which are indexed backwards
  46    y  output buffer
  47    s  output buffer stride
  48 */
  49 inline _ftype_t* pfir(unsigned int n, unsigned int d, unsigned int xi, _ftype_t** w, _ftype_t** x, _ftype_t* y, unsigned int s)
  50 {
  51   register _ftype_t* xt = *x + xi;
  52   register _ftype_t* wt = *w;
  53   register int    nt = 2*n;
  54   while(d-- > 0){
  55     *y = fir(n,wt,xt);
  56     wt+=n;
  57     xt+=nt;
  58     y+=s;
  59   }
  60   return y;
  61 }
  62
  63 /* Add new data to circular queue designed to be used with a parallel
  64    FIR filter, with d filters. xq is the circular queue, in pointing
  65    at the new samples, xi current index in xq and n the length of the
  66    filter. xq must be n*2 by k big, s is the index for in.
  67 */
  68 inline int updatepq(unsigned int n, unsigned int d, unsigned int xi, _ftype_t** xq, _ftype_t* in, unsigned int s)
  69 {
  70   register _ftype_t* txq = *xq + xi;
  71   register int nt = n*2;
  72
  73   while(d-- >0){
  74     *txq= *(txq+n) = *in;
  75     txq+=nt;
  76     in+=s;
  77   }
  78   return (++xi)&(n-1);
  79 }
  80
  81 /******************************************************************************
  82 *  FIR filter design
  83 ******************************************************************************/
  84
  85 /* Design FIR filter using the Window method
  86
  87    n     filter length must be odd for HP and BS filters
  88    w     buffer for the filter taps (must be n long)
  89    fc    cutoff frequencies (1 for LP and HP, 2 for BP and BS)
  90          0 < fc < 1 where 1 <=> Fs/2
  91    flags window and filter type as defined in filter.h
  92          variables are ored together: i.e. LP|HAMMING will give a
  93          low pass filter designed using a hamming window
  94    opt   beta constant used only when designing using kaiser windows
  95
  96    returns 0 if OK, -1 if fail
  97 */
  98 int design_fir(unsigned int n, _ftype_t* w, _ftype_t* fc, unsigned int flags, _ftype_t opt)
  99 {
 100   unsigned int  o   = n & 1;            // Indicator for odd filter length
 101   unsigned int  end = ((n + 1) >> 1) - o;       // Loop end
 102   unsigned int  i;                      // Loop index
 103
 104   _ftype_t k1 = 2 * M_PI;               // 2*pi*fc1
 105   _ftype_t k2 = 0.5 * (_ftype_t)(1 - o);// Constant used if the filter has even length
 106   _ftype_t k3;                          // 2*pi*fc2 Constant used in BP and BS design
 107   _ftype_t g  = 0.0;                    // Gain
 108   _ftype_t t1,t2,t3;                    // Temporary variables
 109   _ftype_t fc1,fc2;                     // Cutoff frequencies
 110
 111   // Sanity check
 112   if(!w || (n == 0)) return -1;
 113
 114   // Get window coefficients
 115   switch(flags & WINDOW_MASK){
 116   case(BOXCAR):
 117     boxcar(n,w); break;
 118   case(TRIANG):
 119     triang(n,w); break;
 120   case(HAMMING):
 121     hamming(n,w); break;
 122   case(HANNING):
 123     hanning(n,w); break;
 124   case(BLACKMAN):
 125     blackman(n,w); break;
 126   case(FLATTOP):
 127     flattop(n,w); break;
 128   case(KAISER):
 129     kaiser(n,w,opt); break;
 130   default:
 131     return -1;
 132   }
 133
 134   if(flags & (LP | HP)){
 135     fc1=*fc;
 136     // Cutoff frequency must be < 0.5 where 0.5 <=> Fs/2
 137     fc1 = ((fc1 <= 1.0) && (fc1 > 0.0)) ? fc1/2 : 0.25;
 138     k1 *= fc1;
 139
 140     if(flags & LP){ // Low pass filter
 141
 142       // If the filter length is odd, there is one point which is exactly
 143       // in the middle. The value at this point is 2*fCutoff*sin(x)/x,
 144       // where x is zero. To make sure nothing strange happens, we set this
 145       // value separately.
 146       if (o){
 147         w[end] = fc1 * w[end] * 2.0;
 148         g=w[end];
 149       }
 150
 151       // Create filter
 152       for (i=0 ; i<end ; i++){
 153         t1 = (_ftype_t)(i+1) - k2;
 154         w[end-i-1] = w[n-end+i] = w[end-i-1] * sin(k1 * t1)/(M_PI * t1); // Sinc
 155         g += 2*w[end-i-1]; // Total gain in filter
 156       }
 157     }
 158     else{ // High pass filter
 159       if (!o) // High pass filters must have odd length
 160         return -1;
 161       w[end] = 1.0 - (fc1 * w[end] * 2.0);
 162       g= w[end];
 163
 164       // Create filter
 165       for (i=0 ; i<end ; i++){
 166         t1 = (_ftype_t)(i+1);
 167         w[end-i-1] = w[n-end+i] = -1 * w[end-i-1] * sin(k1 * t1)/(M_PI * t1); // Sinc
 168         g += ((i&1) ? (2*w[end-i-1]) : (-2*w[end-i-1])); // Total gain in filter
 169       }
 170     }
 171   }
 172
 173   if(flags & (BP | BS)){
 174     fc1=fc[0];
 175     fc2=fc[1];
 176     // Cutoff frequencies must be < 1.0 where 1.0 <=> Fs/2
 177     fc1 = ((fc1 <= 1.0) && (fc1 > 0.0)) ? fc1/2 : 0.25;
 178     fc2 = ((fc2 <= 1.0) && (fc2 > 0.0)) ? fc2/2 : 0.25;
 179     k3  = k1 * fc2; // 2*pi*fc2
 180     k1 *= fc1;      // 2*pi*fc1
 181
 182     if(flags & BP){ // Band pass
 183       // Calculate center tap
 184       if (o){
 185         g=w[end]*(fc1+fc2);
 186         w[end] = (fc2 - fc1) * w[end] * 2.0;
 187       }
 188
 189       // Create filter
 190       for (i=0 ; i<end ; i++){
 191         t1 = (_ftype_t)(i+1) - k2;
 192         t2 = sin(k3 * t1)/(M_PI * t1); // Sinc fc2
 193         t3 = sin(k1 * t1)/(M_PI * t1); // Sinc fc1
 194         g += w[end-i-1] * (t3 + t2);   // Total gain in filter
 195         w[end-i-1] = w[n-end+i] = w[end-i-1] * (t2 - t3);
 196       }
 197     }
 198     else{ // Band stop
 199       if (!o) // Band stop filters must have odd length
 200         return -1;
 201       w[end] = 1.0 - (fc2 - fc1) * w[end] * 2.0;
 202       g= w[end];
 203
 204       // Create filter
 205       for (i=0 ; i<end ; i++){
 206         t1 = (_ftype_t)(i+1);
 207         t2 = sin(k1 * t1)/(M_PI * t1); // Sinc fc1
 208         t3 = sin(k3 * t1)/(M_PI * t1); // Sinc fc2
 209         w[end-i-1] = w[n-end+i] = w[end-i-1] * (t2 - t3);
 210         g += 2*w[end-i-1]; // Total gain in filter
 211       }
 212     }
 213   }
 214
 215   // Normalize gain
 216   g=1/g;
 217   for (i=0; i<n; i++)
 218     w[i] *= g;
 219
 220   return 0;
 221 }
 222
 223 /* Design polyphase FIR filter from prototype filter
 224
 225    n     length of prototype filter
 226    k     number of polyphase components
 227    w     prototype filter taps
 228    pw    Parallel FIR filter
 229    g     Filter gain
 230    flags FWD forward indexing
 231          REW reverse indexing
 232          ODD multiply every 2nd filter tap by -1 => HP filter
 233
 234    returns 0 if OK, -1 if fail
 235 */
 236 int design_pfir(unsigned int n, unsigned int k, _ftype_t* w, _ftype_t** pw, _ftype_t g, unsigned int flags)
 237 {
 238   int l = (int)n/k;     // Length of individual FIR filters
 239   int i;        // Counters
 240   int j;
 241   _ftype_t t;   // g * w[i]
 242
 243   // Sanity check
 244   if(l<1 || k<1 || !w || !pw)
 245     return -1;
 246
 247   // Do the stuff
 248   if(flags&REW){
 249     for(j=l-1;j>-1;j--){//Columns
 250       for(i=0;i<(int)k;i++){//Rows
 251         t=g *  *w++;
 252         pw[i][j]=t * ((flags & ODD) ? ((j & 1) ? -1 : 1) : 1);
 253       }
 254     }
 255   }
 256   else{
 257     for(j=0;j<l;j++){//Columns
 258       for(i=0;i<(int)k;i++){//Rows
 259         t=g *  *w++;
 260         pw[i][j]=t * ((flags & ODD) ? ((j & 1) ? 1 : -1) : 1);
 261       }
 262     }
 263   }
 264   return -1;
 265 }
 266
 267 /******************************************************************************
 268 *  IIR filter design
 269 ******************************************************************************/
 270
 271 /* Helper functions for the bilinear transform */
 272
 273 /* Pre-warp the coefficients of a numerator or denominator.
 274    Note that a0 is assumed to be 1, so there is no wrapping
 275    of it.
 276 */
 277 void prewarp(_ftype_t* a, _ftype_t fc, _ftype_t fs)
 278 {
 279   _ftype_t wp;
 280   wp = 2.0 * fs * tan(M_PI * fc / fs);
 281   a[2] = a[2]/(wp * wp);
 282   a[1] = a[1]/wp;
 283 }
 284
 285 /* Transform the numerator and denominator coefficients of s-domain
 286    biquad section into corresponding z-domain coefficients.
 287
 288    The transfer function for z-domain is:
 289
 290           1 + alpha1 * z^(-1) + alpha2 * z^(-2)
 291    H(z) = -------------------------------------
 292           1 + beta1 * z^(-1) + beta2 * z^(-2)
 293
 294    Store the 4 IIR coefficients in array pointed by coef in following
 295    order:
 296    beta1, beta2    (denominator)
 297    alpha1, alpha2  (numerator)
 298
 299    Arguments:
 300    a       - s-domain numerator coefficients
 301    b       - s-domain denominator coefficients
 302    k       - filter gain factor. Initially set to 1 and modified by each
 303              biquad section in such a way, as to make it the
 304              coefficient by which to multiply the overall filter gain
 305              in order to achieve a desired overall filter gain,
 306              specified in initial value of k.
 307    fs      - sampling rate (Hz)
 308    coef    - array of z-domain coefficients to be filled in.
 309
 310    Return: On return, set coef z-domain coefficients and k to the gain
 311    required to maintain overall gain = 1.0;
 312 */
 313 void bilinear(_ftype_t* a, _ftype_t* b, _ftype_t* k, _ftype_t fs, _ftype_t *coef)
 314 {
 315   _ftype_t ad, bd;
 316
 317   /* alpha (Numerator in s-domain) */
 318   ad = 4. * a[2] * fs * fs + 2. * a[1] * fs + a[0];
 319   /* beta (Denominator in s-domain) */
 320   bd = 4. * b[2] * fs * fs + 2. * b[1] * fs + b[0];
 321
 322   /* Update gain constant for this section */
 323   *k *= ad/bd;
 324
 325   /* Denominator */
 326   *coef++ = (2. * b[0] - 8. * b[2] * fs * fs)/bd; /* beta1 */
 327   *coef++ = (4. * b[2] * fs * fs - 2. * b[1] * fs + b[0])/bd; /* beta2 */
 328
 329   /* Numerator */
 330   *coef++ = (2. * a[0] - 8. * a[2] * fs * fs)/ad; /* alpha1 */
 331   *coef   = (4. * a[2] * fs * fs - 2. * a[1] * fs + a[0])/ad;   /* alpha2 */
 332 }
 333
 334
 335
 336 /* IIR filter design using bilinear transform and prewarp. Transforms
 337    2nd order s domain analog filter into a digital IIR biquad link. To
 338    create a filter fill in a, b, Q and fs and make space for coef and k.
 339
 340
 341    Example Butterworth design:
 342
 343    Below are Butterworth polynomials, arranged as a series of 2nd
 344    order sections:
 345
 346    Note: n is filter order.
 347
 348    n  Polynomials
 349    -------------------------------------------------------------------
 350    2  s^2 + 1.4142s + 1
 351    4  (s^2 + 0.765367s + 1) * (s^2 + 1.847759s + 1)
 352    6  (s^2 + 0.5176387s + 1) * (s^2 + 1.414214 + 1) * (s^2 + 1.931852s + 1)
 353
 354    For n=4 we have following equation for the filter transfer function:
 355                        1                              1
 356    T(s) = --------------------------- * ----------------------------
 357           s^2 + (1/Q) * 0.765367s + 1   s^2 + (1/Q) * 1.847759s + 1
 358
 359    The filter consists of two 2nd order sections since highest s power
 360    is 2.  Now we can take the coefficients, or the numbers by which s
 361    is multiplied and plug them into a standard formula to be used by
 362    bilinear transform.
 363
 364    Our standard form for each 2nd order section is:
 365
 366           a2 * s^2 + a1 * s + a0
 367    H(s) = ----------------------
 368           b2 * s^2 + b1 * s + b0
 369
 370    Note that Butterworth numerator is 1 for all filter sections, which
 371    means s^2 = 0 and s^1 = 0
 372
 373    Let's convert standard Butterworth polynomials into this form:
 374
 375              0 + 0 + 1                  0 + 0 + 1
 376    --------------------------- * --------------------------
 377    1 + ((1/Q) * 0.765367) + 1   1 + ((1/Q) * 1.847759) + 1
 378
 379    Section 1:
 380    a2 = 0; a1 = 0; a0 = 1;
 381    b2 = 1; b1 = 0.765367; b0 = 1;
 382
 383    Section 2:
 384    a2 = 0; a1 = 0; a0 = 1;
 385    b2 = 1; b1 = 1.847759; b0 = 1;
 386
 387    Q is filter quality factor or resonance, in the range of 1 to
 388    1000. The overall filter Q is a product of all 2nd order stages.
 389    For example, the 6th order filter (3 stages, or biquads) with
 390    individual Q of 2 will have filter Q = 2 * 2 * 2 = 8.
 391
 392
 393    Arguments:
 394    a       - s-domain numerator coefficients, a[1] is always assumed to be 1.0
 395    b       - s-domain denominator coefficients
 396    Q       - Q value for the filter
 397    k       - filter gain factor. Initially set to 1 and modified by each
 398              biquad section in such a way, as to make it the
 399              coefficient by which to multiply the overall filter gain
 400              in order to achieve a desired overall filter gain,
 401              specified in initial value of k.
 402    fs      - sampling rate (Hz)
 403    coef    - array of z-domain coefficients to be filled in.
 404
 405    Note: Upon return from each call, the k argument will be set to a
 406    value, by which to multiply our actual signal in order for the gain
 407    to be one. On second call to szxform() we provide k that was
 408    changed by the previous section. During actual audio filtering
 409    k can be used for gain compensation.
 410
 411    return -1 if fail 0 if success.
 412 */
 413 int szxform(_ftype_t* a, _ftype_t* b, _ftype_t Q, _ftype_t fc, _ftype_t fs, _ftype_t *k, _ftype_t *coef)
 414 {
 415   _ftype_t at[3];
 416   _ftype_t bt[3];
 417
 418   if(!a || !b || !k || !coef || (Q>1000.0 || Q< 1.0))
 419     return -1;
 420
 421   memcpy(at,a,3*sizeof(_ftype_t));
 422   memcpy(bt,b,3*sizeof(_ftype_t));
 423
 424   bt[1]/=Q;
 425
 426   /* Calculate a and b and overwrite the original values */
 427   prewarp(at, fc, fs);
 428   prewarp(bt, fc, fs);
 429   /* Execute bilinear transform */
 430   bilinear(at, bt, k, fs, coef);
 431
 432   return 0;
 433 }
 434