libdspl-2.0
Библиотека алгоритмов цифровой обработки сигналов
filter_ap.c
1 /*
2 * Copyright (c) 2015-2019 Sergey Bakhurin
3 * Digital Signal Processing Library [http://dsplib.org]
4 *
5 * This file is part of libdspl-2.0.
6 *
7 * is free software: you can redistribute it and/or modify
8 * it under the terms of the GNU Lesser General Public License as published by
9 * the Free Software Foundation, either version 3 of the License, or
10 * (at your option) any later version.
11 *
12 * DSPL is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU Lesser General Public License
18 * along with Foobar. If not, see <http://www.gnu.org/licenses/>.
19 */
20 
21 #include <stdio.h>
22 #include <stdlib.h>
23 #include <string.h>
24 #include "dspl.h"
25 
26 
27 
28 
29 
30 /******************************************************************************
31 Analog Normalized Butterworth filter
32 *******************************************************************************/
33 int DSPL_API butter_ap(double rp, int ord, double* b, double* a)
34 {
35  int res;
36  complex_t *z = NULL;
37  complex_t *p = NULL;
38  int nz, np;
39 
40  if(rp < 0.0)
41  return ERROR_FILTER_RP;
42  if(ord < 1)
43  return ERROR_FILTER_ORD;
44  if(!a || !b)
45  return ERROR_PTR;
46 
47  z = (complex_t*) malloc(ord*sizeof(complex_t));
48  p = (complex_t*) malloc(ord*sizeof(complex_t));
49 
50 
51  res = butter_ap_zp(ord, rp, z, &nz, p, &np);
52  if(res != RES_OK)
53  goto exit_label;
54 
55  res = filter_zp2ab(z, nz, p, np, ord, b, a);
56  if(res != RES_OK)
57  goto exit_label;
58 
59  b[0] = a[0];
60 
61 
62 exit_label:
63  if(z)
64  free(z);
65  if(p)
66  free(p);
67  return res;
68 }
69 
70 
71 
72 
73 
74 
75 
76 
77 /******************************************************************************
78 Analog Normalized Butterworth filter zeros and poles
79 *******************************************************************************/
80 int DSPL_API butter_ap_zp(int ord, double rp, complex_t *z, int* nz,
81  complex_t *p, int* np)
82 {
83  double alpha;
84  double theta;
85  double ep;
86  int r;
87  int L;
88  int ind = 0, k;
89 
90  if(rp < 0 || rp == 0)
91  return ERROR_FILTER_RP;
92  if(ord < 1)
93  return ERROR_FILTER_ORD;
94  if(!z || !p || !nz || !np)
95  return ERROR_PTR;
96 
97  ep = sqrt(pow(10.0, rp*0.1) - 1.0);
98  r = ord % 2;
99  L = (int)((ord-r)/2);
100 
101  alpha = pow(ep, -1.0/(double)ord);
102  if(r)
103  {
104  RE(p[ind]) = -alpha;
105  IM(p[ind]) = 0.0;
106  ind++;
107  }
108  for(k = 0; k < L; k++)
109  {
110  theta = M_PI*(double)(2*k + 1)/(double)(2*ord);
111  RE(p[ind]) = RE(p[ind+1]) = -alpha * sin(theta);
112  IM(p[ind]) = alpha * cos(theta);
113  IM(p[ind+1]) = -alpha * cos(theta);
114  ind+=2;
115  }
116  *np = ord;
117  *nz = 0;
118  return RES_OK;
119 }
120 
121 
122 
123 
124 
125 
126 /******************************************************************************
127 Analog Normalized Chebyshev type 1 filter
128 *******************************************************************************/
129 int DSPL_API cheby1_ap(double rp, int ord, double* b, double* a)
130 {
131  int res;
132  complex_t *z = NULL;
133  complex_t *p = NULL;
134  int nz, np, k;
135  complex_t h0 = {1.0, 0.0};
136  double tmp;
137 
138 
139  if(rp < 0.0)
140  return ERROR_FILTER_RP;
141  if(ord < 1)
142  return ERROR_FILTER_ORD;
143  if(!a || !b)
144  return ERROR_PTR;
145 
146  z = (complex_t*) malloc(ord*sizeof(complex_t));
147  p = (complex_t*) malloc(ord*sizeof(complex_t));
148 
149 
150  res = cheby1_ap_zp(ord, rp, z, &nz, p, &np);
151  if(res != RES_OK)
152  goto exit_label;
153 
154  res = filter_zp2ab(z, nz, p, np, ord, b, a);
155  if(res != RES_OK)
156  goto exit_label;
157 
158 
159  if(!(ord % 2))
160  RE(h0) = 1.0 / pow(10.0, rp*0.05);
161 
162  for(k = 0; k < np; k++)
163  {
164  tmp = CMRE(h0, p[k]);
165  IM(h0) = CMIM(h0, p[k]);
166  RE(h0) = tmp;
167  }
168 
169  b[0] = fabs(RE(h0));
170 
171 exit_label:
172  if(z)
173  free(z);
174  if(p)
175  free(p);
176  return res;
177 }
178 
179 
180 
181 
182 
183 /******************************************************************************
184 Analog Normalized Chebyshev type 1 filter zeros and poles
185 *******************************************************************************/
186 int DSPL_API cheby1_ap_zp(int ord, double rp, complex_t *z, int* nz,
187  complex_t *p, int* np)
188 {
189  double theta;
190  double ep;
191  double beta;
192  double shbeta;
193  double chbeta;
194  int r;
195  int L;
196  int ind = 0, k;
197 
198  if(rp < 0 || rp == 0)
199  return ERROR_FILTER_RP;
200  if(ord < 1)
201  return ERROR_FILTER_ORD;
202  if(!z || !p || !nz || !np)
203  return ERROR_PTR;
204 
205  ep = sqrt(pow(10.0, rp*0.1) - 1.0);
206  r = ord % 2;
207  L = (int)((ord-r)/2);
208 
209 
210  beta = asinh(1.0/ep)/(double)ord;
211  chbeta = cosh(beta);
212  shbeta = sinh(beta);
213 
214  if(r)
215  {
216  RE(p[ind]) = -shbeta;
217  IM(p[ind]) = 0.0;
218  ind++;
219  }
220  for(k = 0; k < L; k++)
221  {
222  theta = M_PI*(double)(2*k + 1)/(double)(2*ord);
223  RE(p[ind]) = RE(p[ind+1]) = -shbeta * sin(theta);
224  IM(p[ind]) = chbeta * cos(theta);
225  IM(p[ind+1]) = -IM(p[ind]);
226  ind+=2;
227  }
228  *np = ord;
229  *nz = 0;
230  return RES_OK;
231 }
232 
233 
234 
235 
236 /******************************************************************************
237  * Analog Normalized Chebyshev type 2 filter
238  ******************************************************************************/
239 int DSPL_API cheby2_ap(double rs, int ord, double* b, double* a)
240 {
241  int res;
242  complex_t *z = NULL;
243  complex_t *p = NULL;
244  int nz, np;
245  double norm;
246 
247 
248  if(rs < 0.0)
249  return ERROR_FILTER_RP;
250  if(ord < 1)
251  return ERROR_FILTER_ORD;
252  if(!a || !b)
253  return ERROR_PTR;
254 
255  z = (complex_t*) malloc(ord*sizeof(complex_t));
256  p = (complex_t*) malloc(ord*sizeof(complex_t));
257 
258 
259  res = cheby2_ap_zp(ord, rs, z, &nz, p, &np);
260  if(res != RES_OK)
261  goto exit_label;
262 
263  res = filter_zp2ab(z, nz, p, np, ord, b, a);
264  if(res != RES_OK)
265  goto exit_label;
266 
267  norm = a[0] / b[0];
268 
269  for(nz = 0; nz < ord+1; nz++)
270  b[nz]*=norm;
271 
272 exit_label:
273  if(z)
274  free(z);
275  if(p)
276  free(p);
277  return res;
278 }
279 
280 
281 
282 
283 /******************************************************************************
284  * Analog Normalized Chebyshev type 2 filter with wp = 1 rad/s
285  ******************************************************************************/
286 int DSPL_API cheby2_ap_wp1(double rp, double rs, int ord, double* b, double* a)
287 {
288  int err;
289  double es, gp, alpha, beta, y, wp;
290 
291  if(rp <= 0)
292  return ERROR_FILTER_RP;
293 
294  err = cheby2_ap(rs, ord, b, a);
295  if(err!=RES_OK)
296  goto exit_label;
297 
298  es = sqrt(pow(10.0, rs*0.1) - 1.0);
299  gp = pow(10.0, -rp*0.05);
300  alpha = gp * es / sqrt(1.0 - gp*gp);
301  beta = alpha + sqrt(alpha * alpha - 1.0);
302  y = log(beta)/ (double)ord;
303  wp = 2.0 / (exp(y) + exp(-y));
304 
305  err = low2low(b, a, ord, wp, 1.0, b, a);
306 
307 exit_label:
308  return err;
309 }
310 
311 
312 
313 
314 /******************************************************************************
315 Analog Normalized Chebyshev type 2 filter zeros and poles
316 *******************************************************************************/
317 int DSPL_API cheby2_ap_zp(int ord, double rs, complex_t *z, int* nz,
318  complex_t *p, int* np)
319 {
320  double es;
321  int L, r, k;
322  double beta;
323  int iz, ip;
324 
325  double alpha;
326  double chb, shb, sa, ca;
327  double ssh2, cch2;
328 
329  if(rs < 0 || rs == 0)
330  return ERROR_FILTER_RS;
331  if(ord < 1)
332  return ERROR_FILTER_ORD;
333  if(!z || !p || !nz || !np)
334  return ERROR_PTR;
335 
336  es = sqrt(pow(10.0, rs*0.1) - 1.0);
337  r = ord % 2;
338  L = (int)((ord-r)/2);
339 
340  beta = asinh(es)/(double)ord;
341 
342  chb = cosh(beta);
343  shb = sinh(beta);
344 
345  iz = ip = 0;
346 
347  if(r)
348  {
349  RE(p[0]) = -1.0 / sinh(beta);
350  IM(p[0]) = 0.0;
351  ip = 1;
352  }
353 
354  for(k = 0; k < L; k++)
355  {
356  alpha = M_PI*(double)(2*k + 1)/(double)(2*ord);
357  sa = sin(alpha);
358  ca = cos(alpha);
359  ssh2 = sa*shb;
360  ssh2 *= ssh2;
361 
362  cch2 = ca*chb;
363  cch2 *= cch2;
364 
365  RE(z[iz]) = RE(z[iz+1]) = 0.0;
366  IM(z[iz]) = 1.0 / ca;
367  IM(z[iz+1]) = -IM(z[iz]);
368  iz+=2;
369 
370  RE(p[ip]) = RE(p[ip+1]) = -sa*shb / (ssh2 + cch2);
371  IM(p[ip]) = ca*chb / (ssh2 + cch2);
372  IM(p[ip+1]) = -IM(p[ip]);
373  ip+=2;
374  }
375  *nz = iz;
376  *np = ip;
377 
378  return RES_OK;
379 }
380 
381 
382 
383 
384 
385 
386 
387 /******************************************************************************
388  * Analog Normalized Elliptic filter
389  *******************************************************************************/
390 int DSPL_API ellip_ap(double rp, double rs, int ord, double* b, double* a)
391 {
392  int res;
393  complex_t *z = NULL;
394  complex_t *p = NULL;
395  int nz, np;
396  double norm, g0;
397 
398 
399  if(rp < 0.0)
400  return ERROR_FILTER_RP;
401  if(rs < 0.0)
402  return ERROR_FILTER_RS;
403  if(ord < 1)
404  return ERROR_FILTER_ORD;
405  if(!a || !b)
406  return ERROR_PTR;
407 
408  z = (complex_t*) malloc(ord*sizeof(complex_t));
409  p = (complex_t*) malloc(ord*sizeof(complex_t));
410 
411 
412  res = ellip_ap_zp(ord, rp, rs, z, &nz, p, &np);
413  if(res != RES_OK)
414  goto exit_label;
415 
416  res = filter_zp2ab(z, nz, p, np, ord, b, a);
417  if(res != RES_OK)
418  goto exit_label;
419 
420 
421  g0 = 1.0;
422  if(!(ord % 2))
423  {
424  g0 = 1.0 / pow(10.0, rp*0.05);
425  }
426 
427 
428  norm = g0 * a[0] / b[0];
429 
430  for(nz = 0; nz < ord+1; nz++)
431  b[nz]*=norm;
432 
433  exit_label:
434  if(z)
435  free(z);
436  if(p)
437  free(p);
438  return res;
439 }
440 
441 
442 
443 
444 
445 
446 /******************************************************************************
447  A *nalog Normalized Chebyshev type 2 filter zeros and poles
448  *******************************************************************************/
449 int DSPL_API ellip_ap_zp(int ord, double rp, double rs,
450  complex_t *z, int* nz, complex_t *p, int* np)
451 {
452  double es, ep;
453  int L, r, n, res;
454  int iz, ip;
455  double ke, k, u, t;
456  complex_t tc, v0, jv0;
457 
458 
459  if(rp < 0 || rp == 0)
460  return ERROR_FILTER_RP;
461  if(rs < 0 || rs == 0)
462  return ERROR_FILTER_RS;
463  if(ord < 1)
464  return ERROR_FILTER_ORD;
465  if(!z || !p || !nz || !np)
466  return ERROR_PTR;
467 
468  es = sqrt(pow(10.0, rs*0.1) - 1.0);
469  ep = sqrt(pow(10.0, rp*0.1) - 1.0);
470  ke = ep / es;
471 
472  r = ord % 2;
473  L = (int)((ord-r)/2);
474 
475  res = ellip_modulareq(rp, rs, ord, &k);
476  if(res != RES_OK)
477  return res;
478  // v0
479  RE(tc) = 0.0;
480  IM(tc) = 1.0 / ep;
481 
482  ellip_asn_cmplx(&tc, 1, ke, &v0);
483 
484  t = RE(v0);
485  RE(v0) = IM(v0) / (double)ord;
486  IM(v0) = -t / (double)ord;
487 
488  RE(jv0) = -IM(v0);
489  IM(jv0) = RE(v0);
490 
491 
492  iz = ip = 0;
493 
494  if(r)
495  {
496  res = ellip_sn_cmplx(&jv0, 1, k, &tc);
497  if(res != RES_OK)
498  return res;
499  RE(p[0]) = -IM(tc);
500  IM(p[0]) = RE(tc);
501  ip = 1;
502  }
503 
504  for(n = 0; n < L; n++)
505  {
506  u = (double)(2 * n + 1)/(double)ord;
507 
508  res = ellip_cd(& u, 1, k, &t);
509  if(res != RES_OK)
510  return res;
511 
512  RE(z[iz]) = RE(z[iz+1]) = 0.0;
513  IM(z[iz]) = 1.0/(k*t);
514  IM(z[iz+1]) = -1.0/(k*t);
515  iz+=2;
516 
517  RE(tc) = u - RE(jv0);
518  IM(tc) = - IM(jv0);
519 
520  res = ellip_cd_cmplx(&tc, 1, k, p+ip+1);
521  if(res != RES_OK)
522  return res;
523 
524  RE(p[ip]) = -IM(p[ip+1]);
525  IM(p[ip]) = RE(p[ip+1]);
526 
527  RE(p[ip+1]) = RE(p[ip]);
528  IM(p[ip+1]) = -IM(p[ip]);
529 
530  ip+=2;
531 
532 
533  }
534  *nz = iz;
535  *np = ip;
536 
537  return RES_OK;
538 }
539 
540 
541 
542 
543 /******************************************************************************
544 Zeros and poles to filter coefficients recalc
545 *******************************************************************************/
546 int DSPL_API filter_zp2ab(complex_t *z, int nz, complex_t *p, int np,
547  int ord, double* b, double* a)
548 {
549  complex_t *acc = NULL;
550  int res;
551 
552  if(!z || !p || !b || !a)
553  return ERROR_PTR;
554  if(nz < 0 || np < 0)
555  return ERROR_SIZE;
556  if(nz > ord || np > ord)
557  return ERROR_POLY_ORD;
558 
559  acc = (complex_t*) malloc((ord+1) * sizeof(complex_t));
560  res = poly_z2a_cmplx(z, nz, ord, acc);
561  if(res != RES_OK)
562  goto exit_label;
563 
564  res = cmplx2re(acc, ord+1, b, NULL);
565  if(res != RES_OK)
566  goto exit_label;
567 
568  res = poly_z2a_cmplx(p, np, ord, acc);
569  if(res != RES_OK)
570  goto exit_label;
571 
572  res = cmplx2re(acc, ord+1, a, NULL);
573  if(res != RES_OK)
574  goto exit_label;
575 
576 
577 
578 exit_label:
579  if(acc)
580  free(acc);
581  return res;
582 
583 }
584 
int low2low(double *b, double *a, int ord, double w0, double w1, double *beta, double *alpha)
Частотное преобразование ФНЧ-ФНЧ
Definition: filter_ft.c:179
#define ERROR_SIZE
Ошибка при передаче размера массива. Данная ошибка возникает когда помимо указателя на массив входных...
Definition: dspl.h:148
double complex_t[2]
Описание комплексного типа данных.
Definition: dspl.h:41
int butter_ap(double rp, int ord, double *b, double *a)
Расчет передаточной характеристики аналогового нормированного ФНЧ Баттерворта.
Definition: filter_ap.c:33
#define ERROR_PTR
Ошибка указателя. Данная ошибка означает, что один из обязательных указателей (память под который дол...
Definition: dspl.h:140
#define IM(x)
Макрос определяющий мнимую часть комплексного числа.
Definition: dspl.h:78
#define RE(x)
Макрос определяющий реальную часть комплексного числа.
Definition: dspl.h:77
#define ERROR_FILTER_ORD
Порядок фильтра задан неверно. Порядок фильтра должен быть задан положительным целым значением.
Definition: dspl.h:111
#define ERROR_FILTER_RS
Параметр подавления фильтра в полосе заграждения задан неверно. Данный параметр задается в дБ и долже...
Definition: dspl.h:114
int ellip_cd_cmplx(complex_t *u, int n, double k, complex_t *y)
Эллиптическая функция Якоби комплексного аргумента
Definition: ellipj.c:424
int cmplx2re(complex_t *x, int n, double *re, double *im)
Преобразование массива комплексных данных в два массива вещественных данных, содержащих реальную и мн...
Definition: complex.c:222
int cheby1_ap(double rp, int ord, double *b, double *a)
Расчет передаточной характеристики аналогового нормированного ФНЧ Чебышёва первого рода.
Definition: filter_ap.c:129
int ellip_cd(double *u, int n, double k, double *y)
Эллиптическая функция Якоби вещественного аргумента
Definition: ellipj.c:360
int ellip_asn_cmplx(complex_t *w, int n, double k, complex_t *u)
Обратная эллиптическая функция Якоби комплексного аргумента
Definition: ellipj.c:275
int cheby2_ap(double rs, int ord, double *b, double *a)
Расчет передаточной характеристики аналогового нормированного ФНЧ Чебышёва второго рода.
Definition: filter_ap.c:239
#define ERROR_FILTER_RP
Параметр неравномерности фильтра в полосе пропускания задан неверно. Данный параметр задается в дБ и ...
Definition: dspl.h:113
int ellip_sn_cmplx(complex_t *u, int n, double k, complex_t *y)
Эллиптическая функция Якоби комплексного аргумента
Definition: ellipj.c:758
#define RES_OK
Функция завершилась корректно. Ошибки отсутствуют.
Definition: dspl.h:94