« back to all changes in this revision
Viewing changes to src/common-filters.c
- browse files at revision 1
- compare with another revision
- download diff
- download tarball
- view history from revision 1
- Committer: Package Import Robot
- Author(s): Nicolas Bourdaud
- Date: 2011-12-01 12:09:30 UTC
- Revision ID: package-import@ubuntu.com-20111201120930-lmia8ytlwmif9yta
Tags: upstream-1.1
Import upstream version 1.1
- files added:
- build-aux
- config
- debian
- doc
- doc/examples
- m4
- src
- tests
added
removed
src/common-filters.c
1
/*
2
Copyright (C) 2008-2011 Nicolas Bourdaud <nicolas.bourdaud@epfl.ch>
3
4
This file is part of the rtfilter library
5
6
The rtfilter library is free software: you can redistribute it and/or
7
modify it under the terms of the version 3 of the GNU Lesser General
8
Public License as published by the Free Software Foundation.
9
10
This program is distributed in the hope that it will be useful,
11
but WITHOUT ANY WARRANTY; without even the implied warranty of
12
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13
GNU Lesser General Public License for more details.
14
15
You should have received a copy of the GNU Lesser General Public License
16
along with this program. If not, see <http://www.gnu.org/licenses/>.
17
*/
18
#if HAVE_CONFIG_H
19
# include <config.h>
20
#endif
21
22
#include <memory.h>
23
#include <stdlib.h>
24
#include <math.h>
25
#include <complex.h>
26
#include "rtfilter.h"
27
#include "rtf_common.h"
28
29
#define PId 3.1415926535897932384626433832795L
30
#define PIf 3.1415926535897932384626433832795f
31
32
/***************************************************************
33
* *
34
* Helper functions *
35
* *
36
***************************************************************/
37
static
38
void apply_window(double *fir, unsigned int length, KernelWindow window)
39
{
40
unsigned int i;
41
double M = length - 1;
42
43
switch (window) {
44
case HAMMING_WINDOW:
45
for (i = 0; i < length; i++)
46
fir[i] *= 0.54 + 0.46 * cos(2.0 * PIf * ((double)i / M - 0.5));
47
break;
48
49
case BLACKMAN_WINDOW:
50
for (i = 0; i < length; i++)
51
fir[i] *=
52
0.42 +
53
0.5 * cos(2.0 * PIf * ((double) i / M - 0.5)) +
54
0.08 * cos(4.0 * PIf * ((double) i / M - 0.5));
55
break;
56
57
case RECT_WINDOW:
58
break;
59
}
60
}
61
62
63
static
64
void normalize_fir(double *fir, unsigned int length)
65
{
66
unsigned int i;
67
double sum = 0.0;
68
69
for (i = 0; i < length; i++)
70
sum += fir[i];
71
72
for (i = 0; i < length; i++)
73
fir[i] /= sum;
74
}
75
76
static
77
void compute_convolution(double *product, double *sig1, unsigned int len1,
78
double *sig2, unsigned int len2)
79
{
80
unsigned int i, j;
81
82
memset(product, 0, (len1 + len2 - 1) * sizeof(*product));
83
84
for (i = 0; i < len1; i++)
85
for (j = 0; j < len2; j++)
86
product[i + j] += sig1[i] * sig2[j];
87
}
88
89
static
90
void FFT(complex double *X, double *t, unsigned int length){
91
92
unsigned int i,j;
93
94
for(i=0; i<length; i++){
95
X[i]=0;
96
for(j=0; j<length; j++){
97
X[i]=X[i] + t[j]*cexp((-2.0*I*M_PI)*(j)*(i)/length);
98
}
99
}
100
101
}
102
103
static
104
void compute_fir_lowpass(double *fir, unsigned int length, double fc)
105
{
106
unsigned int i;
107
double half_len = (double) ((unsigned int) (length / 2));
108
109
for (i = 0; i < length; i++)
110
if (i != length / 2)
111
fir[i] =
112
sin(2.0 * PIf * (double) fc *
113
((double) i - half_len)) / ((double) i -
114
half_len);
115
else
116
fir[i] = 2.0 * PIf * fc;
117
}
118
119
static
120
void reverse_fir(double *fir, unsigned int length)
121
{
122
unsigned int i;
123
124
// compute delay minus lowpass fir
125
for (i = 0; i < length; i++)
126
fir[i] = -1.0 * fir[i];
127
fir[length - 1] += 1.0;
128
}
129
130
/* Algorithm taken form:
131
132
"Introduction to Digital Filters with Audio Applications",
133
by Julius O. Smith III, (September 2007 Edition).
134
https://ccrma.stanford.edu/~jos/filters/Numerical_Computation_Group_Delay.html
135
*/
136
static
137
double compute_IIR_filter_delay(double *num, double *den,
138
unsigned int length)
139
{
140
unsigned int i,length_c;
141
double *a,*b,*c,*cr;
142
complex double *X,*Y;
143
double Delay = 0.0, d = 0.0;
144
145
length_c=2*length-1;
146
147
a = malloc( (length)*sizeof(*a));
148
b = malloc( (length)*sizeof(*b));
149
c = malloc( (length_c)*sizeof(*c));
150
cr = malloc( (length_c)*sizeof(*cr));
151
X = malloc( (length_c)*sizeof(*X));
152
Y = malloc( (length_c)*sizeof(*Y));
153
if (!a || !b || !c || !cr|| !X || !Y){
154
Delay=0.0;
155
goto exit;
156
}
157
158
for(i=0;i<length;i++){
159
b[i]=den[length-i-1];
160
a[i]=num[i];
161
}
162
compute_convolution(c,b,length,a,length);
163
164
for (i=0;i<length_c;i++)
165
cr[i] = c[i]*i;
166
167
FFT(Y,c,length_c);
168
FFT(X,cr,length_c);
169
170
for (i=0;i<length_c;i++) {
171
d = creal(X[i]/Y[i]);
172
173
if (d > Delay)
174
Delay = d;
175
}
176
177
exit:
178
free(a);
179
free(b);
180
free(c);
181
free(cr);
182
free(X);
183
free(Y);
184
return Delay;
185
}
186
187
188
/******************************
189
* inspired by DSP guide ch33 *
190
******************************/
191
static
192
void get_pole_coefs(double p, double np, double fc, double r, int highpass, double a[3], double b[3])
193
{
194
double rp, ip, es, vx, kx, t, w, m, d, x0, x1, x2, y1, y2, k;
195
196
// calculate pole locate on the unit circle
197
rp = -cos(PId / (np * 2.0) + (p - 1.0) * PId / np);
198
ip = sin(PId / (np * 2.0) + (p - 1.0) * PId / np);
199
200
// Warp from a circle to an ellipse
201
if (r != 0.0) {
202
/* es = sqrt(pow(1.0 / (1.0 - r), 2) - 1.0);
203
vx = (1.0 / np) * log((1.0 / es) +
204
sqrt((1.0 / (es * es)) +
205
1.0));
206
kx = (1.0 / np) * log((1.0 / es) +
207
sqrt((1.0 / (es * es)) -
208
1.0));
209
kx = (exp(kx) + exp(-kx)) / 2.0;
210
rp = rp * ((exp(vx) - exp(-vx)) / 2.0) / kx;
211
ip = ip * ((exp(vx) + exp(-vx)) / 2.0) / kx;
212
*/
213
es = sqrt(pow(1.0 / (1.0 - r), 2) - 1.0);
214
vx = asinh(1.0/es) / np;
215
kx = acosh(1.0/es) / np;
216
kx = cosh( kx );
217
rp *= sinh(vx) / kx;
218
ip *= cosh(vx) / kx;
219
}
220
221
222
// s to z domains conversion
223
t = 2.0*tan(0.5);
224
w = 2.0*PId*fc;
225
m = rp*rp + ip*ip;
226
d = 4.0 - 4.0*rp*t + m*t*t;
227
x0 = t*t/d;
228
x1 = 2.0*t*t/d;
229
x2 = t*t/d;
230
y1 = (8.0 - 2.0*m*t*t)/d;
231
y2 = (-4.0 - 4.0*rp*t - m*t*t)/d;
232
233
// LP(s) to LP(z) or LP(s) to HP(z)
234
if (highpass)
235
k = -cos(w/2.0 + 0.5)/cos(w/2.0 - 0.5);
236
else
237
k = sin(0.5 - w/2.0)/sin(0.5 + w/2.0);
238
d = 1.0 + y1*k - y2*k*k;
239
a[0] = (x0 - x1*k + x2*k*k)/d;
240
a[1] = (-2.0*x0*k + x1 + x1*k*k - 2.0*x2*k)/d;
241
a[2] = (x0*k*k - x1*k + x2)/d;
242
b[1] = (2.0*k + y1 + y1*k*k - 2.0*y2*k)/d;
243
b[2] = (-k*k - y1*k + y2)/d;
244
if (highpass) {
245
a[1] *= -1.0;
246
b[1] *= -1.0;
247
}
248
}
249
250
/******************************
251
* inspired by DSP guide ch33 *
252
******************************/
253
static
254
int compute_cheby_iir(double *num, double *den, unsigned int num_pole,
255
int highpass, double ripple, double cutoff_freq)
256
{
257
double *a, *b, *ta, *tb;
258
double ap[3], bp[3];
259
double sa, sb, gain;
260
unsigned int i, p;
261
int retval = 1;
262
263
// Allocate temporary arrays
264
a = malloc((num_pole + 3) * sizeof(*a));
265
b = malloc((num_pole + 3) * sizeof(*b));
266
ta = malloc((num_pole + 3) * sizeof(*ta));
267
tb = malloc((num_pole + 3) * sizeof(*tb));
268
if (!a || !b || !ta || !tb) {
269
retval = 0;
270
goto exit;
271
}
272
memset(a, 0, (num_pole + 3) * sizeof(*a));
273
memset(b, 0, (num_pole + 3) * sizeof(*b));
274
275
a[2] = 1.0;
276
b[2] = 1.0;
277
278
for (p = 1; p <= num_pole / 2; p++) {
279
// Compute the coefficients for this pole
280
get_pole_coefs(p, num_pole, cutoff_freq, ripple, highpass, ap, bp);
281
282
// Add coefficients to the cascade
283
memcpy(ta, a, (num_pole + 3) * sizeof(*a));
284
memcpy(tb, b, (num_pole + 3) * sizeof(*b));
285
for (i = 2; i <= num_pole + 2; i++) {
286
a[i] = ap[0]*ta[i] + ap[1]*ta[i-1] + ap[2]*ta[i-2];
287
b[i] = tb[i] - bp[1]*tb[i-1] - bp[2]*tb[i-2];
288
}
289
}
290
291
// Finish combining coefficients
292
b[2] = 0.0;
293
for (i = 0; i <= num_pole; i++) {
294
a[i] = a[i + 2];
295
b[i] = -b[i + 2];
296
}
297
298
// Normalize the gain
299
sa = sb = 0.0;
300
for (i = 0; i <= num_pole; i++) {
301
sa += a[i] * ((highpass && i % 2) ? -1.0 : 1.0);
302
sb += b[i] * ((highpass && i % 2) ? -1.0 : 1.0);
303
}
304
gain = sa / (1.0 - sb);
305
for (i = 0; i <= num_pole; i++)
306
a[i] /= gain;
307
308
// Copy the results to the num and den
309
for (i = 0; i <= num_pole; i++) {
310
num[i] = a[i];
311
den[i] = -b[i];
312
}
313
// den[0] must be 1.0
314
den[0] = 1.0;
315
316
exit:
317
free(a);
318
free(b);
319
free(ta);
320
free(tb);
321
return retval;
322
}
323
324
/** compute_bandpass_complex_filter:
325
* \param fl normalized lowest cutoff freq of the bandpass filter. (the normal frequency divided by the sampling freq)
326
* \param fh normalized highest cutoff freq of the bandpass filter. (the normal frequency divided by the sampling freq)
327
* \param num_pole The number of pole the z-transform of the filter should possess
328
329
This function creates a complex bandpass filter from a Chebyshev low pass filter.
330
*/
331
static
332
int compute_bandpass_complex_filter(complex double *num,
333
complex double *den,
334
unsigned int num_pole,
335
double fl, double fh)
336
{
337
double *a=NULL, *b=NULL;
338
complex double *ac,*bc;
339
double ripple,fc,alpha,Delay;
340
unsigned int i,retval=1;
341
342
// Allocate temporary arrays
343
a = malloc( (num_pole+1)*sizeof(*a));
344
b = malloc( (num_pole+1)*sizeof(*b));
345
ac = malloc((num_pole+1)*sizeof(*ac));
346
bc = malloc( (num_pole+1)*sizeof(*bc));
347
if (!a || !b || !ac || !bc){
348
retval=0;
349
goto exit;
350
}
351
352
alpha = M_PI*(fl+fh); // Rotation angle in radians to produces
353
// the desired analitic filter
354
fc = (fh-fl)/2.0; // Normalized cutoff frequency
355
// of the low pass filter
356
ripple = 0.01;
357
358
// prepare the z-transform of low pass filter
359
if (!compute_cheby_iir(b, a, num_pole, 0, ripple, fc)){
360
retval=0;
361
goto exit;
362
}
363
364
// Compute the low pass filter delay; the complex filter
365
Delay=compute_IIR_filter_delay(b, a,num_pole+1);
366
367
/* Note: The complex filter introduces a delay equal to
368
e^(j*alpha*D) (D: Delay low pass filter).To get rid of the
369
undesired frequency independent phase factor, the filter with
370
rotated poles and zeros should be multiplied by
371
the constant e^(-j*alpha*D).*/
372
373
374
// compute complex coefficients (rotating poles and zeros).
375
for(i=0;i<num_pole + 1; i++) {
376
// complex numerator
377
ac[i]= 2.0*cexp(-1.0*I*alpha*Delay)
378
*b[i]*cexp(1.0*I*alpha*(i+1));
379
380
// complex denominator
381
bc[i]= a[i]*cexp(1.0*I*alpha*(i+1));
382
}
383
384
for(i=0;i<num_pole + 1; i++){
385
num[i]=ac[i];
386
den[i]=bc[i];
387
}
388
389
exit:
390
free(a);
391
free(b);
392
free(ac);
393
free(bc);
394
return retval;
395
}
396
397
398
/**************************************************************************
399
* *
400
* Create particular filters *
401
* *
402
**************************************************************************/
403
/**
404
* \param nchann number of channels the filter will process
405
* \param type type of data the filter will process (\c RTF_FLOAT or \c RTF_DOUBLE)
406
* \param fir_length number of sample used to compute the mean
407
* \return the handle of the newly created filter in case of success, \c NULL otherwise.
408
*/
409
API_EXPORTED
410
hfilter rtf_create_fir_mean(unsigned int nchann, int proctype,
411
unsigned int fir_length)
412
{
413
unsigned int i;
414
double* fir= NULL;
415
hfilter filt;
416
417
// Alloc temporary fir
418
fir = malloc(fir_length*sizeof(*fir));
419
if (!fir)
420
return NULL;
421
422
// prepare the finite impulse response
423
for (i = 0; i < fir_length; i++)
424
fir[i] = 1.0f / (double) fir_length;
425
426
filt = rtf_create_filter(nchann, proctype,
427
fir_length, fir, 0, NULL,
428
RTF_DOUBLE);
429
430
free(fir);
431
return filt;
432
}
433
434
/**
435
* \param nchann number of channels the filter will process
436
* \param proctype type of data the filter will process (\c RTF_FLOAT or \c RTF_DOUBLE)
437
* \param fc Normalized cutoff frequency (the normal frequency divided by the sampling frequency)
438
* \param half_length the half size of the impulse response (in number of samples)
439
* \param window The type of the kernel wondow to use for designing the filter
440
* \return the handle of the newly created filter in case of success, \c NULL otherwise.
441
*/
442
API_EXPORTED
443
hfilter rtf_create_fir_lowpass(unsigned int nchann, int proctype,
444
double fc, unsigned int half_length,
445
KernelWindow window)
446
{
447
double *fir = NULL;
448
hfilter filt;
449
unsigned int fir_length = 2 * half_length + 1;
450
451
// Alloc temporary fir
452
fir = malloc(fir_length*sizeof(*fir));
453
if (!fir)
454
return NULL;
455
456
// prepare the finite impulse response
457
compute_fir_lowpass(fir, fir_length, fc);
458
apply_window(fir, fir_length, window);
459
normalize_fir(fir, fir_length);
460
461
filt = rtf_create_filter(nchann, proctype,
462
fir_length, fir, 0, NULL,
463
RTF_DOUBLE);
464
465
free(fir);
466
return filt;
467
}
468
469
470
/**
471
* \param nchann number of channels the filter will process
472
* \param proctype type of data the filter will process (\c RTF_FLOAT or \c RTF_DOUBLE)
473
* \param fc Normalized cutoff frequency (the normal frequency divided by the sampling frequency)
474
* \param half_length the half size of the impulse response (in number of samples)
475
* \param window The type of the kernel wondow to use for designing the filter
476
* \return the handle of the newly created filter in case of success, \c NULL otherwise.
477
*/
478
API_EXPORTED
479
hfilter rtf_create_fir_highpass(unsigned int nchann, int proctype,
480
double fc, unsigned int half_length,
481
KernelWindow window)
482
{
483
double *fir = NULL;
484
hfilter filt;
485
unsigned int fir_length = 2 * half_length + 1;
486
487
// Alloc temporary fir
488
fir = malloc(fir_length*sizeof(*fir));
489
if (!fir)
490
return NULL;
491
492
// prepare the finite impulse response
493
compute_fir_lowpass(fir, fir_length, fc);
494
apply_window(fir, fir_length, window);
495
normalize_fir(fir, fir_length);
496
reverse_fir(fir, fir_length);
497
498
filt = rtf_create_filter(nchann, proctype,
499
fir_length, fir, 0, NULL,
500
RTF_DOUBLE);
501
502
free(fir);
503
return filt;
504
}
505
506
507
/**
508
* \param nchann number of channels the filter will process
509
* \param proctype type of data the filter will process (\c datatype_float or \c datatype_double)
510
* \param fc_low normalized cutoff frequency of the lowpass part (the normal frequency divided by the sampling frequency)
511
* \param fc_high normalized cutoff frequency of the highpass part (the normal frequency divided by the sampling frequency)
512
* \param half_length the half size of the impulse response (in number of samples)
513
* \param window the type of the kernel wondow to use for designing the filter
514
* \return the handle of the newly created filter in case of success, \c null otherwise.
515
*/
516
API_EXPORTED
517
hfilter rtf_create_fir_bandpass(unsigned int nchann, int proctype,
518
double fc_low, double fc_high,
519
unsigned int half_length,
520
KernelWindow window)
521
{
522
unsigned int len = 2 * (half_length / 2) + 1;
523
double fir_low[len], fir_high[len];
524
double *fir = NULL;
525
hfilter filt;
526
unsigned int fir_length = 2 * half_length + 1;
527
528
// Alloc temporary fir
529
fir = malloc(fir_length*sizeof(*fir));
530
if (!fir)
531
return NULL;
532
533
// Create the lowpass finite impulse response
534
compute_fir_lowpass(fir_low, len, fc_low);
535
apply_window(fir_low, len, window);
536
normalize_fir(fir_low, len);
537
538
// Create the highpass finite impulse response
539
compute_fir_lowpass(fir_high, len, fc_high);
540
apply_window(fir_high, len, window);
541
normalize_fir(fir_high, len);
542
reverse_fir(fir_high, len);
543
544
// compute the convolution product of the two FIR
545
compute_convolution(fir, fir_low, len, fir_high, len);
546
547
filt = rtf_create_filter(nchann, proctype,
548
fir_length, fir, 0, NULL,
549
RTF_DOUBLE);
550
551
free(fir);
552
return filt;
553
}
554
555
556
/**
557
* \param nchann number of channels the filter will process
558
* \param proctype type of data the filter will process (\c datatype_float or \c datatype_double)
559
* \param fc normalized cutoff frequency (the normal frequency divided by the sampling frequency)
560
* \param num_pole The number of pole the z-transform of the filter should possess
561
* \param highpass flag to specify the type of filter (0 for a lowpass, 1 for a highpass)
562
* \param ripple ripple
563
* \return the handle of the newly created filter in case of success, \c null otherwise.
564
*/
565
API_EXPORTED
566
hfilter rtf_create_chebychev(unsigned int nchann, int proctype,
567
double fc, unsigned int num_pole,
568
int highpass, double ripple)
569
{
570
double *num = NULL, *den = NULL;
571
hfilter filt = NULL;
572
573
if (num_pole % 2 != 0)
574
return NULL;
575
576
num = malloc( (num_pole+1)*sizeof(*num));
577
den = malloc( (num_pole+1)*sizeof(*den));
578
if (!num || !den)
579
goto out;
580
581
// prepare the z-transform of the filter
582
if (!compute_cheby_iir(num, den, num_pole, highpass, ripple, fc))
583
goto out;
584
585
filt = rtf_create_filter(nchann, proctype,
586
num_pole+1, num, num_pole+1, den,
587
RTF_DOUBLE);
588
589
out:
590
free(num);
591
free(den);
592
return filt;
593
}
594
595
/**
596
* \param nchann number of channels the filter will process
597
* \param proctype type of data the filter will process (\c datatype_float or \c datatype_double)
598
* \param fc normalized cutoff frequency (the normal frequency divided by the sampling frequency)
599
* \param num_pole The number of pole the z-transform of the filter should possess
600
* \param highpass flag to specify the type of filter (0 for a lowpass, 1 for a highpass)
601
* \return the handle of the newly created filter in case of success, \c null otherwise.
602
*/
603
API_EXPORTED
604
hfilter rtf_create_butterworth(unsigned int nchann, int proctype,
605
double fc, unsigned int num_pole,
606
int highpass)
607
{
608
return rtf_create_chebychev(nchann, proctype,
609
fc, num_pole, highpass, 0.0);
610
}
611
612
/**
613
* \param nchann number of channels the filter will process
614
* \param type type of data the filter will process (\c datatype_float or \c datatype_double)
615
* \param fs sampling frequency in Hz
616
* \return the handle of the newly created filter in case of success, \c null otherwise.
617
*/
618
API_EXPORTED
619
hfilter rtf_create_integral(unsigned int nchann, int type, double fs)
620
{
621
hfilter filt;
622
double a = 1.0/fs, b[2] = {1.0, -1.0};
623
624
filt = rtf_create_filter(nchann, type, 1, &a, 2, &b, RTF_DOUBLE);
625
626
return filt;
627
}
628
629
/**
630
rtf_create_bandpass_analytic:
631
632
Analytic filter: Is a complex filter generating a signal whose spectrum equals the positive spectrum from a (real) input signal. The resulting signal is said to be “analytic".
633
The technique relies on the rotation of the pole-zero plot of a low pass filter.
634
635
* \param nchann number of channels the filter will process.
636
* \param proctype type of data the filter will process (\c datatype_float or \c datatype_double).
637
* \param fl normalized low bandpass cutoff frequency (the normal frequency divided by the sampling frequency).
638
* \param fh normalized hihg bandpass cutoff frequency (the normal frequency divided by the sampling frequency).
639
* \param num_pole The number of pole the z-transform of the filter should possess.
640
* \return The handle of the newly created filter in case of success, \c null otherwise.
641
*/
642
API_EXPORTED
643
hfilter rtf_create_bandpass_analytic(unsigned int nchann,
644
int proctype,
645
double fl, double fh,
646
unsigned int num_pole)
647
{
648
complex double *a = NULL, *b = NULL;
649
hfilter filt = NULL;
650
651
if (num_pole % 2 != 0)
652
return NULL;
653
654
a = malloc((num_pole+1)*sizeof(*a));
655
b = malloc((num_pole+1)*sizeof(*b));
656
if (!a || !b)
657
goto out;
658
659
// prepare the z-transform of the complex bandpass filter
660
if (!compute_bandpass_complex_filter(b,a,num_pole,fl,fh))
661
goto out;
662
663
filt = rtf_create_filter(nchann, proctype,
664
num_pole+1, b, num_pole+1, a,
665
RTF_CDOUBLE);
666
out:
667
free(a);
668
free(b);
669
return filt;
670
}
671
672
673
674
675
Loggerhead is a web-based interface for Breezy
Version: 2.0.1