summaryrefslogtreecommitdiffstats
path: root/libaf/filter.c
blob: b272125fd8bd50241332b07df1c784b6ffad9bf1 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
/*
 * design and implementation of different types of digital filters
 *
 * Copyright (C) 2001 Anders Johansson ajh@atri.curtin.edu.au
 *
 * This file is part of MPlayer.
 *
 * MPlayer is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * MPlayer is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with MPlayer; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <string.h>
#include <math.h>
#include "dsp.h"

/******************************************************************************
*  FIR filter implementations
******************************************************************************/

/* C implementation of FIR filter y=w*x

   n number of filter taps, where mod(n,4)==0
   w filter taps
   x input signal must be a circular buffer which is indexed backwards
*/
inline FLOAT_TYPE af_filter_fir(register unsigned int n, const FLOAT_TYPE* w,
                                const FLOAT_TYPE* x)
{
  register FLOAT_TYPE y; // Output
  y = 0.0;
  do{
    n--;
    y+=w[n]*x[n];
  }while(n != 0);
  return y;
}

/******************************************************************************
*  FIR filter design
******************************************************************************/

/* Design FIR filter using the Window method

   n     filter length must be odd for HP and BS filters
   w     buffer for the filter taps (must be n long)
   fc    cutoff frequencies (1 for LP and HP, 2 for BP and BS)
         0 < fc < 1 where 1 <=> Fs/2
   flags window and filter type as defined in filter.h
         variables are ored together: i.e. LP|HAMMING will give a
	 low pass filter designed using a hamming window
   opt   beta constant used only when designing using kaiser windows

   returns 0 if OK, -1 if fail
*/
int af_filter_design_fir(unsigned int n, FLOAT_TYPE* w, const FLOAT_TYPE* fc,
                         unsigned int flags, FLOAT_TYPE opt)
{
  unsigned int	o   = n & 1;          	// Indicator for odd filter length
  unsigned int	end = ((n + 1) >> 1) - o;       // Loop end
  unsigned int	i;			// Loop index

  FLOAT_TYPE k1 = 2 * M_PI;		// 2*pi*fc1
  FLOAT_TYPE k2 = 0.5 * (FLOAT_TYPE)(1 - o);// Constant used if the filter has even length
  FLOAT_TYPE k3;		        // 2*pi*fc2 Constant used in BP and BS design
  FLOAT_TYPE g  = 0.0;     		// Gain
  FLOAT_TYPE t1,t2,t3;     		// Temporary variables
  FLOAT_TYPE fc1,fc2;			// Cutoff frequencies

  // Sanity check
  if(!w || (n == 0)) return -1;

  // Get window coefficients
  switch(flags & WINDOW_MASK){
  case(BOXCAR):
    af_window_boxcar(n,w); break;
  case(TRIANG):
    af_window_triang(n,w); break;
  case(HAMMING):
    af_window_hamming(n,w); break;
  case(HANNING):
    af_window_hanning(n,w); break;
  case(BLACKMAN):
    af_window_blackman(n,w); break;
  case(FLATTOP):
    af_window_flattop(n,w); break;
  case(KAISER):
    af_window_kaiser(n,w,opt); break;
  default:
    return -1;
  }

  if(flags & (LP | HP)){
    fc1=*fc;
    // Cutoff frequency must be < 0.5 where 0.5 <=> Fs/2
    fc1 = ((fc1 <= 1.0) && (fc1 > 0.0)) ? fc1/2 : 0.25;
    k1 *= fc1;

    if(flags & LP){ // Low pass filter

      // If the filter length is odd, there is one point which is exactly
      // in the middle. The value at this point is 2*fCutoff*sin(x)/x,
      // where x is zero. To make sure nothing strange happens, we set this
      // value separately.
      if (o){
	w[end] = fc1 * w[end] * 2.0;
	g=w[end];
      }

      // Create filter
      for (i=0 ; i<end ; i++){
	t1 = (FLOAT_TYPE)(i+1) - k2;
	w[end-i-1] = w[n-end+i] = w[end-i-1] * sin(k1 * t1)/(M_PI * t1); // Sinc
	g += 2*w[end-i-1]; // Total gain in filter
      }
    }
    else{ // High pass filter
      if (!o) // High pass filters must have odd length
	return -1;
      w[end] = 1.0 - (fc1 * w[end] * 2.0);
      g= w[end];

      // Create filter
      for (i=0 ; i<end ; i++){
	t1 = (FLOAT_TYPE)(i+1);
	w[end-i-1] = w[n-end+i] = -1 * w[end-i-1] * sin(k1 * t1)/(M_PI * t1); // Sinc
	g += ((i&1) ? (2*w[end-i-1]) : (-2*w[end-i-1])); // Total gain in filter
      }
    }
  }

  if(flags & (BP | BS)){
    fc1=fc[0];
    fc2=fc[1];
    // Cutoff frequencies must be < 1.0 where 1.0 <=> Fs/2
    fc1 = ((fc1 <= 1.0) && (fc1 > 0.0)) ? fc1/2 : 0.25;
    fc2 = ((fc2 <= 1.0) && (fc2 > 0.0)) ? fc2/2 : 0.25;
    k3  = k1 * fc2; // 2*pi*fc2
    k1 *= fc1;      // 2*pi*fc1

    if(flags & BP){ // Band pass
      // Calculate center tap
      if (o){
	g=w[end]*(fc1+fc2);
	w[end] = (fc2 - fc1) * w[end] * 2.0;
      }

      // Create filter
      for (i=0 ; i<end ; i++){
	t1 = (FLOAT_TYPE)(i+1) - k2;
	t2 = sin(k3 * t1)/(M_PI * t1); // Sinc fc2
	t3 = sin(k1 * t1)/(M_PI * t1); // Sinc fc1
	g += w[end-i-1] * (t3 + t2);   // Total gain in filter
	w[end-i-1] = w[n-end+i] = w[end-i-1] * (t2 - t3);
      }
    }
    else{ // Band stop
      if (!o) // Band stop filters must have odd length
	return -1;
      w[end] = 1.0 - (fc2 - fc1) * w[end] * 2.0;
      g= w[end];

      // Create filter
      for (i=0 ; i<end ; i++){
	t1 = (FLOAT_TYPE)(i+1);
	t2 = sin(k1 * t1)/(M_PI * t1); // Sinc fc1
	t3 = sin(k3 * t1)/(M_PI * t1); // Sinc fc2
	w[end-i-1] = w[n-end+i] = w[end-i-1] * (t2 - t3);
	g += 2*w[end-i-1]; // Total gain in filter
      }
    }
  }

  // Normalize gain
  g=1/g;
  for (i=0; i<n; i++)
    w[i] *= g;

  return 0;
}

/******************************************************************************
*  IIR filter design
******************************************************************************/

/* Helper functions for the bilinear transform */

/* Pre-warp the coefficients of a numerator or denominator.
   Note that a0 is assumed to be 1, so there is no wrapping
   of it.
*/
static void af_filter_prewarp(FLOAT_TYPE* a, FLOAT_TYPE fc, FLOAT_TYPE fs)
{
  FLOAT_TYPE wp;
  wp = 2.0 * fs * tan(M_PI * fc / fs);
  a[2] = a[2]/(wp * wp);
  a[1] = a[1]/wp;
}

/* Transform the numerator and denominator coefficients of s-domain
   biquad section into corresponding z-domain coefficients.

   The transfer function for z-domain is:

          1 + alpha1 * z^(-1) + alpha2 * z^(-2)
   H(z) = -------------------------------------
          1 + beta1 * z^(-1) + beta2 * z^(-2)

   Store the 4 IIR coefficients in array pointed by coef in following
   order:
   beta1, beta2    (denominator)
   alpha1, alpha2  (numerator)

   Arguments:
   a       - s-domain numerator coefficients
   b       - s-domain denominator coefficients
   k 	   - filter gain factor. Initially set to 1 and modified by each
             biquad section in such a way, as to make it the
             coefficient by which to multiply the overall filter gain
             in order to achieve a desired overall filter gain,
             specified in initial value of k.
   fs 	   - sampling rate (Hz)
   coef    - array of z-domain coefficients to be filled in.

   Return: On return, set coef z-domain coefficients and k to the gain
   required to maintain overall gain = 1.0;
*/
static void af_filter_bilinear(const FLOAT_TYPE* a, const FLOAT_TYPE* b, FLOAT_TYPE* k,
                               FLOAT_TYPE fs, FLOAT_TYPE *coef)
{
  FLOAT_TYPE ad, bd;

  /* alpha (Numerator in s-domain) */
  ad = 4. * a[2] * fs * fs + 2. * a[1] * fs + a[0];
  /* beta (Denominator in s-domain) */
  bd = 4. * b[2] * fs * fs + 2. * b[1] * fs + b[0];

  /* Update gain constant for this section */
  *k *= ad/bd;

  /* Denominator */
  *coef++ = (2. * b[0] - 8. * b[2] * fs * fs)/bd; /* beta1 */
  *coef++ = (4. * b[2] * fs * fs - 2. * b[1] * fs + b[0])/bd; /* beta2 */

  /* Numerator */
  *coef++ = (2. * a[0] - 8. * a[2] * fs * fs)/ad; /* alpha1 */
  *coef   = (4. * a[2] * fs * fs - 2. * a[1] * fs + a[0])/ad;   /* alpha2 */
}



/* IIR filter design using bilinear transform and prewarp. Transforms
   2nd order s domain analog filter into a digital IIR biquad link. To
   create a filter fill in a, b, Q and fs and make space for coef and k.


   Example Butterworth design:

   Below are Butterworth polynomials, arranged as a series of 2nd
   order sections:

   Note: n is filter order.

   n  Polynomials
   -------------------------------------------------------------------
   2  s^2 + 1.4142s + 1
   4  (s^2 + 0.765367s + 1) * (s^2 + 1.847759s + 1)
   6  (s^2 + 0.5176387s + 1) * (s^2 + 1.414214 + 1) * (s^2 + 1.931852s + 1)

   For n=4 we have following equation for the filter transfer function:
                       1                              1
   T(s) = --------------------------- * ----------------------------
          s^2 + (1/Q) * 0.765367s + 1   s^2 + (1/Q) * 1.847759s + 1

   The filter consists of two 2nd order sections since highest s power
   is 2.  Now we can take the coefficients, or the numbers by which s
   is multiplied and plug them into a standard formula to be used by
   bilinear transform.

   Our standard form for each 2nd order section is:

          a2 * s^2 + a1 * s + a0
   H(s) = ----------------------
          b2 * s^2 + b1 * s + b0

   Note that Butterworth numerator is 1 for all filter sections, which
   means s^2 = 0 and s^1 = 0

   Let's convert standard Butterworth polynomials into this form:

             0 + 0 + 1                  0 + 0 + 1
   --------------------------- * --------------------------
   1 + ((1/Q) * 0.765367) + 1   1 + ((1/Q) * 1.847759) + 1

   Section 1:
   a2 = 0; a1 = 0; a0 = 1;
   b2 = 1; b1 = 0.765367; b0 = 1;

   Section 2:
   a2 = 0; a1 = 0; a0 = 1;
   b2 = 1; b1 = 1.847759; b0 = 1;

   Q is filter quality factor or resonance, in the range of 1 to
   1000. The overall filter Q is a product of all 2nd order stages.
   For example, the 6th order filter (3 stages, or biquads) with
   individual Q of 2 will have filter Q = 2 * 2 * 2 = 8.


   Arguments:
   a       - s-domain numerator coefficients, a[1] is always assumed to be 1.0
   b       - s-domain denominator coefficients
   Q	   - Q value for the filter
   k 	   - filter gain factor. Initially set to 1 and modified by each
             biquad section in such a way, as to make it the
             coefficient by which to multiply the overall filter gain
             in order to achieve a desired overall filter gain,
             specified in initial value of k.
   fs 	   - sampling rate (Hz)
   coef    - array of z-domain coefficients to be filled in.

   Note: Upon return from each call, the k argument will be set to a
   value, by which to multiply our actual signal in order for the gain
   to be one. On second call to szxform() we provide k that was
   changed by the previous section. During actual audio filtering
   k can be used for gain compensation.

   return -1 if fail 0 if success.
*/
int af_filter_szxform(const FLOAT_TYPE* a, const FLOAT_TYPE* b, FLOAT_TYPE Q, FLOAT_TYPE fc,
                      FLOAT_TYPE fs, FLOAT_TYPE *k, FLOAT_TYPE *coef)
{
  FLOAT_TYPE at[3];
  FLOAT_TYPE bt[3];

  if(!a || !b || !k || !coef || (Q>1000.0 || Q< 1.0))
    return -1;

  memcpy(at,a,3*sizeof(FLOAT_TYPE));
  memcpy(bt,b,3*sizeof(FLOAT_TYPE));

  bt[1]/=Q;

  /* Calculate a and b and overwrite the original values */
  af_filter_prewarp(at, fc, fs);
  af_filter_prewarp(bt, fc, fs);
  /* Execute bilinear transform */
  af_filter_bilinear(at, bt, k, fs, coef);

  return 0;
}