libdspl-2.0
Библиотека алгоритмов цифровой обработки сигналов
xcorr.c
1 /*
2 * Copyright (c) 2015-2019 Sergey Bakhurin
3 * Digital Signal Processing Library [http://dsplib.org]
4 *
5 * This file is part of DSPL.
6 *
7 * is free software: you can redistribute it and/or modify
8 * it under the terms of the GNU 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 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 int xcorr_fft_size(int nx, int ny, int* pnfft, int* pndata);
28 
29 int xcorr_get_lag_cmplx(complex_t* x, int nd, int nr, complex_t* r, double* t);
30 
31 int xcorr_krn(complex_t* x, int nx, complex_t* y, int ny, fft_t* pfft,
32  int flag, int nr, complex_t* r, double* t);
33 
34 int xcorr_scale_cmplx(complex_t* x, int nd, int flag);
35 
36 
37 
38 #ifdef DOXYGEN_ENGLISH
39 
125 #endif
126 #ifdef DOXYGEN_RUSSIAN
127 
213 #endif
214 int DSPL_API xcorr(double* x, int nx, double* y, int ny,
215  int flag, int nr, double* r, double* t)
216 {
217  fft_t fft = {0};
218  int err;
219  complex_t *cx = NULL;
220  complex_t *cy = NULL;
221  complex_t *cr = NULL;
222 
223  cr = (complex_t*)malloc((2 * nr + 1) * sizeof(complex_t));
224  if(!cr)
225  {
226  err = ERROR_MALLOC;
227  goto exit_label;
228  }
229  cx = (complex_t*)malloc( nx * sizeof(complex_t));
230  if(!cx)
231  {
232  err = ERROR_MALLOC;
233  goto exit_label;
234  }
235  cy = (complex_t*)malloc( ny * sizeof(complex_t));
236  if(!cy)
237  {
238  err = ERROR_MALLOC;
239  goto exit_label;
240  }
241 
242  err = re2cmplx(x, nx, cx);
243  if(err != RES_OK)
244  goto exit_label;
245 
246  err = re2cmplx(y, ny, cy);
247 
248  if(err != RES_OK)
249  goto exit_label;
250 
251  err = xcorr_krn(cx, nx, cy, ny, &fft, flag, nr, cr, t);
252  if(err != RES_OK)
253  goto exit_label;
254 
255  err = cmplx2re(cr, 2*nr+1, r, NULL);
256 
257 exit_label:
258  if(cr)
259  free(cr);
260  if(cx)
261  free(cx);
262  if(cy)
263  free(cy);
264 
265  fft_free(&fft);
266  return err;
267 }
268 
269 
270 
271 
272 
273 #ifdef DOXYGEN_ENGLISH
274 
360 #endif
361 #ifdef DOXYGEN_RUSSIAN
362 
448 #endif
449 int DSPL_API xcorr_cmplx(complex_t* x, int nx, complex_t* y, int ny,
450  int flag, int nr, complex_t* r, double* t)
451 {
452  fft_t fft = {0};
453  int err;
454  err = xcorr_krn(x, nx, y, ny, &fft, flag, nr, r, t);
455  fft_free(&fft);
456  return err;
457 }
458 
459 
460 
461 
462 #ifdef DOXYGEN_ENGLISH
463 
464 #endif
465 #ifdef DOXYGEN_RUSSIAN
466 
467 #endif
468 int xcorr_get_lag_cmplx(complex_t* x, int nd, int nr, complex_t* r, double* t)
469 {
470  int i;
471  if(!x || !r)
472  return ERROR_PTR;
473  if(nd < 1 || nr < 1)
474  return ERROR_SIZE;
475 
476  if(nr < nd)
477  memcpy(r, x+nd-1-nr, (2*nr+1)*sizeof(complex_t));
478  else
479  {
480  memset(r, 0, (2*nr+1) * sizeof(complex_t));
481  memcpy(r + nr - nd + 1, x, (2*nd-1)*sizeof(complex_t));
482  }
483  if(t)
484  for(i = 0; i < 2*nr+1; i++)
485  t[i] = (double)i - (double)nr;
486  return RES_OK;
487 }
488 
489 
490 
491 
492 
493 #ifdef DOXYGEN_ENGLISH
494 
495 #endif
496 #ifdef DOXYGEN_RUSSIAN
497 
498 #endif
499 int xcorr_krn(complex_t* x, int nx, complex_t* y, int ny, fft_t* pfft,
500  int flag, int nr, complex_t* r, double* t)
501 {
502  complex_t *px = NULL;
503  complex_t *py = NULL;
504  complex_t *pc = NULL;
505  complex_t *pX = NULL;
506  complex_t *pY = NULL;
507  complex_t *pC = NULL;
508 
509  int nfft, ndata;
510  int err, i;
511 
512  if(!x || !y || !r)
513  return ERROR_PTR;
514  if(nx < 1 || ny < 1 || nr < 1)
515  return ERROR_SIZE;
516 
517  err = xcorr_fft_size(nx, ny, &nfft, &ndata);
518  if(err!= RES_OK)
519  goto exit_label;
520 
521  /* memory allocation */
522  px = (complex_t*)malloc(nfft * sizeof(complex_t));
523  if(!px)
524  {
525  err = ERROR_MALLOC;
526  goto exit_label;
527  }
528 
529  py = (complex_t*)malloc(nfft * sizeof(complex_t));
530  if(!py)
531  {
532  err = ERROR_MALLOC;
533  goto exit_label;
534  }
535 
536  pc = (complex_t*)malloc(nfft * sizeof(complex_t));
537  if(!pc)
538  {
539  err = ERROR_MALLOC;
540  goto exit_label;
541  }
542 
543  pX = (complex_t*)malloc(nfft * sizeof(complex_t));
544  if(!pX)
545  {
546  err = ERROR_MALLOC;
547  goto exit_label;
548  }
549 
550  pY = (complex_t*)malloc(nfft * sizeof(complex_t));
551  if(!pY)
552  {
553  err = ERROR_MALLOC;
554  goto exit_label;
555  }
556 
557  pC = (complex_t*)malloc(nfft * sizeof(complex_t));
558  if(!pC)
559  {
560  err = ERROR_MALLOC;
561  goto exit_label;
562  }
563 
564  memset(px, 0, nfft * sizeof(complex_t));
565  memset(py, 0, nfft * sizeof(complex_t));
566 
567  memcpy(px + ndata - 1, x, nx * sizeof(complex_t));
568  memcpy(py, y, ny * sizeof(complex_t));
569 
570  err = fft_cmplx(px, nfft, pfft, pX);
571  if(err!= RES_OK)
572  goto exit_label;
573 
574  err = fft_cmplx(py, nfft, pfft, pY);
575  if(err!= RES_OK)
576  goto exit_label;
577 
578  for(i = 0; i < nfft; i++)
579  {
580  RE(pC[i]) = CMCONJRE(pX[i], pY[i]);
581  IM(pC[i]) = CMCONJIM(pX[i], pY[i]);
582  }
583 
584  err = ifft_cmplx(pC, nfft, pfft, pc);
585  if(err!= RES_OK)
586  goto exit_label;
587 
588  err = xcorr_scale_cmplx(pc, ndata, flag);
589  if(err!= RES_OK)
590  goto exit_label;
591 
592  err = xcorr_get_lag_cmplx(pc, ndata, nr, r, t);
593 
594 exit_label:
595  if(px)
596  free(px);
597  if(py)
598  free(py);
599  if(pc)
600  free(pc);
601  if(pX)
602  free(pX);
603  if(pY)
604  free(pY);
605  if(pC)
606  free(pC);
607  return err;
608 }
609 
610 
611 
612 
613 #ifdef DOXYGEN_ENGLISH
614 /*******************************************************************************
615 Return FFT size for autocorrelation or cross correlation vector calculation
616 
617 Cross-correlation vector size is
618 N = 2 * nx - 1, if nx > ny;
619 N = 2 * ny - 1, if nx <= ny.
620 
621 If cross-correlation size N may not be efficient for FFT
622 then we can add zeros to get high-performance FFT size.
623 
624 For example if N = 1025, then we can add zeros to 2048-points FFT but this way
625 seems not so good because too much zeros.
626 
627 If we rewrite N = 2^L + D, then we can use
628 
629  NFFT = 2^L + 2^(L - P), here P = 0,1,2 or 3.
630 
631 So NFFT = 2^(L-P) * (2^P + 1). Then 2^(L-P) can use radix-2 FFT, and additional
632 composite multiplication if P = 0,1,2 or 3 equals
633 9, 5, 3 or 2, and we have high-performance FFT algorithms for its points.
634 If P = 4 then composite multiplier is (2^P + 1) = 17, has no good FFT.
635 *******************************************************************************/
636 #endif
637 #ifdef DOXYGEN_RUSSIAN
638 /*******************************************************************************
639 Возвращает размер FFT для расчета полного вектора автокорреляции
640 или кросскорреляции.
641 
642 Размер кросскорреляции равен
643 N = 2 * nx - 1, если nx > ny;
644 N = 2 * ny - 1, eсли nx <= ny.
645 
646 Посколку N может оказаться неудачным размером для FFT, то можно добить нулями
647 до удобной длины.
648 
649 Если например N = 1025, то добивать до длины 2048 не очень эффективно, потому
650 что много лишних нулей.
651 
652 Если мы рассмотрим N = 2^L + D, то целесообразно использовать
653 
654  NFFT = 2^L + 2^(L - P), где P = 0,1,2 или 3.
655 
656 Тогда NFFT = 2^(L-P) * (2^P + 1). Тогда 2^(L-P) реализуем как radix-2, а
657 дополнительный составной множитель при P = 0,1,2 или 3 равен соответсвенно
658 9, 5, 3 или 2, а для этих длин существуют хорошие процедуры.
659 При P = 4 составной множитель будет (2^P + 1) = 17, что не очень хорошо.
660 *******************************************************************************/
661 #endif
662 int xcorr_fft_size(int nx, int ny, int* pnfft, int* pndata)
663 {
664  int nfft, nfft2, r2, dnfft;
665 
666  if(nx < 1 || ny < 1)
667  return ERROR_SIZE;
668  if(!pnfft || !pndata)
669  return ERROR_PTR;
670 
671  if(nx > ny)
672  {
673  nfft = 2*nx - 1;
674  *pndata = nx;
675  }
676  else
677  {
678  nfft = 2*ny - 1;
679  *pndata = ny;
680  }
681  nfft2 = nfft;
682 
683  r2 = 0;
684  while(nfft2 >>= 1)
685  r2++;
686 
687  if(r2 > 3)
688  {
689  dnfft = 1 << (r2 - 3);
690  while(((1 << r2) + dnfft) < nfft)
691  dnfft <<= 1;
692  nfft = (1 << r2) + dnfft;
693  }
694 
695  *pnfft = nfft;
696  return RES_OK;
697 }
698 
699 
700 
701 
702 
703 #ifdef DOXYGEN_ENGLISH
704 
705 #endif
706 #ifdef DOXYGEN_RUSSIAN
707 
708 #endif
709 int xcorr_scale_cmplx(complex_t* x, int nd, int flag)
710 {
711  int i;
712  double w;
713  if(!x)
714  return ERROR_PTR;
715  if(nd < 1)
716  return ERROR_SIZE;
717 
718  switch(flag)
719  {
720  case DSPL_XCORR_NOSCALE:
721  break;
722  case DSPL_XCORR_BIASED:
723  for(i = 0; i < 2 * nd - 1; i++)
724  {
725  w = 1.0 / (double)nd;
726  RE(x[i]) *= w;
727  IM(x[i]) *= w;
728  }
729  break;
730  case DSPL_XCORR_UNBIASED:
731  for(i = 1; i < 2 * nd - 1; i++)
732  {
733  w = 1.0 / ((double)nd - fabs((double)(i - nd)));
734  RE(x[i-1]) *= w;
735  IM(x[i-1]) *= w;
736  }
737  break;
738  default:
739  return ERROR_XCORR_FLAG;
740  }
741  return RES_OK;
742 }
#define RE(x)
Макрос определяющий реальную часть комплексного числа.
Definition: dspl.h:359
#define ERROR_PTR
Ошибка указателя. Данная ошибка означает, что один из обязательных указателей (память под который дол...
Definition: dspl.h:549
#define ERROR_SIZE
Ошибка при передаче размера массива. Данная ошибка возникает когда помимо указателя на массив входных...
Definition: dspl.h:557
int fft_cmplx(complex_t *x, int n, fft_t *pfft, complex_t *y)
Быстрое преобразование Фурье комплексного сигнала
Definition: fft.c:645
int ifft_cmplx(complex_t *x, int n, fft_t *pfft, complex_t *y)
Обратное быстрое преобразование Фурье
Definition: fft.c:179
void fft_free(fft_t *pfft)
Очистить структуру fft_t алгоритма БПФ
Definition: fft.c:1009
int xcorr_cmplx(complex_t *x, int nx, complex_t *y, int ny, int flag, int nr, complex_t *r, double *t)
Оценка вектора взаимной корреляции для дискретных комплексных последовательностей x и y.
Definition: xcorr.c:449
int xcorr(double *x, int nx, double *y, int ny, int flag, int nr, double *r, double *t)
Оценка вектора взаимной корреляции для дискретных вещественных последовательностей x и y.
Definition: xcorr.c:214
Структура данных объекта быстрого преобразования Фурье
Definition: dspl.h:227
int fft(double *x, int n, fft_t *pfft, complex_t *y)
Быстрое преобразование Фурье вещественного сигнала
Definition: fft.c:356
int re2cmplx(double *x, int n, complex_t *y)
Преобразование массива вещественных данных в массив комплексных данных.
Definition: complex.c:246
int cmplx2re(complex_t *x, int n, double *re, double *im)
Преобразование массива комплексных данных в два массива вещественных данных, содержащих реальную и мн...
Definition: complex.c:130
double complex_t[2]
Описание комплексного типа данных.
Definition: dspl.h:86
#define RES_OK
Функция завершилась корректно. Ошибки отсутствуют.
Definition: dspl.h:497
#define IM(x)
Макрос определяющий мнимую часть комплексного числа.
Definition: dspl.h:417
#define ERROR_MALLOC
Ошибка динамического выделения памяти. Данная ошибка означает, что при динамическом выделении памяти ...
Definition: dspl.h:538