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