libdspl-2.0
Digital Signal Processing Algorithm Library
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 #ifdef DOXYGEN_ENGLISH
32 
97 #endif
98 #ifdef DOXYGEN_RUSSIAN
99 
165 #endif
166 int DSPL_API group_delay(double* pb, double* pa, int ord, int flag,
167  double* w, int n, double* tau)
168 {
169  double a, b, c, d, da, db, dc, dd, f, e;
170  int t, m;
171 
172  double *qa = NULL;
173 
174  if(!pb || !w || !tau || (!pa && (flag & DSPL_FLAG_ANALOG)))
175  return ERROR_PTR;
176  if(ord < 1)
177  return ERROR_FILTER_ORD;
178  if(n < 1)
179  return ERROR_SIZE;
180 
181 
182  if(pa)
183  qa = pa;
184  else
185  {
186  qa = (double*)malloc((ord+1) * sizeof(double));
187  memset(qa, 0, (ord+1) * sizeof(double));
188  qa[0] = 1.0;
189  }
190 
191  for(t = 0; t < n; t++)
192  {
193  a = b = c = d = da = db = dc = dd = 0.0;
194  if(flag & DSPL_FLAG_ANALOG)
195  {
196  for(m = 0; m < ord+1; m+=4)
197  {
198  a += pb[m] * pow(w[t], (double)m);
199  c += qa[m] * pow(w[t], (double)m);
200  da += pb[m] * (double) m * pow(w[t], (double)(m-1));
201  dc += qa[m] * (double) m * pow(w[t], (double)(m-1));
202  }
203  for(m = 2; m < ord+1; m+=4)
204  {
205  a -= pb[m] * pow(w[t], (double)m);
206  c -= qa[m] * pow(w[t], (double)m);
207  da -= pb[m] * (double) m * pow(w[t], (double)(m-1));
208  dc -= qa[m] * (double) m * pow(w[t], (double)(m-1));
209  }
210 
211  for(m = 1; m < ord+1; m+=4)
212  {
213  b += pb[m] * pow(w[t], (double)m) ;
214  d += qa[m] * pow(w[t], (double)m) ;
215  db += pb[m] * (double) m * pow(w[t], (double)(m-1)) ;
216  dd += qa[m] * (double) m * pow(w[t], (double)(m-1)) ;
217  }
218 
219  for(m = 3; m < ord+1; m+=4)
220  {
221  b -= pb[m] * pow(w[t], (double)m) ;
222  d -= qa[m] * pow(w[t], (double)m) ;
223  db -= pb[m] * (double) m * pow(w[t], (double)(m-1)) ;
224  dd -= qa[m] * (double) m * pow(w[t], (double)(m-1)) ;
225  }
226 
227  }
228  else
229  {
230  for(m = 0; m < ord+1; m++)
231  {
232  a += pb[m] * cos(w[t]*(double)m);
233  b -= pb[m] * sin(w[t]*(double)m);
234  c += qa[m] * cos(w[t]*(double)m);
235  d -= qa[m] * sin(w[t]*(double)m);
236 
237  da -= pb[m] *(double)m * sin(w[t]*(double)m);
238  db -= pb[m] *(double)m * cos(w[t]*(double)m);
239  dc -= qa[m] *(double)m * sin(w[t]*(double)m);
240  dd -= qa[m] *(double)m * cos(w[t]*(double)m);
241  }
242  }
243 
244  f = da * c + a * dc + db * d + b * dd;
245  e = db * c + b * dc - da * d - a * dd;
246  tau[t] = (f * (b * c - a * d) - e * (a * c + b * d)) /
247  ((a * a + b * b) * (c * c + d * d));
248  }
249 
250  if(qa != pa)
251  free(qa);
252 
253  return RES_OK;
254 }
255 
256 
257 #ifdef DOXYGEN_ENGLISH
258 
356 #endif
357 #ifdef DOXYGEN_RUSSIAN
358 
462 #endif
463 int DSPL_API filter_freq_resp(double* b, double* a, int ord,
464  double* w, int n, int flag,
465  double* mag, double* phi, double* tau)
466 {
467  int res, k, flag_analog;
468 
469  complex_t *hc = NULL;
470  double *phi0 = NULL;
471  double *phi1 = NULL;
472  double *w0 = NULL;
473  double *w1 = NULL;
474 
475  if(!b || !w)
476  return ERROR_PTR;
477  if(ord < 1)
478  return ERROR_FILTER_ORD;
479  if(n < 1)
480  return ERROR_SIZE;
481 
482  flag_analog = flag & DSPL_FLAG_ANALOG;
483 
484  hc = (complex_t*) malloc (n*sizeof(complex_t));
485 
486  res = flag_analog ?
487  freqs(b, a, ord, w, n, hc) :
488  freqz(b, a, ord, w, n, hc);
489 
490  if(res != RES_OK)
491  goto exit_label;
492 
493 
494  if(mag)
495  {
496  if(flag & DSPL_FLAG_LOGMAG)
497  {
498  for(k = 0; k < n; k++)
499  mag[k] = 10.0 * log10(ABSSQR(hc[k]));
500  }
501  else
502  {
503  for(k = 0; k < n; k++)
504  mag[k] = sqrt(ABSSQR(hc[k]));
505  }
506  }
507 
508 
509  if(phi)
510  {
511  for(k = 0; k < n; k++)
512  phi[k] = atan2(IM(hc[k]), RE(hc[k]));
513 
514  if(flag & DSPL_FLAG_UNWRAP)
515  {
516  res = unwrap(phi, n, M_2PI, 0.8);
517  if(res != RES_OK)
518  goto exit_label;
519  }
520  }
521 
522 
523  if(tau)
524  res = group_delay(b, a, ord, flag, w, n, tau);
525 
526 
527 
528 exit_label:
529  if(hc)
530  free(hc);
531  if(phi0)
532  free(phi0);
533  if(phi1)
534  free(phi1);
535  if(w0)
536  free(w0);
537  if(w1)
538  free(w1);
539  return res;
540 }
541 
542 
543 
544 
545 #ifdef DOXYGEN_ENGLISH
546 
595 #endif
596 #ifdef DOXYGEN_RUSSIAN
597 
658 #endif
659 int DSPL_API freqs(double* b, double* a, int ord,
660  double* w, int n, complex_t *h)
661 {
662  complex_t jw;
663  complex_t *bc = NULL;
664  complex_t *ac = NULL;
665  complex_t num, den;
666  double mag;
667  int k;
668  int res;
669 
670  if(!b || !a || !w || !h)
671  return ERROR_PTR;
672  if(ord<0)
673  return ERROR_FILTER_ORD;
674  if(n<1)
675  return ERROR_SIZE;
676 
677  RE(jw) = 0.0;
678 
679  bc = (complex_t*) malloc((ord+1) * sizeof(complex_t));
680  res = re2cmplx(b, ord+1, bc);
681 
682  if( res!=RES_OK )
683  goto exit_label;
684 
685  ac = (complex_t*) malloc((ord+1) * sizeof(complex_t));
686  res = re2cmplx(a, ord+1, ac);
687  if( res!=RES_OK )
688  goto exit_label;
689 
690  for(k = 0; k < n; k++)
691  {
692  IM(jw) = w[k];
693  res = polyval_cmplx(bc, ord, &jw, 1, &num);
694  if(res != RES_OK)
695  goto exit_label;
696  res = polyval_cmplx(ac, ord, &jw, 1, &den);
697  if(res != RES_OK)
698  goto exit_label;
699  mag = ABSSQR(den);
700  if(mag == 0.0)
701  {
702  res = ERROR_DIV_ZERO;
703  goto exit_label;
704  }
705  mag = 1.0 / mag;
706  RE(h[k]) = CMCONJRE(num, den) * mag;
707  IM(h[k]) = CMCONJIM(num, den) * mag;
708  }
709  res = RES_OK;
710 exit_label:
711  if(bc)
712  free(bc);
713  if(ac)
714  free(ac);
715  return res;
716 }
717 
718 
719 
720 
721 
722 
723 #ifdef DOXYGEN_ENGLISH
724 
725 #endif
726 #ifdef DOXYGEN_RUSSIAN
727 
728 #endif
729 int DSPL_API freqs_cmplx(double* b, double* a, int ord,
730  complex_t* s, int n, complex_t *h)
731 {
732  complex_t *bc = NULL;
733  complex_t *ac = NULL;
734  complex_t num, den;
735  double mag;
736  int k;
737  int res;
738 
739  if(!b || !a || !s || !h)
740  return ERROR_PTR;
741  if(ord<0)
742  return ERROR_FILTER_ORD;
743  if(n<1)
744  return ERROR_SIZE;
745 
746 
747  bc = (complex_t*) malloc((ord+1) * sizeof(complex_t));
748  res = re2cmplx(b, ord+1, bc);
749 
750  if( res!=RES_OK )
751  goto exit_label;
752 
753  ac = (complex_t*) malloc((ord+1) * sizeof(complex_t));
754  res = re2cmplx(a, ord+1, ac);
755  if( res!=RES_OK )
756  goto exit_label;
757 
758  for(k = 0; k < n; k++)
759  {
760  res = polyval_cmplx(bc, ord, s+k, 1, &num);
761  if(res != RES_OK)
762  goto exit_label;
763  res = polyval_cmplx(ac, ord, s+k, 1, &den);
764  if(res != RES_OK)
765  goto exit_label;
766  mag = ABSSQR(den);
767  if(mag == 0.0)
768  {
769  res = ERROR_DIV_ZERO;
770  goto exit_label;
771  }
772  mag = 1.0 / mag;
773  RE(h[k]) = CMCONJRE(num, den) * mag;
774  IM(h[k]) = CMCONJIM(num, den) * mag;
775 
776  }
777  res = RES_OK;
778  exit_label:
779  if(bc)
780  free(bc);
781  if(ac)
782  free(ac);
783  return res;
784 }
785 
786 
787 #ifdef DOXYGEN_ENGLISH
788 
789 #endif
790 #ifdef DOXYGEN_RUSSIAN
791 
792 #endif
793 int DSPL_API freqs2time(double* b, double* a, int ord, double fs,
794  int n, fft_t* pfft, double *t, double *h)
795 {
796  double *w = NULL;
797  complex_t *hs = NULL;
798  complex_t *ht = NULL;
799  int err, k;
800 
801  if(!b || !a || !t || !h)
802  return ERROR_PTR;
803  if(ord<1)
804  return ERROR_FILTER_ORD;
805  if(n<1)
806  return ERROR_SIZE;
807 
808  w = (double*)malloc(n*sizeof(double));
809  hs = (complex_t*)malloc(n*sizeof(complex_t));
810 
811 
812  err = linspace(-fs*0.5, fs*0.5, n, DSPL_PERIODIC, w);
813  if(err != RES_OK)
814  goto exit_label;
815 
816  err = freqs(b, a, ord, w, n, hs);
817  if(err != RES_OK)
818  goto exit_label;
819 
820  err = fft_shift_cmplx(hs, n, hs);
821  if(err != RES_OK)
822  goto exit_label;
823 
824  ht = (complex_t*)malloc(n*sizeof(complex_t));
825 
826  err = ifft_cmplx(hs, n, pfft, ht);
827  if(err != RES_OK)
828  {
829  err = idft_cmplx(hs, n, ht);
830  if(err != RES_OK)
831  goto exit_label;
832  }
833 
834  for(k = 0; k < n; k++)
835  {
836  t[k] = (double)k/fs;
837  h[k] = RE(ht[k]) * fs;
838  }
839 
840 exit_label:
841  if(w)
842  free(w);
843  if(hs)
844  free(hs);
845  if(ht)
846  free(ht);
847  return err;
848 }
849 
850 
851 
852 #ifdef DOXYGEN_ENGLISH
853 
908 #endif
909 #ifdef DOXYGEN_RUSSIAN
910 
979 #endif
980 int DSPL_API freqz(double* b, double* a, int ord, double* w,
981  int n, complex_t *h)
982 {
983  complex_t jw;
984  complex_t *bc = NULL;
985  complex_t *ac = NULL;
986  complex_t num, den;
987  double mag;
988  int k;
989  int res;
990 
991  if(!b || !w || !h)
992  return ERROR_PTR;
993  if(ord<0)
994  return ERROR_FILTER_ORD;
995  if(n<1)
996  return ERROR_SIZE;
997 
998 
999  bc = (complex_t*) malloc((ord+1) * sizeof(complex_t));
1000  res = re2cmplx(b, ord+1, bc);
1001  if( res!=RES_OK )
1002  goto exit_label;
1003 
1004  if(a)
1005  {
1006  /* IIR filter if a != NULL */
1007  ac = (complex_t*) malloc((ord+1) * sizeof(complex_t));
1008  res = re2cmplx(a, ord+1, ac);
1009  if( res!=RES_OK )
1010  goto exit_label;
1011  for(k = 0; k < n; k++)
1012  {
1013  RE(jw) = cos(w[k]);
1014  IM(jw) = -sin(w[k]);
1015  res = polyval_cmplx(bc, ord, &jw, 1, &num);
1016  if(res != RES_OK)
1017  goto exit_label;
1018  res = polyval_cmplx(ac, ord, &jw, 1, &den);
1019  if(res != RES_OK)
1020  goto exit_label;
1021  mag = ABSSQR(den);
1022  if(mag == 0.0)
1023  {
1024  res = ERROR_DIV_ZERO;
1025  goto exit_label;
1026  }
1027  mag = 1.0 / mag;
1028  RE(h[k]) = CMCONJRE(num, den) * mag;
1029  IM(h[k]) = CMCONJIM(num, den) * mag;
1030  }
1031  }
1032  else
1033  {
1034  /* FIR filter if a == NULL */
1035  for(k = 0; k < n; k++)
1036  {
1037  RE(jw) = cos(w[k]);
1038  IM(jw) = -sin(w[k]);
1039  res = polyval_cmplx(bc, ord, &jw, 1, h+k);
1040  if(res != RES_OK)
1041  goto exit_label;
1042  }
1043  }
1044  res = RES_OK;
1045 exit_label:
1046  if(bc)
1047  free(bc);
1048  if(ac)
1049  free(ac);
1050  return res;
1051 }
1052 
1053 
1054 
1055 #ifdef DOXYGEN_ENGLISH
1056 
1121 #endif
1122 #ifdef DOXYGEN_RUSSIAN
1123 
1189 #endif
1190 int DSPL_API phase_delay(double* b, double* a, int ord, int flag,
1191  double* w, int n, double* tau)
1192 {
1193  int err, i;
1194  double *phi = NULL;
1195 
1196  if(n > 0)
1197  phi = (double*)malloc(n*sizeof(double));
1198  else
1199  return ERROR_SIZE;
1200 
1201  err = filter_freq_resp(b, a, ord, w, n, flag | DSPL_FLAG_UNWRAP, NULL, phi, NULL);
1202  if(err!=RES_OK)
1203  goto exit_label;
1204  for(i = 0; i < n; i++)
1205  {
1206  tau[i] = w[i] ? ( - phi[i] / w[i]) : ( - phi[i] / (w[i] + 1E-9) );
1207  }
1208 exit_label:
1209  if(phi)
1210  free(phi);
1211  return err;
1212 }
1213 
1214 
1215 
1216 
1217 #ifdef DOXYGEN_ENGLISH
1218 
1219 #endif
1220 #ifdef DOXYGEN_RUSSIAN
1221 
1222 #endif
1223 int DSPL_API unwrap(double* phi, int n, double lev, double mar)
1224 {
1225  double a[2] = {0.0, 0.0};
1226  double d;
1227  double th;
1228  int k;
1229  int flag = 1;
1230 
1231  if(!phi)
1232  return ERROR_PTR;
1233 
1234  if(n<1)
1235  return ERROR_SIZE;
1236 
1237  if(lev<=0 || mar <=0)
1238  return ERROR_UNWRAP;
1239 
1240  th = mar*lev;
1241  while(flag)
1242  {
1243  flag = 0;
1244  a[0] = a[1] = 0.0;
1245  for(k = 0; k<n-1; k++)
1246  {
1247  d = phi[k+1] - phi[k];
1248  if( d > th)
1249  {
1250  a[0] -= lev;
1251  flag = 1;
1252  }
1253  if( d < -th)
1254  {
1255  a[0] += lev;
1256  flag = 1;
1257  }
1258  phi[k]+=a[1];
1259  a[1] = a[0];
1260  }
1261  phi[n-1]+=a[1];
1262  }
1263 
1264  return RES_OK;
1265 }
1266 
#define ERROR_FILTER_ORD
The filter order is incorrect. The order of the filter must be specified by a positive integer value.
Definition: dspl.h:514
#define RE(x)
Macro sets real part of the complex number.
Definition: dspl.h:359
#define ERROR_PTR
Pointer error. This error means that one of the required pointers (memory to be allocated for) is tra...
Definition: dspl.h:545
int DSPL_API idft_cmplx(complex_t *x, int n, complex_t *y)
Inverse discrete Fourier transform of the complex spectrum.
Definition: dft.c:500
#define ERROR_SIZE
Error array size. This error occurs when in addition to the pointer the wrong input is passed to the ...
Definition: dspl.h:553
int DSPL_API ifft_cmplx(complex_t *x, int n, fft_t *pfft, complex_t *y)
Inverse fast Fourier transform.
Definition: fft.c:179
int DSPL_API phase_delay(double *b, double *a, int ord, int flag, double *w, int n, double *tau)
Phase delay calculation for digital or analog filter corresponds to , or transfer function.
Definition: filter_an.c:1190
Fast Fourier Transform Object Data Structure.
Definition: dspl.h:227
#define ERROR_DIV_ZERO
Division by zero error. The function returns this error if a computational algorithm occurs division ...
Definition: dspl.h:506
int DSPL_API re2cmplx(double *x, int n, complex_t *y)
Convert real array to the complex array.
Definition: complex.c:246
int DSPL_API freqz(double *b, double *a, int ord, double *w, int n, complex_t *h)
Function calculates the digital filter frequency response corresponds to transfer function .
Definition: filter_an.c:980
double complex_t[2]
Complex data type.
Definition: dspl.h:86
int DSPL_API filter_freq_resp(double *b, double *a, int ord, double *w, int n, int flag, double *mag, double *phi, double *tau)
Magnitude, phase response and group delay vectors calculation for digital or analog filter correspond...
Definition: filter_an.c:463
#define RES_OK
The function completed correctly. No errors.
Definition: dspl.h:497
#define ABSSQR(x)
The macro returns the square of the modulus of a complex number x.
Definition: dspl.h:473
int DSPL_API freqs(double *b, double *a, int ord, double *w, int n, complex_t *h)
Analog filter frequency response calculation.
Definition: filter_an.c:659
int DSPL_API linspace(double x0, double x1, int n, int type, double *x)
Function fills a vector with n linearly spaced elements between x0 and x1.
Definition: array.c:1009
#define ERROR_UNWRAP
Error in unwrap function. The phase period and indent parameter must be positive numbers.
Definition: dspl.h:557
#define IM(x)
Macro sets imaginary part of the complex number.
Definition: dspl.h:417
int DSPL_API group_delay(double *pb, double *pa, int ord, int flag, double *w, int n, double *tau)
Group delay calculation for digital or analog filter corresponds to , or transfer function.
Definition: filter_an.c:166