libaf/filter.c

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