libaf/filter.c

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