libdspl-2.0
Библиотека алгоритмов цифровой обработки сигналов
xcorr.c
1/*
2* Copyright (c) 2015-2022 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
27int xcorr_fft_size(int nx, int ny, int* pnfft, int* pndata);
28
29int xcorr_get_lag_cmplx(complex_t* x, int nd, int nr, complex_t* r, double* t);
30
31int 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
34int xcorr_scale_cmplx(complex_t* x, int nd, int flag);
35
36
37
38#ifdef DOXYGEN_ENGLISH
125#endif
126#ifdef DOXYGEN_RUSSIAN
213#endif
214int 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
257exit_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
360#endif
361#ifdef DOXYGEN_RUSSIAN
448#endif
449int 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
468int 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
499int 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
594exit_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/*******************************************************************************
615Return FFT size for autocorrelation or cross correlation vector calculation
616
617Cross-correlation vector size is
618N = 2 * nx - 1, if nx > ny;
619N = 2 * ny - 1, if nx <= ny.
620
621If cross-correlation size N may not be efficient for FFT
622then we can add zeros to get high-performance FFT size.
623
624For example if N = 1025, then we can add zeros to 2048-points FFT but this way
625seems not so good because too much zeros.
626
627If 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
631So NFFT = 2^(L-P) * (2^P + 1). Then 2^(L-P) can use radix-2 FFT, and additional
632composite multiplication if P = 0,1,2 or 3 equals
6339, 5, 3 or 2, and we have high-performance FFT algorithms for its points.
634If 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Размер кросскорреляции равен
643N = 2 * nx - 1, если nx > ny;
644N = 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 равен соответсвенно
6589, 5, 3 или 2, а для этих длин существуют хорошие процедуры.
659При P = 4 составной множитель будет (2^P + 1) = 17, что не очень хорошо.
660*******************************************************************************/
661#endif
662int 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
709int 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}
void fft_free(fft_t *pfft)
Очистить структуру fft_t алгоритма БПФ
Definition: fft_free.c:63
int fft_cmplx(complex_t *x, int n, fft_t *pfft, complex_t *y)
Быстрое преобразование Фурье комплексного сигнала
Definition: fft_cmplx.c:179
int ifft_cmplx(complex_t *x, int n, fft_t *pfft, complex_t *y)
Обратное быстрое преобразование Фурье
Definition: ifft_cmplx.c:179
int fft(double *x, int n, fft_t *pfft, complex_t *y)
Быстрое преобразование Фурье вещественного сигнала
Definition: fft.c:173
#define ERROR_MALLOC
Ошибка динамического выделения памяти. Данная ошибка означает, что при динамическом выделении памяти ...
Definition: dspl.h:599
#define RES_OK
Функция завершилась корректно. Ошибки отсутствуют.
Definition: dspl.h:558
#define ERROR_PTR
Ошибка указателя. Данная ошибка означает, что один из обязательных указателей (память под который дол...
Definition: dspl.h:610
#define ERROR_SIZE
Ошибка при передаче размера массива. Данная ошибка возникает когда помимо указателя на массив входных...
Definition: dspl.h:618
int xcorr(double *x, int nx, double *y, int ny, int flag, int nr, double *r, double *t)
Оценка вектора взаимной корреляции для дискретных вещественных последовательностей x и y.
Definition: xcorr.c:214
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 cmplx2re(complex_t *x, int n, double *re, double *im)
Преобразование массива комплексных данных в два массива вещественных данных, содержащих реальную и мн...
Definition: cmplx2re.c:130
int re2cmplx(double *x, int n, complex_t *y)
Преобразование массива вещественных данных в массив комплексных данных.
Definition: re2cmplx.c:120
#define RE(x)
Макрос определяющий реальную часть комплексного числа.
Definition: dspl.h:420
double complex_t[2]
Описание комплексного типа данных.
Definition: dspl.h:86
#define IM(x)
Макрос определяющий мнимую часть комплексного числа.
Definition: dspl.h:478
Структура данных объекта быстрого преобразования Фурье
Definition: dspl.h:278