1 /*
2 * Copyright (c) 2007 - 2015 Joseph Gaeddert
3 *
4 * Permission is hereby granted, free of charge, to any person obtaining a copy
5 * of this software and associated documentation files (the "Software"), to deal
6 * in the Software without restriction, including without limitation the rights
7 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8 * copies of the Software, and to permit persons to whom the Software is
9 * furnished to do so, subject to the following conditions:
10 *
11 * The above copyright notice and this permission notice shall be included in
12 * all copies or substantial portions of the Software.
13 *
14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
20 * THE SOFTWARE.
21 */
22
23 //
24 // iir (infinite impulse response) filter design
25 //
26 // References
27 // [Constantinides:1967] A. G. Constantinides, "Frequency
28 // Transformations for Digital Filters." IEEE Electronic
29 // Letters, vol. 3, no. 11, pp 487-489, 1967.
30 //
31
32 #include <stdio.h>
33 #include <stdlib.h>
34 #include <string.h>
35 #include <math.h>
36 #include <assert.h>
37
38 #include "liquid.internal.h"
39
40 #define LIQUID_IIRDES_DEBUG_PRINT 0
41
42 // Sorts array _z of complex numbers into complex conjugate pairs to
43 // within a tolerance. Conjugate pairs are ordered by increasing real
44 // component with the negative imaginary element first. All pure-real
45 // elements are placed at the end of the array.
46 //
47 // Example:
48 // v: liquid_cplxpair(v):
49 // 10 + j*3 -3 - j*4
50 // 5 + j*0 3 + j*4
51 // -3 + j*4 10 - j*3
52 // 10 - j*3 10 + j*3
53 // 3 + j*0 3 + j*0
54 // -3 + j*4 5 + j*0
55 //
56 // _z : complex array (size _n)
57 // _n : number of elements in _z
58 // _tol : tolerance for finding complex pairs
59 // _p : resulting pairs, pure real values of _z at end
liquid_cplxpair(float complex * _z,unsigned int _n,float _tol,float complex * _p)60 void liquid_cplxpair(float complex * _z,
61 unsigned int _n,
62 float _tol,
63 float complex * _p)
64 {
65 // validate input
66 if (_tol < 0) {
67 fprintf(stderr,"error: liquid_cplxpair(), tolerance must be positive\n");
68 exit(1);
69 }
70
71 // keep track of which elements have been paired
72 unsigned char paired[_n];
73 memset(paired,0,sizeof(paired));
74 unsigned int num_pairs=0;
75
76 unsigned int i,j,k=0;
77 for (i=0; i<_n; i++) {
78 // ignore value if already paired, or if imaginary
79 // component is less than tolerance
80 if (paired[i] || fabsf(cimagf(_z[i])) < _tol)
81 continue;
82
83 for (j=0; j<_n; j++) {
84 // ignore value if already paired, or if imaginary
85 // component is less than tolerance
86 if (j==i || paired[j] || fabsf(cimagf(_z[j])) < _tol)
87 continue;
88
89 if ( fabsf(cimagf(_z[i])+cimagf(_z[j])) < _tol &&
90 fabsf(crealf(_z[i])-crealf(_z[j])) < _tol )
91 {
92 // found complex conjugate pair
93 _p[k++] = _z[i];
94 _p[k++] = _z[j];
95 paired[i] = 1;
96 paired[j] = 1;
97 num_pairs++;
98 break;
99 }
100 }
101 }
102 assert(k <= _n);
103
104 // sort through remaining unpaired values and ensure
105 // they are purely real
106 for (i=0; i<_n; i++) {
107 if (paired[i])
108 continue;
109
110 if (cimagf(_z[i]) > _tol) {
111 fprintf(stderr,"warning, liquid_cplxpair(), complex numbers cannot be paired\n");
112 } else {
113 _p[k++] = _z[i];
114 paired[i] = 1;
115 }
116 }
117
118 // clean up result
119 // * force pairs to be perfect conjugates with
120 // negative imaginary component first
121 // * complex conjugate pairs are ordered by
122 // increasing real component
123 // * pure-real elements are ordered by increasing
124 // value
125 liquid_cplxpair_cleanup(_p, _n, num_pairs);
126 }
127
128 // post-process cleanup used with liquid_cplxpair
129 //
130 // once pairs have been identified and ordered, this method
131 // will clean up the result by ensuring the following:
132 // * pairs are perfect conjugates
133 // * pairs have negative imaginary component first
134 // * pairs are ordered by increasing real component
135 // * pure-real elements are ordered by increasing value
136 //
137 // _p : pre-processed complex array [size: _n x 1]
138 // _n : array length
139 // _num_pairs : number of complex conjugate pairs
liquid_cplxpair_cleanup(float complex * _p,unsigned int _n,unsigned int _num_pairs)140 void liquid_cplxpair_cleanup(float complex * _p,
141 unsigned int _n,
142 unsigned int _num_pairs)
143 {
144 unsigned int i;
145 unsigned int j;
146 float complex tmp;
147
148 // ensure perfect conjugates, with negative imaginary
149 // element coming first
150 for (i=0; i<_num_pairs; i++) {
151 // ensure perfect conjugates
152 _p[2*i+0] = cimagf(_p[2*i]) < 0 ? _p[2*i] : conjf(_p[2*i]);
153 _p[2*i+1] = conjf(_p[2*i+0]);
154 }
155
156 // sort conjugate pairs
157 for (i=0; i<_num_pairs; i++) {
158 for (j=_num_pairs-1; j>i; j--) {
159 if ( crealf(_p[2*(j-1)]) > crealf(_p[2*j]) ) {
160 // swap pairs
161 tmp = _p[2*(j-1)+0];
162 _p[2*(j-1)+0] = _p[2*j+0];
163 _p[2*j +0] = tmp;
164
165 tmp = _p[2*(j-1)+1];
166 _p[2*(j-1)+1] = _p[2*j+1];
167 _p[2*j +1] = tmp;
168 }
169 }
170 }
171
172 // sort pure-real values
173 for (i=2*_num_pairs; i<_n; i++) {
174 for (j=_n-1; j>i; j--) {
175 if ( crealf(_p[j-1]) > crealf(_p[j]) ) {
176 // swap elements
177 tmp = _p[j-1];
178 _p[j-1] = _p[j];
179 _p[j ] = tmp;
180 }
181 }
182 }
183
184 }
185
186
187 //
188 // new IIR design
189 //
190
191 // Compute frequency pre-warping factor. See [Constantinides:1967]
192 // _btype : band type (e.g. LIQUID_IIRDES_HIGHPASS)
193 // _fc : low-pass cutoff frequency
194 // _f0 : center frequency (band-pass|stop cases only)
iirdes_freqprewarp(liquid_iirdes_bandtype _btype,float _fc,float _f0)195 float iirdes_freqprewarp(liquid_iirdes_bandtype _btype,
196 float _fc,
197 float _f0)
198 {
199 float m = 0.0f;
200 if (_btype == LIQUID_IIRDES_LOWPASS) {
201 // low pass
202 m = tanf(M_PI * _fc);
203 } else if (_btype == LIQUID_IIRDES_HIGHPASS) {
204 // high pass
205 m = -cosf(M_PI * _fc) / sinf(M_PI * _fc);
206 } else if (_btype == LIQUID_IIRDES_BANDPASS) {
207 // band pass
208 m = (cosf(2*M_PI*_fc) - cosf(2*M_PI*_f0) )/ sinf(2*M_PI*_fc);
209 } else if (_btype == LIQUID_IIRDES_BANDSTOP) {
210 // band stop
211 m = sinf(2*M_PI*_fc)/(cosf(2*M_PI*_fc) - cosf(2*M_PI*_f0));
212 }
213 m = fabsf(m);
214
215 return m;
216 }
217
218 // convert analog zeros, poles, gain to digital zeros, poles gain
219 // _za : analog zeros (length: _nza)
220 // _nza : number of analog zeros
221 // _pa : analog poles (length: _npa)
222 // _npa : number of analog poles
223 // _ka : nominal gain (NOTE: this does not necessarily carry over from analog gain)
224 // _m : frequency pre-warping factor
225 // _zd : digital zeros (length: _npa)
226 // _pd : digital poles (length: _npa)
227 // _kd : digital gain
228 //
229 // The filter order is characterized by the number of analog
230 // poles. The analog filter may have up to _npa zeros.
231 // The number of digital zeros and poles is equal to _npa.
bilinear_zpkf(float complex * _za,unsigned int _nza,float complex * _pa,unsigned int _npa,float complex _ka,float _m,float complex * _zd,float complex * _pd,float complex * _kd)232 void bilinear_zpkf(float complex * _za,
233 unsigned int _nza,
234 float complex * _pa,
235 unsigned int _npa,
236 float complex _ka,
237 float _m,
238 float complex * _zd,
239 float complex * _pd,
240 float complex * _kd)
241 {
242 unsigned int i;
243
244 // filter order is equal to number of analog poles
245 unsigned int n = _npa;
246 float complex G = _ka; // nominal gain
247 for (i=0; i<n; i++) {
248 // compute digital zeros (pad with -1s)
249 if (i < _nza) {
250 float complex zm = _za[i] * _m;
251 _zd[i] = (1.0 + zm)/(1.0 - zm);
252 } else {
253 _zd[i] = -1.0;
254 }
255
256 // compute digital poles
257 float complex pm = _pa[i] * _m;
258 _pd[i] = (1.0 + pm)/(1.0 - pm);
259
260 // compute digital gain
261 G *= (1.0 - _pd[i])/(1.0 - _zd[i]);
262 }
263 *_kd = G;
264
265 #if LIQUID_IIRDES_DEBUG_PRINT
266 // print poles and zeros
267 printf("zpk_a2df() poles (discrete):\n");
268 for (i=0; i<n; i++)
269 printf(" pd[%3u] = %12.8f + j*%12.8f\n", i, crealf(_pd[i]), cimagf(_pd[i]));
270 printf("zpk_a2df() zeros (discrete):\n");
271 for (i=0; i<n; i++)
272 printf(" zd[%3u] = %12.8f + j*%12.8f\n", i, crealf(_zd[i]), cimagf(_zd[i]));
273 printf("zpk_a2df() gain (discrete):\n");
274 printf(" kd = %12.8f + j*%12.8f\n", crealf(G), cimagf(G));
275 #endif
276 }
277
278 // convert discrete z/p/k form to transfer function form
279 // _zd : digital zeros (length: _n)
280 // _pd : digital poles (length: _n)
281 // _n : filter order
282 // _k : digital gain
283 // _b : output numerator (length: _n+1)
284 // _a : output denominator (length: _n+1)
iirdes_dzpk2tff(float complex * _zd,float complex * _pd,unsigned int _n,float complex _k,float * _b,float * _a)285 void iirdes_dzpk2tff(float complex * _zd,
286 float complex * _pd,
287 unsigned int _n,
288 float complex _k,
289 float * _b,
290 float * _a)
291 {
292 unsigned int i;
293 float complex q[_n+1];
294
295 // expand poles
296 polycf_expandroots(_pd,_n,q);
297 for (i=0; i<=_n; i++)
298 _a[i] = crealf(q[_n-i]);
299
300 // expand zeros
301 polycf_expandroots(_zd, _n, q);
302 for (i=0; i<=_n; i++)
303 _b[i] = crealf(q[_n-i]*_k);
304 }
305
306 // converts discrete-time zero/pole/gain (zpk) recursive (iir)
307 // filter representation to second-order sections (sos) form
308 //
309 // _zd : discrete zeros array (size _n)
310 // _pd : discrete poles array (size _n)
311 // _n : number of poles, zeros
312 // _kd : gain
313 //
314 // _B : output numerator matrix (size (L+r) x 3)
315 // _A : output denominator matrix (size (L+r) x 3)
316 //
317 // L is the number of sections in the cascade:
318 // r = _n % 2
319 // L = (_n - r) / 2;
iirdes_dzpk2sosf(float complex * _zd,float complex * _pd,unsigned int _n,float complex _kd,float * _B,float * _A)320 void iirdes_dzpk2sosf(float complex * _zd,
321 float complex * _pd,
322 unsigned int _n,
323 float complex _kd,
324 float * _B,
325 float * _A)
326 {
327 int i;
328 float tol=1e-6f; // tolerance for conjuate pair computation
329
330 // find/group complex conjugate pairs (poles)
331 float complex zp[_n];
332 liquid_cplxpair(_zd,_n,tol,zp);
333
334 // find/group complex conjugate pairs (zeros)
335 float complex pp[_n];
336 liquid_cplxpair(_pd,_n,tol,pp);
337
338 // TODO : group pole pairs with zero pairs
339
340 // _n = 2*L + r
341 unsigned int r = _n % 2; // odd/even order
342 unsigned int L = (_n - r)/2; // filter semi-length
343
344 #if LIQUID_IIRDES_DEBUG_PRINT
345 printf(" n=%u, r=%u, L=%u\n", _n, r, L);
346 printf("poles :\n");
347 for (i=0; i<_n; i++)
348 printf(" p[%3u] = %12.8f + j*%12.8f\n", i, crealf(_pd[i]), cimagf(_pd[i]));
349 printf("zeros :\n");
350 for (i=0; i<_n; i++)
351 printf(" z[%3u] = %12.8f + j*%12.8f\n", i, crealf(_zd[i]), cimagf(_zd[i]));
352
353 printf("poles (conjugate pairs):\n");
354 for (i=0; i<_n; i++)
355 printf(" p[%3u] = %12.8f + j*%12.8f\n", i, crealf(pp[i]), cimagf(pp[i]));
356 printf("zeros (conjugate pairs):\n");
357 for (i=0; i<_n; i++)
358 printf(" z[%3u] = %12.8f + j*%12.8f\n", i, crealf(zp[i]), cimagf(zp[i]));
359 #endif
360
361 float complex z0, z1;
362 float complex p0, p1;
363 for (i=0; i<L; i++) {
364 p0 = -pp[2*i+0];
365 p1 = -pp[2*i+1];
366
367 z0 = -zp[2*i+0];
368 z1 = -zp[2*i+1];
369
370 // expand complex pole pairs
371 _A[3*i+0] = 1.0;
372 _A[3*i+1] = crealf(p0+p1);
373 _A[3*i+2] = crealf(p0*p1);
374
375 // expand complex zero pairs
376 _B[3*i+0] = 1.0;
377 _B[3*i+1] = crealf(z0+z1);
378 _B[3*i+2] = crealf(z0*z1);
379 }
380
381 // add remaining zero/pole pair if order is odd
382 if (r) {
383 // keep these two lines for when poles and zeros get grouped
384 p0 = -pp[_n-1];
385 z0 = -zp[_n-1];
386
387 _A[3*i+0] = 1.0;
388 _A[3*i+1] = p0;
389 _A[3*i+2] = 0.0;
390
391 _B[3*i+0] = 1.0;
392 _B[3*i+1] = z0;
393 _B[3*i+2] = 0.0;
394 }
395
396 // distribute gain equally amongst all feed-forward
397 // coefficients
398 float k = powf( crealf(_kd), 1.0f/(float)(L+r) );
399
400 // adjust gain of first element
401 for (i=0; i<L+r; i++) {
402 _B[3*i+0] *= k;
403 _B[3*i+1] *= k;
404 _B[3*i+2] *= k;
405 }
406 }
407
408 // digital z/p/k low-pass to high-pass transformation
409 // _zd : digital zeros (low-pass prototype)
410 // _pd : digital poles (low-pass prototype)
411 // _n : low-pass filter order
412 // _zdt : digital zeros transformed [length: _n]
413 // _pdt : digital poles transformed [length: _n]
iirdes_dzpk_lp2hp(liquid_float_complex * _zd,liquid_float_complex * _pd,unsigned int _n,liquid_float_complex * _zdt,liquid_float_complex * _pdt)414 void iirdes_dzpk_lp2hp(liquid_float_complex * _zd,
415 liquid_float_complex * _pd,
416 unsigned int _n,
417 liquid_float_complex * _zdt,
418 liquid_float_complex * _pdt)
419 {
420 unsigned int i;
421 for (i=0; i<_n; i++) {
422 _zdt[i] = -_zd[i];
423 _pdt[i] = -_pd[i];
424 }
425 }
426
427
428 // digital z/p/k low-pass to band-pass transformation
429 // _zd : digital zeros (low-pass prototype)
430 // _pd : digital poles (low-pass prototype)
431 // _n : low-pass filter order
432 // _f0 : center frequency
433 // _zdt : digital zeros transformed [length: 2*_n]
434 // _pdt : digital poles transformed [length: 2*_n]
iirdes_dzpk_lp2bp(liquid_float_complex * _zd,liquid_float_complex * _pd,unsigned int _n,float _f0,liquid_float_complex * _zdt,liquid_float_complex * _pdt)435 void iirdes_dzpk_lp2bp(liquid_float_complex * _zd,
436 liquid_float_complex * _pd,
437 unsigned int _n,
438 float _f0,
439 liquid_float_complex * _zdt,
440 liquid_float_complex * _pdt)
441 {
442 //
443 float c0 = cosf(2*M_PI*_f0);
444
445 // transform zeros, poles using quadratic formula
446 unsigned int i;
447 float complex t0;
448 for (i=0; i<_n; i++) {
449 t0 = 1 + _zd[i];
450 _zdt[2*i+0] = 0.5f*(c0*t0 + csqrtf(c0*c0*t0*t0 - 4*_zd[i]));
451 _zdt[2*i+1] = 0.5f*(c0*t0 - csqrtf(c0*c0*t0*t0 - 4*_zd[i]));
452
453 t0 = 1 + _pd[i];
454 _pdt[2*i+0] = 0.5f*(c0*t0 + csqrtf(c0*c0*t0*t0 - 4*_pd[i]));
455 _pdt[2*i+1] = 0.5f*(c0*t0 - csqrtf(c0*c0*t0*t0 - 4*_pd[i]));
456 }
457 }
458
459 // IIR filter design template
460 // _ftype : filter type (e.g. LIQUID_IIRDES_BUTTER)
461 // _btype : band type (e.g. LIQUID_IIRDES_BANDPASS)
462 // _format : coefficients format (e.g. LIQUID_IIRDES_SOS)
463 // _n : filter order
464 // _fc : low-pass prototype cut-off frequency
465 // _f0 : center frequency (band-pass, band-stop)
466 // _Ap : pass-band ripple in dB
467 // _As : stop-band ripple in dB
468 // _B : numerator
469 // _A : denominator
liquid_iirdes(liquid_iirdes_filtertype _ftype,liquid_iirdes_bandtype _btype,liquid_iirdes_format _format,unsigned int _n,float _fc,float _f0,float _Ap,float _As,float * _B,float * _A)470 void liquid_iirdes(liquid_iirdes_filtertype _ftype,
471 liquid_iirdes_bandtype _btype,
472 liquid_iirdes_format _format,
473 unsigned int _n,
474 float _fc,
475 float _f0,
476 float _Ap,
477 float _As,
478 float * _B,
479 float * _A)
480 {
481 // validate input
482 if (_fc <= 0 || _fc >= 0.5) {
483 fprintf(stderr,"error: liquid_iirdes(), cutoff frequency out of range\n");
484 exit(1);
485 } else if (_f0 < 0 || _f0 > 0.5) {
486 fprintf(stderr,"error: liquid_iirdes(), center frequency out of range\n");
487 exit(1);
488 } else if (_Ap <= 0) {
489 fprintf(stderr,"error: liquid_iirdes(), pass-band ripple out of range\n");
490 exit(1);
491 } else if (_As <= 0) {
492 fprintf(stderr,"error: liquid_iirdes(), stop-band ripple out of range\n");
493 exit(1);
494 } else if (_n == 0) {
495 fprintf(stderr,"error: liquid_iirdes(), filter order must be > 0\n");
496 exit(1);
497 }
498
499 // number of analaog poles/zeros
500 unsigned int npa = _n;
501 unsigned int nza;
502
503 // analog poles/zeros/gain
504 float complex pa[_n];
505 float complex za[_n];
506 float complex ka;
507 float complex k0 = 1.0f; // nominal digital gain
508
509 // derived values
510 unsigned int r = _n%2; // odd/even filter order
511 unsigned int L = (_n-r)/2; // filter semi-length
512
513 // specific filter variables
514 float epsilon, Gp, Gs, ep, es;
515
516 // compute zeros and poles of analog prototype
517 switch (_ftype) {
518 case LIQUID_IIRDES_BUTTER:
519 // Butterworth filter design : no zeros, _n poles
520 nza = 0;
521 k0 = 1.0f;
522 butter_azpkf(_n,za,pa,&ka);
523 break;
524 case LIQUID_IIRDES_CHEBY1:
525 // Cheby-I filter design : no zeros, _n poles, pass-band ripple
526 nza = 0;
527 epsilon = sqrtf( powf(10.0f, _Ap / 10.0f) - 1.0f );
528 k0 = r ? 1.0f : 1.0f / sqrt(1.0f + epsilon*epsilon);
529 cheby1_azpkf(_n,epsilon,za,pa,&ka);
530 break;
531 case LIQUID_IIRDES_CHEBY2:
532 // Cheby-II filter design : _n-r zeros, _n poles, stop-band ripple
533 nza = 2*L;
534 epsilon = powf(10.0f, -_As/20.0f);
535 k0 = 1.0f;
536 cheby2_azpkf(_n,epsilon,za,pa,&ka);
537 break;
538 case LIQUID_IIRDES_ELLIP:
539 // elliptic filter design : _n-r zeros, _n poles, pass/stop-band ripple
540 nza = 2*L;
541 Gp = powf(10.0f, -_Ap / 20.0f); // pass-band gain
542 Gs = powf(10.0f, -_As / 20.0f); // stop-band gain
543 ep = sqrtf(1.0f/(Gp*Gp) - 1.0f); // pass-band epsilon
544 es = sqrtf(1.0f/(Gs*Gs) - 1.0f); // stop-band epsilon
545 k0 = r ? 1.0f : 1.0f / sqrt(1.0f + ep*ep);
546 ellip_azpkf(_n,ep,es,za,pa,&ka);
547 break;
548 case LIQUID_IIRDES_BESSEL:
549 // Bessel filter design : no zeros, _n poles
550 nza = 0;
551 k0 = 1.0f;
552 bessel_azpkf(_n,za,pa,&ka);
553 break;
554 default:
555 fprintf(stderr,"error: liquid_iirdes(), unknown filter type\n");
556 exit(1);
557 }
558
559 #if LIQUID_IIRDES_DEBUG_PRINT
560 unsigned int i;
561
562 printf("poles (analog):\n");
563 for (i=0; i<npa; i++)
564 printf(" pa[%3u] = %12.8f + j*%12.8f\n", i, crealf(pa[i]), cimagf(pa[i]));
565 printf("zeros (analog):\n");
566 for (i=0; i<nza; i++)
567 printf(" za[%3u] = %12.8f + j*%12.8f\n", i, crealf(za[i]), cimagf(za[i]));
568 printf("gain (analog):\n");
569 printf(" ka : %12.8f + j*%12.8f\n", crealf(ka), cimagf(ka));
570 #endif
571
572 // complex digital poles/zeros/gain
573 // NOTE: allocated double the filter order to cover band-pass, band-stop cases
574 float complex zd[2*_n];
575 float complex pd[2*_n];
576 float complex kd;
577 float m = iirdes_freqprewarp(_btype,_fc,_f0);
578 //printf("m : %12.8f\n", m);
579 bilinear_zpkf(za, nza,
580 pa, npa,
581 k0, m,
582 zd, pd, &kd);
583
584 #if LIQUID_IIRDES_DEBUG_PRINT
585 printf("zeros (digital, low-pass prototype):\n");
586 for (i=0; i<_n; i++)
587 printf(" zd[%3u] = %12.4e + j*%12.4e;\n", i, crealf(zd[i]), cimagf(zd[i]));
588 printf("poles (digital, low-pass prototype):\n");
589 for (i=0; i<_n; i++)
590 printf(" pd[%3u] = %12.4e + j*%12.4e;\n", i, crealf(pd[i]), cimagf(pd[i]));
591 printf("gain (digital):\n");
592 printf(" kd : %12.8f + j*%12.8f\n", crealf(kd), cimagf(kd));
593 #endif
594
595 // negate zeros, poles for high-pass and band-stop cases
596 if (_btype == LIQUID_IIRDES_HIGHPASS ||
597 _btype == LIQUID_IIRDES_BANDSTOP)
598 {
599 // run transform, place resulting zeros, poles
600 // back in same original arrays
601 iirdes_dzpk_lp2hp(zd, pd, _n, zd, pd);
602 }
603
604 // transform zeros, poles in band-pass, band-stop cases
605 // NOTE: this also doubles the filter order
606 if (_btype == LIQUID_IIRDES_BANDPASS ||
607 _btype == LIQUID_IIRDES_BANDSTOP)
608 {
609 // allocate memory for transformed zeros, poles
610 float complex zd1[2*_n];
611 float complex pd1[2*_n];
612
613 // run zeros, poles low-pass -> band-pass trasform
614 iirdes_dzpk_lp2bp(zd, pd, // low-pass prototype zeros, poles
615 _n, // filter order
616 _f0, // center frequency
617 zd1,pd1); // transformed zeros, poles (length: 2*n)
618
619 // copy transformed zeros, poles
620 memmove(zd, zd1, 2*_n*sizeof(float complex));
621 memmove(pd, pd1, 2*_n*sizeof(float complex));
622
623 // update paramters; filter order doubles which changes the
624 // number of second-order sections and forces there to never
625 // be any remainder (r=0 always).
626 _n = 2*_n; // _n is now even
627 #if LIQUID_IIRDES_DEBUG_PRINT
628 r = _n % 2; // r is now zero
629 L = (_n-r)/2; // L is now the original value of _n
630 #endif
631 }
632
633 if (_format == LIQUID_IIRDES_TF) {
634 // convert complex digital poles/zeros/gain into transfer
635 // function : H(z) = B(z) / A(z)
636 // where length(B,A) = low/high-pass ? _n + 1 : 2*_n + 1
637 iirdes_dzpk2tff(zd,pd,_n,kd,_B,_A);
638
639 #if LIQUID_IIRDES_DEBUG_PRINT
640 // print coefficients
641 for (i=0; i<=_n; i++) printf("b[%3u] = %12.8f;\n", i, _B[i]);
642 for (i=0; i<=_n; i++) printf("a[%3u] = %12.8f;\n", i, _A[i]);
643 #endif
644 } else {
645 // convert complex digital poles/zeros/gain into second-
646 // order sections form :
647 // H(z) = prod { (b0 + b1*z^-1 + b2*z^-2) / (a0 + a1*z^-1 + a2*z^-2) }
648 // where size(B,A) = low|high-pass : [3]x[L+r]
649 // band-pass|stop : [3]x[2*L]
650 iirdes_dzpk2sosf(zd,pd,_n,kd,_B,_A);
651
652 #if LIQUID_IIRDES_DEBUG_PRINT
653 // print coefficients
654 printf("B [%u x 3] :\n", L+r);
655 for (i=0; i<L+r; i++)
656 printf(" %12.8f %12.8f %12.8f\n", _B[3*i+0], _B[3*i+1], _B[3*i+2]);
657 printf("A [%u x 3] :\n", L+r);
658 for (i=0; i<L+r; i++)
659 printf(" %12.8f %12.8f %12.8f\n", _A[3*i+0], _A[3*i+1], _A[3*i+2]);
660 #endif
661
662 }
663 }
664
665 // checks stability of iir filter
666 // _b : feed-forward coefficients [size: _n x 1]
667 // _a : feed-back coefficients [size: _n x 1]
668 // _n : number of coefficients
iirdes_isstable(float * _b,float * _a,unsigned int _n)669 int iirdes_isstable(float * _b,
670 float * _a,
671 unsigned int _n)
672 {
673 // validate input
674 if (_n < 2) {
675 fprintf(stderr,"error: iirdes_isstable(), filter order too low\n");
676 exit(1);
677 }
678 unsigned int i;
679
680 // flip denominator, left to right
681 float a_hat[_n];
682 for (i=0; i<_n; i++)
683 a_hat[i] = _a[_n-i-1];
684
685 // compute poles (roots of denominator)
686 float complex roots[_n-1];
687 polyf_findroots_bairstow(a_hat, _n, roots);
688
689 #if 0
690 // print roots
691 printf("\nroots:\n");
692 for (i=0; i<_n-1; i++)
693 printf(" r[%3u] = %12.8f + j *%12.8f\n", i, crealf(roots[i]), cimagf(roots[i]));
694 #endif
695
696 // compute magnitude of poles
697 for (i=0; i<_n-1; i++) {
698 if (cabsf(roots[i]) > 1.0)
699 return 0;
700 }
701
702 return 1;
703 }
704
705
706