libdspl-2.0
Библиотека алгоритмов цифровой обработки сигналов
filter_an.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 
22 #include <stdlib.h>
23 #include <string.h>
24 #include <math.h>
25 #include "dspl.h"
26 
27 
28 
29 
30 
31 /******************************************************************************
32 Magnitude, phase response and group delay of a digital filter H(z) or
33 analog filter H(s)
34 *******************************************************************************/
35 int DSPL_API filter_freq_resp(double* b, double* a, int ord,
36  double* w, int n, int flag,
37  double* mag, double* phi, double* tau)
38 {
39  int res, k, flag_analog;
40 
41  complex_t *hc = NULL;
42  double *phi0 = NULL;
43  double *phi1 = NULL;
44  double *w0 = NULL;
45  double *w1 = NULL;
46 
47  if(!b || !w)
48  return ERROR_PTR;
49  if(ord < 1)
50  return ERROR_FILTER_ORD;
51  if(n < 1)
52  return ERROR_SIZE;
53 
54  flag_analog = flag & DSPL_FLAG_ANALOG;
55 
56  hc = (complex_t*) malloc (n*sizeof(complex_t));
57 
58  res = flag_analog ? freqs(b, a, ord, w, n, hc) : freqz(b, a, ord, w, n, hc);
59  if(res != RES_OK)
60  goto exit_label;
61 
62 
63  if(mag)
64  {
65  if(flag & DSPL_FLAG_LOGMAG)
66  {
67  for(k = 0; k < n; k++)
68  mag[k] = 10.0 * log10(ABSSQR(hc[k]));
69  }
70  else
71  {
72  for(k = 0; k < n; k++)
73  mag[k] = sqrt(ABSSQR(hc[k]));
74  }
75  }
76 
77 
78  if(phi)
79  {
80  for(k = 0; k < n; k++)
81  phi[k] = atan2(IM(hc[k]), RE(hc[k]));
82 
83  if(flag & DSPL_FLAG_UNWRAP)
84  {
85  res = unwrap(phi, n, M_2PI, 0.8);
86  if(res != RES_OK)
87  goto exit_label;
88  }
89  }
90 
91 
92  if(tau)
93  {
94  phi0 = (double*) malloc(n*sizeof(double));
95  phi1 = (double*) malloc(n*sizeof(double));
96  w0 = (double*) malloc(n*sizeof(double));
97  w1 = (double*) malloc(n*sizeof(double));
98 
99  w0[0] = w[0] - (w[1] - w[0])*0.02;
100  w1[0] = w[0] + (w[1] - w[0])*0.02;
101 
102  for(k = 1; k < n; k++)
103  {
104  w0[k] = w[k] - (w[k] - w[k-1])*0.02;
105  w1[k] = w[k] + (w[k] - w[k-1])*0.02;
106  }
107  res = filter_freq_resp(b, a, ord, w0, n,
108  DSPL_FLAG_UNWRAP | flag_analog, NULL, phi0, NULL);
109  if(res != RES_OK)
110  goto exit_label;
111  res = filter_freq_resp(b, a, ord, w1, n,
112  DSPL_FLAG_UNWRAP | flag_analog, NULL, phi1, NULL);
113  if(res != RES_OK)
114  goto exit_label;
115  for(k = 0; k < n; k++)
116  tau[k] = (phi0[k] - phi1[k])/(w1[k] - w0[k]);
117  }
118 
119 
120 exit_label:
121  if(hc)
122  free(hc);
123  if(phi0)
124  free(phi0);
125  if(phi1)
126  free(phi1);
127  if(w0)
128  free(w0);
129  if(w1)
130  free(w1);
131  return res;
132 }
133 
134 
135 
136 
137 /******************************************************************************
138 Complex frequency response of an analog filter H(s)
139 *******************************************************************************/
140 int DSPL_API freqs(double* b, double* a, int ord,
141  double* w, int n, complex_t *h)
142 {
143  complex_t jw;
144  complex_t *bc = NULL;
145  complex_t *ac = NULL;
146  complex_t num, den;
147  double mag;
148  int k;
149  int res;
150 
151  if(!b || !a || !w || !h)
152  return ERROR_PTR;
153  if(ord<0)
154  return ERROR_FILTER_ORD;
155  if(n<1)
156  return ERROR_SIZE;
157 
158  RE(jw) = 0.0;
159 
160  bc = (complex_t*) malloc((ord+1) * sizeof(complex_t));
161  res = re2cmplx(b, ord+1, bc);
162 
163  if( res!=RES_OK )
164  goto exit_label;
165 
166  ac = (complex_t*) malloc((ord+1) * sizeof(complex_t));
167  res = re2cmplx(a, ord+1, ac);
168  if( res!=RES_OK )
169  goto exit_label;
170 
171  for(k = 0; k < n; k++)
172  {
173  IM(jw) = w[k];
174  res = polyval_cmplx(bc, ord, &jw, 1, &num);
175  if(res != RES_OK)
176  goto exit_label;
177  res = polyval_cmplx(ac, ord, &jw, 1, &den);
178  if(res != RES_OK)
179  goto exit_label;
180  mag = ABSSQR(den);
181  if(mag == 0.0)
182  {
183  res = ERROR_DIV_ZERO;
184  goto exit_label;
185  }
186  mag = 1.0 / mag;
187  RE(h[k]) = CMCONJRE(num, den) * mag;
188  IM(h[k]) = CMCONJIM(num, den) * mag;
189  }
190  res = RES_OK;
191 exit_label:
192  if(bc)
193  free(bc);
194  if(ac)
195  free(ac);
196  return res;
197 }
198 
199 
200 
201 
202 
203 
204 /******************************************************************************
205  * Complex frequency response of an analog filter H(s), s is complex variable
206  ******************************************************************************/
207 int DSPL_API freqs_cmplx(double* b, double* a, int ord,
208  complex_t* s, int n, complex_t *h)
209 {
210  complex_t *bc = NULL;
211  complex_t *ac = NULL;
212  complex_t num, den;
213  double mag;
214  int k;
215  int res;
216 
217  if(!b || !a || !s || !h)
218  return ERROR_PTR;
219  if(ord<0)
220  return ERROR_FILTER_ORD;
221  if(n<1)
222  return ERROR_SIZE;
223 
224 
225  bc = (complex_t*) malloc((ord+1) * sizeof(complex_t));
226  res = re2cmplx(b, ord+1, bc);
227 
228  if( res!=RES_OK )
229  goto exit_label;
230 
231  ac = (complex_t*) malloc((ord+1) * sizeof(complex_t));
232  res = re2cmplx(a, ord+1, ac);
233  if( res!=RES_OK )
234  goto exit_label;
235 
236  for(k = 0; k < n; k++)
237  {
238  res = polyval_cmplx(bc, ord, s+k, 1, &num);
239  if(res != RES_OK)
240  goto exit_label;
241  res = polyval_cmplx(ac, ord, s+k, 1, &den);
242  if(res != RES_OK)
243  goto exit_label;
244  mag = ABSSQR(den);
245  if(mag == 0.0)
246  {
247  res = ERROR_DIV_ZERO;
248  goto exit_label;
249  }
250  mag = 1.0 / mag;
251  RE(h[k]) = CMCONJRE(num, den) * mag;
252  IM(h[k]) = CMCONJIM(num, den) * mag;
253 
254  }
255  res = RES_OK;
256  exit_label:
257  if(bc)
258  free(bc);
259  if(ac)
260  free(ac);
261  return res;
262 }
263 
264 
265 
266 
267 
268 
269 
270 /******************************************************************************
271 impulse response of an analog filter H(s)
272 *******************************************************************************/
273 int DSPL_API freqs2time(double* b, double* a, int ord, double fs,
274  int n, fft_t* pfft, double *t, double *h)
275 {
276  double *w = NULL;
277  complex_t *hs = NULL;
278  complex_t *ht = NULL;
279  int err, k;
280 
281  if(!b || !a || !t || !h)
282  return ERROR_PTR;
283  if(ord<1)
284  return ERROR_FILTER_ORD;
285  if(n<1)
286  return ERROR_SIZE;
287 
288  w = (double*)malloc(n*sizeof(double));
289  hs = (complex_t*)malloc(n*sizeof(complex_t));
290 
291 
292  err = linspace(-fs*0.5, fs*0.5, n, DSPL_PERIODIC, w);
293  if(err != RES_OK)
294  goto exit_label;
295 
296  err = freqs(b, a, ord, w, n, hs);
297  if(err != RES_OK)
298  goto exit_label;
299 
300  err = fft_shift_cmplx(hs, n, hs);
301  if(err != RES_OK)
302  goto exit_label;
303 
304  ht = (complex_t*)malloc(n*sizeof(complex_t));
305 
306  err = ifft_cmplx(hs, n, pfft, ht);
307  if(err != RES_OK)
308  {
309  err = idft_cmplx(hs, n, ht);
310  if(err != RES_OK)
311  goto exit_label;
312  }
313 
314  for(k = 0; k < n; k++)
315  {
316  t[k] = (double)k/fs;
317  h[k] = RE(ht[k]) * fs;
318  }
319 
320 exit_label:
321  if(w)
322  free(w);
323  if(hs)
324  free(hs);
325  if(ht)
326  free(ht);
327  return err;
328 }
329 
330 
331 
332 
333 
334 
335 /******************************************************************************
336 Magnitude, phase response and group delay of an analog filter H(s)
337 
338 int DSPL_API freqs_resp(double* b, double* a, int ord,
339  double* w, int n, int flag,
340  double *h, double* phi, double* tau)
341 {
342  int res, k;
343 
344  complex_t *hc = NULL;
345  double *phi0 = NULL;
346  double *phi1 = NULL;
347  double *w0 = NULL;
348  double *w1 = NULL;
349 
350  if(!b || !a || !w)
351  return ERROR_PTR;
352  if(ord < 1)
353  return ERROR_FILTER_ORD;
354  if(n < 1)
355  return ERROR_SIZE;
356 
357 
358  hc = (complex_t*) malloc (n*sizeof(complex_t));
359  res = freqs(b, a, ord, w, n, hc);
360  if(res != RES_OK)
361  goto exit_label;
362 
363 
364  if(h)
365  {
366  if(flag & DSPL_FLAG_LOG)
367  {
368  for(k = 0; k < n; k++)
369  h[k] = 10.0 * log10(ABSSQR(hc[k]));
370  }
371  else
372  {
373  for(k = 0; k < n; k++)
374  h[k] = sqrt(ABSSQR(hc[k]));
375  }
376  }
377 
378 
379  if(phi)
380  {
381  for(k = 0; k < n; k++)
382  phi[k] = atan2(IM(hc[k]), RE(hc[k]));
383 
384  if(flag & DSPL_FLAG_UNWRAP)
385  {
386  res = unwrap(phi, n, M_2PI, 0.8);
387  if(res != RES_OK)
388  goto exit_label;
389  }
390  }
391 
392 
393  if(tau)
394  {
395  phi0 = (double*) malloc(n*sizeof(double));
396  phi1 = (double*) malloc(n*sizeof(double));
397  w0 = (double*) malloc(n*sizeof(double));
398  w1 = (double*) malloc(n*sizeof(double));
399 
400  w0[0] = w[0] - (w[1] - w[0])*0.02;
401  w1[0] = w[0] + (w[1] - w[0])*0.02;
402 
403  for(k = 1; k < n; k++)
404  {
405  w0[k] = w[k] - (w[k] - w[k-1])*0.02;
406  w1[k] = w[k] + (w[k] - w[k-1])*0.02;
407  }
408  res = freqs_resp(b, a, ord, w0, n, DSPL_FLAG_UNWRAP, NULL, phi0, NULL);
409  if(res != RES_OK)
410  goto exit_label;
411  res = freqs_resp(b, a, ord, w1, n, DSPL_FLAG_UNWRAP, NULL, phi1, NULL);
412  if(res != RES_OK)
413  goto exit_label;
414  for(k = 0; k < n; k++)
415  tau[k] = (phi0[k] - phi1[k])/(w1[k] - w0[k]);
416  }
417 
418 
419 exit_label:
420  if(hc)
421  free(hc);
422  if(phi0)
423  free(phi0);
424  if(phi1)
425  free(phi1);
426  if(w0)
427  free(w0);
428  if(w1)
429  free(w1);
430  return res;
431 }
432 
433 *******************************************************************************/
434 
435 
436 
437 
438 
439 
440 
441 
442 /*******************************************************************************
443 Complex frequency response of a digital filter H(z)
444 *******************************************************************************/
445 int DSPL_API freqz(double* b, double* a, int ord, double* w,
446  int n, complex_t *h)
447 {
448  complex_t jw;
449  complex_t *bc = NULL;
450  complex_t *ac = NULL;
451  complex_t num, den;
452  double mag;
453  int k;
454  int res;
455 
456  if(!b || !w || !h)
457  return ERROR_PTR;
458  if(ord<0)
459  return ERROR_FILTER_ORD;
460  if(n<1)
461  return ERROR_SIZE;
462 
463 
464  bc = (complex_t*) malloc((ord+1) * sizeof(complex_t));
465  res = re2cmplx(b, ord+1, bc);
466  if( res!=RES_OK )
467  goto exit_label;
468 
469  if(a)
470  {
471  /* IIR filter if a != NULL */
472  ac = (complex_t*) malloc((ord+1) * sizeof(complex_t));
473  res = re2cmplx(a, ord+1, ac);
474  if( res!=RES_OK )
475  goto exit_label;
476  for(k = 0; k < n; k++)
477  {
478  RE(jw) = cos(w[k]);
479  IM(jw) = -sin(w[k]);
480  res = polyval_cmplx(bc, ord, &jw, 1, &num);
481  if(res != RES_OK)
482  goto exit_label;
483  res = polyval_cmplx(ac, ord, &jw, 1, &den);
484  if(res != RES_OK)
485  goto exit_label;
486  mag = ABSSQR(den);
487  if(mag == 0.0)
488  {
489  res = ERROR_DIV_ZERO;
490  goto exit_label;
491  }
492  mag = 1.0 / mag;
493  RE(h[k]) = CMCONJRE(num, den) * mag;
494  IM(h[k]) = CMCONJIM(num, den) * mag;
495  }
496  }
497  else
498  {
499  /* FIR filter if a == NULL */
500  for(k = 0; k < n; k++)
501  {
502  RE(jw) = cos(w[k]);
503  IM(jw) = -sin(w[k]);
504  res = polyval_cmplx(bc, ord, &jw, 1, h+k);
505  if(res != RES_OK)
506  goto exit_label;
507  }
508  }
509  res = RES_OK;
510 exit_label:
511  if(bc)
512  free(bc);
513  if(ac)
514  free(ac);
515  return res;
516 }
517 
518 
519 
520 
521 
522 
523 
524 
525 /*******************************************************************************
526 Unwrap function
527 *******************************************************************************/
528 int DSPL_API unwrap(double* phi, int n, double lev, double mar)
529 {
530  double a[2] = {0.0, 0.0};
531  double d;
532  double th;
533  int k;
534  int flag = 1;
535 
536 
537  if(!phi)
538  return ERROR_PTR;
539 
540  if(n<1)
541  return ERROR_SIZE;
542 
543  if(lev<=0 || mar <=0)
544  return ERROR_UNWRAP;
545 
546  th = mar*lev;
547  while(flag)
548  {
549  flag = 0;
550  a[0] = a[1] = 0.0;
551  for(k = 0; k<n-1; k++)
552  {
553  d = phi[k+1] - phi[k];
554  if( d > th)
555  {
556  a[0] -= lev;
557  flag = 1;
558  }
559  if( d < -th)
560  {
561  a[0] += lev;
562  flag = 1;
563  }
564  phi[k]+=a[1];
565  a[1] = a[0];
566  }
567  phi[n-1]+=a[1];
568  }
569 
570  return RES_OK;
571 }
572 
#define ERROR_SIZE
Ошибка при передаче размера массива. Данная ошибка возникает когда помимо указателя на массив входных...
Definition: dspl.h:148
Структура данных объекта быстрого преобразования Фурье
Definition: dspl.h:46
double complex_t[2]
Описание комплексного типа данных.
Definition: dspl.h:41
int filter_freq_resp(double *b, double *a, int ord, double *w, int n, int flag, double *mag, double *phi, double *tau)
Расчет амплитудно-частотной (АЧХ), фазочастотной характеристик (ФЧХ), а также группового времени запа...
Definition: filter_an.c:35
#define ERROR_PTR
Ошибка указателя. Данная ошибка означает, что один из обязательных указателей (память под который дол...
Definition: dspl.h:140
int freqs(double *b, double *a, int ord, double *w, int n, complex_t *h)
Расчет комплексного коэффициента передачи аналогового фильтра.
Definition: filter_an.c:140
#define IM(x)
Макрос определяющий мнимую часть комплексного числа.
Definition: dspl.h:78
#define ABSSQR(x)
Макрос возвращает квадрат модуля комплексного числа x.
Definition: dspl.h:82
int freqz(double *b, double *a, int ord, double *w, int n, complex_t *h)
Расчет комплексного коэффициента передачи цифрового фильтра.
Definition: filter_an.c:445
#define RE(x)
Макрос определяющий реальную часть комплексного числа.
Definition: dspl.h:77
int re2cmplx(double *x, int n, complex_t *y)
Преобразование массива вещественных данных в массив комплексных данных.
Definition: complex.c:441
#define ERROR_FILTER_ORD
Порядок фильтра задан неверно. Порядок фильтра должен быть задан положительным целым значением.
Definition: dspl.h:111
int ifft_cmplx(complex_t *x, int n, fft_t *pfft, complex_t *y)
Обратное быстрое преобразование Фурье
Definition: fft.c:30
int idft_cmplx(complex_t *x, int n, complex_t *y)
Обратное дискретное преобразование Фурье комплексного спектра.
Definition: dft.c:277
int linspace(double x0, double x1, int n, int type, double *x)
Функция заполняет массив линейно-нарастающими, равноотстоящими значениями от x0 до x1
Definition: fillarray.c:31
int polyval_cmplx(complex_t *a, int ord, complex_t *x, int n, complex_t *y)
Расчет комплексного полинома
Definition: polyval.c:96
#define RES_OK
Функция завершилась корректно. Ошибки отсутствуют.
Definition: dspl.h:94