From 141d792e3b8f1ff725e99d83cad164b84185856c Mon Sep 17 00:00:00 2001 From: Dsplib Date: Thu, 8 Oct 2020 19:36:29 +0300 Subject: [PATCH] added functions for power spectrum density estmation Changes to be committed: modified: dspl/ide/codeblocks/dspl.cbp modified: dspl/ide/codeblocks/dspl.layout modified: dspl/ide/codeblocks/dspl.workspace.layout modified: dspl/ide/codeblocks/examples.layout modified: dspl/src/complex.c modified: dspl/src/fft.c new file: dspl/src/psd.c new file: examples/src/psd_welch_test.c modified: include/dspl.c modified: include/dspl.h --- dspl/ide/codeblocks/dspl.cbp | 3 + dspl/ide/codeblocks/dspl.layout | 62 ++- dspl/ide/codeblocks/dspl.workspace.layout | 1 - dspl/ide/codeblocks/examples.layout | 10 +- dspl/src/complex.c | 2 +- dspl/src/fft.c | 241 ++++---- dspl/src/psd.c | 642 ++++++++++++++++++++++ examples/src/psd_welch_test.c | 84 +++ include/dspl.c | 10 + include/dspl.h | 63 +++ 10 files changed, 992 insertions(+), 126 deletions(-) create mode 100644 dspl/src/psd.c create mode 100644 examples/src/psd_welch_test.c diff --git a/dspl/ide/codeblocks/dspl.cbp b/dspl/ide/codeblocks/dspl.cbp index 5353c2f..670e4f3 100644 --- a/dspl/ide/codeblocks/dspl.cbp +++ b/dspl/ide/codeblocks/dspl.cbp @@ -141,6 +141,9 @@ + + diff --git a/dspl/ide/codeblocks/dspl.layout b/dspl/ide/codeblocks/dspl.layout index 5cb64c4..a72a810 100644 --- a/dspl/ide/codeblocks/dspl.layout +++ b/dspl/ide/codeblocks/dspl.layout @@ -2,27 +2,47 @@ - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -42,14 +62,4 @@ - - - - - - - - - - diff --git a/dspl/ide/codeblocks/dspl.workspace.layout b/dspl/ide/codeblocks/dspl.workspace.layout index 175213f..7797cd9 100644 --- a/dspl/ide/codeblocks/dspl.workspace.layout +++ b/dspl/ide/codeblocks/dspl.workspace.layout @@ -2,5 +2,4 @@ - diff --git a/dspl/ide/codeblocks/examples.layout b/dspl/ide/codeblocks/examples.layout index 996d8f2..8f3dc6c 100644 --- a/dspl/ide/codeblocks/examples.layout +++ b/dspl/ide/codeblocks/examples.layout @@ -2,14 +2,14 @@ + + + + + - - - - - diff --git a/dspl/src/complex.c b/dspl/src/complex.c index 4e83915..c6ef540 100644 --- a/dspl/src/complex.c +++ b/dspl/src/complex.c @@ -195,7 +195,7 @@ Vector `y` will keep: \endverbatim \author Sergey Bakhurin. www.dsplib.org -****************************************************************************** */ +***************************************************************************** */ #endif #ifdef DOXYGEN_RUSSIAN /*! **************************************************************************** diff --git a/dspl/src/fft.c b/dspl/src/fft.c index 259299c..2ee6bd7 100644 --- a/dspl/src/fft.c +++ b/dspl/src/fft.c @@ -178,35 +178,35 @@ Result: #endif int DSPL_API ifft_cmplx(complex_t *x, int n, fft_t* pfft, complex_t* y) { - int err, k; - double norm; + int err, k; + double norm; - if(!x || !pfft || !y) + if(!x || !pfft || !y) return ERROR_PTR; - if(n<1) + if(n<1) return ERROR_SIZE; - err = fft_create(pfft, n); - if(err != RES_OK) + err = fft_create(pfft, n); + if(err != RES_OK) return err; - memcpy(pfft->t1, x, n*sizeof(complex_t)); - for(k = 0; k < n; k++) + memcpy(pfft->t1, x, n*sizeof(complex_t)); + for(k = 0; k < n; k++) IM(pfft->t1[k]) = -IM(pfft->t1[k]); - err = fft_krn(pfft->t1, pfft->t0, pfft, n, 0); + err = fft_krn(pfft->t1, pfft->t0, pfft, n, 0); - if(err!=RES_OK) + if(err!=RES_OK) return err; - norm = 1.0 / (double)n; - for(k = 0; k < n; k++) - { + norm = 1.0 / (double)n; + for(k = 0; k < n; k++) + { RE(y[k]) = RE(pfft->t0[k])*norm; IM(y[k]) = -IM(pfft->t0[k])*norm; - } - return RES_OK; + } + return RES_OK; } @@ -374,6 +374,123 @@ int DSPL_API fft(double* x, int n, fft_t* pfft, complex_t* y) +#ifdef DOXYGEN_ENGLISH + +#endif +#ifdef DOXYGEN_RUSSIAN + +#endif +int DSPL_API fft_abs(double* x, int n, fft_t* pfft, + double fs, int flag, + double* mag, double* freq) +{ + int k, err = RES_OK; + complex_t *X = NULL; + if(!x || !pfft) + return ERROR_PTR; + + if(n<1) + return ERROR_SIZE; + + if(mag) + { + X = (complex_t*)malloc(n*sizeof(complex_t)); + err = fft(x, n, pfft, X); + if(err!=RES_OK) + goto error_proc; + + + for(k = 0; k < n; k++) + mag[k] = ABS(X[k]); + if(flag & DSPL_FLAG_FFT_SHIFT) + { + err = fft_shift(mag, n, mag); + if(err!=RES_OK) + goto error_proc; + } + } + + if(freq) + { + if(flag & DSPL_FLAG_FFT_SHIFT) + if(n%2) + err = linspace(-fs*0.5 + fs*0.5/(double)n, + fs*0.5 - fs*0.5/(double)n, + n, DSPL_SYMMETRIC, freq); + else + err = linspace(-fs*0.5, fs*0.5, n, DSPL_PERIODIC, freq); + else + err = linspace(0, fs, n, DSPL_PERIODIC, freq); + } + +error_proc: + if(X) + free(X); + + return err; +} + + + + +#ifdef DOXYGEN_ENGLISH + +#endif +#ifdef DOXYGEN_RUSSIAN + +#endif +int DSPL_API fft_abs_cmplx(complex_t* x, int n, fft_t* pfft, + double fs, int flag, + double* mag, double* freq) +{ + int k, err = RES_OK; + complex_t *X = NULL; + + if(!x || !pfft) + return ERROR_PTR; + + if(n<1) + return ERROR_SIZE; + + if(mag) + { + X = (complex_t*)malloc(n*sizeof(complex_t)); + err = fft_cmplx(x, n, pfft, X); + if(err!=RES_OK) + goto error_proc; + + + for(k = 0; k < n; k++) + mag[k] = ABS(X[k]); + + if(flag & DSPL_FLAG_FFT_SHIFT) + { + err = fft_shift(mag, n, mag); + if(err!=RES_OK) + goto error_proc; + } + } + + if(freq) + { + if(flag & DSPL_FLAG_FFT_SHIFT) + if(n%2) + err = linspace(-fs*0.5 + fs*0.5/(double)n, + fs*0.5 - fs*0.5/(double)n, + n, DSPL_SYMMETRIC, freq); + else + err = linspace(-fs*0.5, fs*0.5, n, DSPL_PERIODIC, freq); + else + err = linspace(0, fs, n, DSPL_PERIODIC, freq); + } +error_proc: + if(X) + free(X); + + return err; +} + + #ifdef DOXYGEN_ENGLISH /*! **************************************************************************** @@ -904,6 +1021,8 @@ void DSPL_API fft_free(fft_t *pfft) + + #ifdef DOXYGEN_ENGLISH #endif @@ -914,52 +1033,20 @@ int DSPL_API fft_mag(double* x, int n, fft_t* pfft, double fs, int flag, double* mag, double* freq) { - int k, err = RES_OK; - complex_t *X = NULL; - if(!x || !pfft) - return ERROR_PTR; - - if(n<1) - return ERROR_SIZE; - + int k, err; + err = fft_abs(x, n, pfft, fs, flag, mag, freq); + if(err != RES_OK) + return err; if(mag) - { - X = (complex_t*)malloc(n*sizeof(complex_t)); - err = fft(x, n, pfft, X); - if(err!=RES_OK) - goto error_proc; - + { if(flag & DSPL_FLAG_LOGMAG) for(k = 0; k < n; k++) - mag[k] = 10.0*log10(ABSSQR(X[k])+DBL_EPSILON); + mag[k] = 20.0 * log10(mag[k] + DBL_EPSILON); else for(k = 0; k < n; k++) - mag[k] = ABS(X[k]); - if(flag & DSPL_FLAG_FFT_SHIFT) - { - err = fft_shift(mag, n, mag); - if(err!=RES_OK) - goto error_proc; - } + mag[k] *= mag[k]; } - if(freq) - { - if(flag & DSPL_FLAG_FFT_SHIFT) - if(n%2) - err = linspace(-fs*0.5 + fs*0.5/(double)n, - fs*0.5 - fs*0.5/(double)n, - n, DSPL_SYMMETRIC, freq); - else - err = linspace(-fs*0.5, fs*0.5, n, DSPL_PERIODIC, freq); - else - err = linspace(0, fs, n, DSPL_PERIODIC, freq); - } - -error_proc: - if(X) - free(X); - return err; } @@ -979,52 +1066,20 @@ int DSPL_API fft_mag_cmplx(complex_t* x, int n, fft_t* pfft, double fs, int flag, double* mag, double* freq) { - int k, err = RES_OK; - complex_t *X = NULL; - - if(!x || !pfft) - return ERROR_PTR; - - if(n<1) - return ERROR_SIZE; - + int k, err; + err = fft_abs_cmplx(x, n, pfft, fs, flag, mag, freq); + if(err != RES_OK) + return err; if(mag) - { - X = (complex_t*)malloc(n*sizeof(complex_t)); - err = fft_cmplx(x, n, pfft, X); - if(err!=RES_OK) - goto error_proc; - + { if(flag & DSPL_FLAG_LOGMAG) for(k = 0; k < n; k++) - mag[k] = 10.0*log10(ABSSQR(X[k])); + mag[k] = 20.0 * log10(mag[k] + DBL_EPSILON); else for(k = 0; k < n; k++) - mag[k] = ABS(X[k]); - if(flag & DSPL_FLAG_FFT_SHIFT) - { - err = fft_shift(mag, n, mag); - if(err!=RES_OK) - goto error_proc; - } + mag[k] *= mag[k]; } - if(freq) - { - if(flag & DSPL_FLAG_FFT_SHIFT) - if(n%2) - err = linspace(-fs*0.5 + fs*0.5/(double)n, - fs*0.5 - fs*0.5/(double)n, - n, DSPL_SYMMETRIC, freq); - else - err = linspace(-fs*0.5, fs*0.5, n, DSPL_PERIODIC, freq); - else - err = linspace(0, fs, n, DSPL_PERIODIC, freq); - } -error_proc: - if(X) - free(X); - return err; } diff --git a/dspl/src/psd.c b/dspl/src/psd.c new file mode 100644 index 0000000..46fcfbc --- /dev/null +++ b/dspl/src/psd.c @@ -0,0 +1,642 @@ +/* +* Copyright (c) 2015-2019 Sergey Bakhurin +* Digital Signal Processing Library [http://dsplib.org] +* +* This file is part of libdspl-2.0. +* +* is free software: you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSPL is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with Foobar. If not, see . +*/ + +#include +#include +#include +#include +#include "dspl.h" + + + + + + +#ifdef DOXYGEN_ENGLISH + +#endif +#ifdef DOXYGEN_RUSSIAN + +#endif +int DSPL_API psd_bartlett(double* x, int n, int nfft, + fft_t* pfft, double fs, + int flag, double* ppsd, double* pfrq) +{ + int err, pos, k; + double *pdgr = NULL; + double *tmp = NULL; + fft_t *ptr_fft = NULL; + + pos = 0; + + pdgr = (double*)malloc(nfft * sizeof(double)); + if(!pdgr) + return ERROR_MALLOC; + + if(!pfft) + { + ptr_fft = (fft_t*)malloc(sizeof(fft_t)); + memset(ptr_fft, 0, sizeof(fft_t)); + } + else + ptr_fft = pfft; + + memset(ppsd, 0, nfft * sizeof(double)); + while(pos + nfft <= n) + { + err = fft_mag(x + pos, nfft, ptr_fft, fs, + flag & DSPL_FLAG_FFT_SHIFT, pdgr, NULL); + if(err != RES_OK) + goto exit_label; + for(k = 0; k < nfft; k++) + ppsd[k] += pdgr[k]; + pos += nfft; + } + + if(pos < n) + { + tmp = (double*)malloc(nfft * sizeof(double)); + if(!tmp) + { + err = ERROR_MALLOC; + goto exit_label; + } + memset(tmp ,0, nfft * sizeof(double)); + memcpy(tmp, x + pos, (n - pos)*sizeof(double)); + + err = fft_mag(tmp, nfft, ptr_fft, fs, + flag & DSPL_FLAG_FFT_SHIFT, pdgr, NULL); + if(err != RES_OK) + goto exit_label; + + for(k = 0; k < nfft; k++) + ppsd[k] += pdgr[k]; + } + + /* fill frequency */ + if(pfrq) + { + if(flag & DSPL_FLAG_FFT_SHIFT) + if(n%2) + err = linspace(-fs*0.5 + fs*0.5/(double)nfft, + fs*0.5 - fs*0.5/(double)nfft, + n, DSPL_SYMMETRIC, pfrq); + else + err = linspace(-fs*0.5, fs*0.5, nfft, DSPL_PERIODIC, pfrq); + else + err = linspace(0, fs, nfft, DSPL_PERIODIC, pfrq); + } + + /* scale magnitude */ + if(flag & DSPL_FLAG_LOGMAG) + { + for(k = 0; k < nfft; k++) + ppsd[k] = 10.0 * log10(ppsd[k] / (double)n / fs); + } + else + { + for(k = 0; k < nfft; k++) + ppsd[k] /= (double)n * fs; + } + + +exit_label: + if(pdgr) + free(pdgr); + if(tmp) + free(tmp); + if(ptr_fft && (ptr_fft != pfft)) + { + fft_free(ptr_fft); + free(ptr_fft); + } + return err; +} + + + + + +#ifdef DOXYGEN_ENGLISH + +#endif +#ifdef DOXYGEN_RUSSIAN + +#endif +int DSPL_API psd_periodogram(double* x, int n, + int win_type, double win_param, + fft_t* pfft, double fs, + int flag, double* ppsd, double* pfrq) +{ + double *w = NULL; + double *s = NULL; + double u, wn; + int err, k; + fft_t *ptr_fft = NULL; + + if(!x || !ppsd) + return ERROR_PTR; + + if(n<1 ) + return ERROR_SIZE; + + if(fs < 0.0) + return ERROR_FS; + + if(!pfft) + { + ptr_fft = (fft_t*)malloc(sizeof(fft_t)); + memset(ptr_fft, 0, sizeof(fft_t)); + } + else + ptr_fft = pfft; + + + if(win_type != DSPL_WIN_RECT) + { + /* Modified periodogram calculation */ + + /* window malloc */ + w = (double*)malloc(n*sizeof(double)); + if(!w) + { + err = ERROR_MALLOC; + goto exit_label; + } + + /* create window */ + err = window(w, n, win_type, win_param); + if(err != RES_OK) + goto exit_label; + + /* window normalization wn = sum(w.^2) */ + wn = 0; + for(k = 0; k < n; k++) + wn += w[k]*w[k]; + + /* signal buffer malloc */ + s = (double*)malloc(n*sizeof(double)); + if(!s) + { + err = ERROR_MALLOC; + goto exit_label; + } + + /* windowing */ + for(k = 0; k < n; k++) + s[k] = x[k] * w[k]; + } + else + { + /* classic periodogram without windowing */ + s = x; + wn = (double)n; + } + + /* calculate FFT */ + err = fft_mag(s, n, ptr_fft, fs, flag, ppsd, pfrq); + if(err != RES_OK) + goto exit_label; + + + if(flag & DSPL_FLAG_LOGMAG) + { + /* normalization in log scale */ + u = 10.0 * log10(wn * fs); + for(k = 0; k < n; k++) + ppsd[k] -= u; + } + else + { + /* normalization in linear scale */ + u = 1.0 / (wn * fs); + for(k = 0; k < n; k++) + ppsd[k] *= u; + } + +exit_label: + if(w) + free(w); + if(s && s != x) + free(s); + if(ptr_fft && (ptr_fft != pfft)) + { + fft_free(ptr_fft); + free(ptr_fft); + } + return err; +} + + + +#ifdef DOXYGEN_ENGLISH + +#endif +#ifdef DOXYGEN_RUSSIAN + +#endif +int DSPL_API psd_periodogram_cmplx(complex_t* x, int n, + int win_type, double win_param, + fft_t* pfft, double fs, + int flag, double* ppsd, double* pfrq) +{ + double *w = NULL; + complex_t *s = NULL; + double u, wn; + int err, k; + fft_t *ptr_fft = NULL; + + if(!x || !ppsd) + return ERROR_PTR; + + if(n<1 ) + return ERROR_SIZE; + + if(fs < 0.0) + return ERROR_FS; + + if(!pfft) + { + ptr_fft = (fft_t*)malloc(sizeof(fft_t)); + memset(ptr_fft, 0, sizeof(fft_t)); + } + else + ptr_fft = pfft; + + + if(win_type != DSPL_WIN_RECT) + { + /* Modified periodogram calculation */ + + /* window malloc */ + w = (double*)malloc(n*sizeof(double)); + if(!w) + { + err = ERROR_MALLOC; + goto exit_label; + } + + /* create window */ + err = window(w, n, win_type, win_param); + if(err != RES_OK) + goto exit_label; + + /* window normalization wn = sum(w.^2) */ + wn = 0; + for(k = 0; k < n; k++) + wn += w[k]*w[k]; + + /* signal buffer malloc */ + s = (complex_t*)malloc(n*sizeof(complex_t)); + if(!s) + { + err = ERROR_MALLOC; + goto exit_label; + } + + /* windowing */ + for(k = 0; k < n; k++) + { + RE(s[k]) = RE(x[k]) * w[k]; + IM(s[k]) = IM(x[k]) * w[k]; + } + } + else + { + /* classic periodogram without windowing */ + s = x; + wn = (double)n; + } + + /* calculate FFT */ + err = fft_mag_cmplx(s, n, ptr_fft, fs, flag, ppsd, pfrq); + if(err != RES_OK) + goto exit_label; + + + if(flag & DSPL_FLAG_LOGMAG) + { + /* normalization in log scale */ + u = 10.0 * log10(wn * fs); + for(k = 0; k < n; k++) + ppsd[k] -= u; + } + else + { + /* normalization in linear scale */ + u = 1.0 / (wn * fs); + for(k = 0; k < n; k++) + ppsd[k] *= u; + } + +exit_label: + if(w) + free(w); + if(s && s != x) + free(s); + if(ptr_fft && (ptr_fft != pfft)) + { + fft_free(ptr_fft); + free(ptr_fft); + } + return err; +} + + + + +#ifdef DOXYGEN_ENGLISH + +#endif +#ifdef DOXYGEN_RUSSIAN + +#endif +int DSPL_API psd_welch(double* x, int n, + int win_type, double win_param, + int npsd, int noverlap, fft_t* pfft, double fs, + int flag, double* ppsd, double* pfrq) +{ + double *win = NULL; + double wg; + complex_t *tmp = NULL; + double *s = NULL; + int err, k, pos, cnt; + fft_t *ptr_fft = NULL; + + if(!x || !ppsd) + return ERROR_PTR; + + if(n<1 || npsd < 1) + return ERROR_SIZE; + + if(noverlap < 1 || noverlap > npsd) + return ERROR_OVERLAP; + + if(fs < 0.0) + return ERROR_FS; + + win = (double*)malloc(npsd * sizeof(double)); + if(!win) + { + err = ERROR_MALLOC; + goto exit_label; + } + + if(!pfft) + { + ptr_fft = (fft_t*)malloc(sizeof(fft_t)); + memset(ptr_fft, 0, sizeof(fft_t)); + } + else + ptr_fft = pfft; + + + err = window(win, npsd, win_type, win_param); + if(err != RES_OK) + goto exit_label; + + wg = 0.0; + for(k = 0; k < npsd; k++) + wg += win[k] * win[k]; + wg = 1.0 / wg; + + + tmp = (complex_t*)malloc(npsd*sizeof(complex_t)); + if(!tmp) + { + err = ERROR_MALLOC; + goto exit_label; + } + + s = (double*)malloc(npsd*sizeof(double)); + if(!s) + { + err = ERROR_MALLOC; + goto exit_label; + } + + pos = 0; + cnt = 0; + memset(ppsd, 0, npsd*sizeof(double)); + while(pos+npsd <= n) + { + for(k = 0; k < npsd; k++) + s[k] = x[k+pos]*win[k]; + + err = fft(s, npsd, ptr_fft, tmp); + if(err != RES_OK) + goto exit_label; + + for(k = 0; k < npsd; k++) + ppsd[k] += wg * ABSSQR(tmp[k]); + + pos += noverlap; + cnt++; + + } + + for(k = 0; k < npsd; k++) + ppsd[k] /= (double)cnt * fs; + + if(flag & DSPL_FLAG_LOGMAG) + { + for(k = 0; k < npsd; k++) + ppsd[k] = 10.0 * log10(ppsd[k] + DBL_EPSILON); + } + + + + if(flag & DSPL_FLAG_PSD_TWOSIDED) + { + err = fft_shift(ppsd, npsd, ppsd); + if(err != RES_OK) + goto exit_label; + } + + if(pfrq) + { + if(flag & DSPL_FLAG_PSD_TWOSIDED) + { + err = linspace(-0.5*fs, fs*0.5, npsd, DSPL_PERIODIC, pfrq); + if(err != RES_OK) + goto exit_label; + } + else + { + err = linspace(0, fs, npsd, DSPL_PERIODIC, pfrq); + if(err != RES_OK) + goto exit_label; + } + } + + err = RES_OK; +exit_label: + if(win) + free(win); + if(tmp) + free(tmp); + if(s) + free(s); + if(ptr_fft && (ptr_fft != pfft)) + free(ptr_fft); + return err; +} + + + + + + + +#ifdef DOXYGEN_ENGLISH + +#endif +#ifdef DOXYGEN_RUSSIAN + +#endif +int DSPL_API psd_welch_cmplx(complex_t* x, int n, + int win_type, double win_param, + int npsd, int noverlap, fft_t* pfft, double fs, + int flag, double* ppsd, double* pfrq) +{ + double *win = NULL; + complex_t *tmp = NULL; + complex_t *s = NULL; + fft_t *ptr_fft = NULL; + + double wg; + int err, k, pos, cnt; + + if(!x || !ppsd) + return ERROR_PTR; + + if(n<1 || npsd < 1) + return ERROR_SIZE; + + if(noverlap < 1 || noverlap > npsd) + return ERROR_OVERLAP; + + if(fs < 0.0) + return ERROR_FS; + + win = (double*)malloc(npsd * sizeof(double)); + if(!win) + { + err = ERROR_MALLOC; + goto exit_label; + } + + if(!pfft) + { + ptr_fft = (fft_t*)malloc(sizeof(fft_t)); + memset(ptr_fft, 0, sizeof(fft_t)); + } + else + ptr_fft = pfft; + + err = window(win, npsd, win_type, win_param); + if(err != RES_OK) + goto exit_label; + + wg = 0.0; + for(k = 0; k < npsd; k++) + wg += win[k] * win[k]; + wg = 1.0 / wg; + + tmp = (complex_t*)malloc(npsd*sizeof(complex_t)); + if(!tmp) + { + err = ERROR_MALLOC; + goto exit_label; + } + + s = (complex_t*)malloc(npsd*sizeof(complex_t)); + if(!s) + { + err = ERROR_MALLOC; + goto exit_label; + } + + pos = 0; + cnt = 0; + memset(ppsd, 0, npsd*sizeof(double)); + while(pos+npsd <= n) + { + for(k = 0; k < npsd; k++) + { + RE(s[k]) = RE(x[k+pos])*win[k]; + IM(s[k]) = IM(x[k+pos])*win[k]; + } + err = fft_cmplx(s, npsd, ptr_fft, tmp); + if(err != RES_OK) + goto exit_label; + + for(k = 0; k < npsd; k++) + ppsd[k] += wg * ABSSQR(tmp[k]); + + pos += noverlap; + cnt++; + + } + + for(k = 0; k < npsd; k++) + ppsd[k] /= (double)cnt * fs; + + if(flag & DSPL_FLAG_LOGMAG) + { + for(k = 0; k < npsd; k++) + ppsd[k] = 10.0 * log10(ppsd[k] + DBL_EPSILON); + } + + if(flag & DSPL_FLAG_PSD_TWOSIDED) + { + err = fft_shift(ppsd, npsd, ppsd); + if(err != RES_OK) + goto exit_label; + } + + if(pfrq) + { + if(flag & DSPL_FLAG_PSD_TWOSIDED) + { + err = linspace(-0.5*fs, fs*0.5, npsd, DSPL_PERIODIC, pfrq); + if(err != RES_OK) + goto exit_label; + } + else + { + err = linspace(0, fs, npsd, DSPL_PERIODIC, pfrq); + if(err != RES_OK) + goto exit_label; + } + } + err = RES_OK; + +exit_label: + if(win) + free(win); + if(tmp) + free(tmp); + if(s) + free(s); + if(ptr_fft && (ptr_fft != pfft)) + free(ptr_fft); + return err; +} diff --git a/examples/src/psd_welch_test.c b/examples/src/psd_welch_test.c new file mode 100644 index 0000000..5137b6f --- /dev/null +++ b/examples/src/psd_welch_test.c @@ -0,0 +1,84 @@ +#include +#include +#include +#include "dspl.h" + + +#define N 8192 +#define NFFT 512 + +#define FS 0.01 + + + +int main(int argc, char* argv[]) +{ + void* hdspl; /* DSPL handle */ + void* hplot; /* GNUPLOT handle */ + hdspl = dspl_load(); // Load DSPL function + random_t rnd = {0}; /* random structure */ + + double *x = NULL; + double *psd = NULL; + double *frq = NULL; + + int k, err; + + x = (double*)malloc(N*sizeof(double)); + psd = (double*)malloc(N*sizeof(double)); + frq = (double*)malloc(N*sizeof(double)); + + /* input signal fill as noise -40 dB/Hz power spectrum density */ + random_init(&rnd, RAND_TYPE_MRG32K3A, NULL); + randn(x, N, 0.0, 0.1, &rnd); + + /* x[k] = 0.1*cos(2*pi*0.1*k) + cos(2*pi*0.2*k) + noise */ + for(k = 0; k < N; k++) + x[k] += cos(M_2PI * 0.26 * (double)k) + 0.1*cos(M_2PI*0.2* (double)k); + + /* Twosided PSD with logscale magnitude */ + err = psd_welch(x, N, DSPL_WIN_BLACKMAN , 0, NFFT, NFFT, NULL, FS, + DSPL_FLAG_LOGMAG | DSPL_FLAG_PSD_TWOSIDED, psd, frq); + + printf("error: 0x%.8x\n", err); + + // Save PSD to the "dat/psd_welch.txt" file + writetxt(frq, psd, NFFT, "dat/psd_welch.txt"); + + + /* Twosided PSD with logscale magnitude */ + err = psd_periodogram(x, N, DSPL_WIN_BLACKMAN, 0, NULL, FS, + DSPL_FLAG_LOGMAG | DSPL_FLAG_PSD_TWOSIDED, psd, frq); + printf("error: 0x%.8x\n", err); + writetxt(frq, psd, N, "dat/psd_periodogram.txt"); + + + /* Twosided PSD with logscale magnitude */ + err = psd_bartlett(x, N, NFFT, NULL, FS, + DSPL_FLAG_LOGMAG | DSPL_FLAG_PSD_TWOSIDED, psd, frq); + printf("error: 0x%.8x\n", err); + + // Save PSD to the "dat/psd_welch.txt" file + writetxt(frq, psd, NFFT, "dat/psd_barlett.txt"); + + /* plotting by GNUPLOT */ + gnuplot_create(argc, argv, 560, 420, "img/psd_welch.png", &hplot); + gnuplot_cmd(hplot, "unset key"); + gnuplot_cmd(hplot, "set grid"); + gnuplot_cmd(hplot, "set xlabel 'frequency'"); + gnuplot_cmd(hplot, "set ylabel 'PSD, dB'"); + + gnuplot_cmd(hplot, "plot 'dat/psd_periodogram.txt' w l lt 3,\\"); + gnuplot_cmd(hplot, " 'dat/psd_barlett.txt' w l lt 1,\\"); + gnuplot_cmd(hplot, " 'dat/psd_welch.txt' w l lt -1"); + + gnuplot_close(hplot); + + dspl_free(hdspl); // free dspl handle + if(x) + free(x); + + return 0; +} + + diff --git a/include/dspl.c b/include/dspl.c index d2e6ba9..29b9f8d 100644 --- a/include/dspl.c +++ b/include/dspl.c @@ -84,6 +84,8 @@ p_ellip_sn_cmplx ellip_sn_cmplx ; p_farrow_lagrange farrow_lagrange ; p_farrow_spline farrow_spline ; p_fft fft ; +p_fft_abs fft_abs ; +p_fft_abs_cmplx fft_abs_cmplx ; p_fft_cmplx fft_cmplx ; p_fft_create fft_create ; p_fft_free fft_free ; @@ -151,6 +153,10 @@ p_poly_z2a_cmplx poly_z2a_cmplx ; p_polyroots polyroots ; p_polyval polyval ; p_polyval_cmplx polyval_cmplx ; +p_psd_bartlett psd_bartlett ; +p_psd_periodogram psd_periodogram ; +p_psd_welch psd_welch ; +p_psd_welch_cmplx psd_welch_cmplx ; p_randb randb ; p_randb2 randb2 ; @@ -465,6 +471,10 @@ void* dspl_load() LOAD_FUNC(polyroots); LOAD_FUNC(polyval); LOAD_FUNC(polyval_cmplx); + LOAD_FUNC(psd_bartlett); + LOAD_FUNC(psd_periodogram); + LOAD_FUNC(psd_welch); + LOAD_FUNC(psd_welch_cmplx); LOAD_FUNC(randi); LOAD_FUNC(randb); diff --git a/include/dspl.h b/include/dspl.h index 890c9e2..507eb25 100644 --- a/include/dspl.h +++ b/include/dspl.h @@ -521,6 +521,7 @@ Variable `z = 1-2j`, here `j` - imaginary unit, but variable `y = 5`. #define ERROR_FNAME 0x06140113 #define ERROR_FOPEN 0x06151605 #define ERROR_FREAD_SIZE 0x06180501 +#define ERROR_FS 0x06190000 #define ERROR_FWRITE_SIZE 0x06231820 /* G 0x07xxxxxx*/ #define ERROR_GNUPLOT_CREATE 0x07161203 @@ -541,6 +542,7 @@ Variable `z = 1-2j`, here `j` - imaginary unit, but variable `y = 5`. #define ERROR_NAN 0x14011400 #define ERROR_NEGATIVE 0x14050701 /* O 0x15xxxxxx*/ +#define ERROR_OVERLAP 0x15220412 /* P 0x16xxxxxx*/ #define ERROR_POLY_AN 0x16150114 #define ERROR_POLY_ORD 0x16151518 @@ -582,6 +584,8 @@ Variable `z = 1-2j`, here `j` - imaginary unit, but variable `y = 5`. #define DSPL_FLAG_LOGMAG 0x00000002 #define DSPL_FLAG_UNWRAP 0x00000004 #define DSPL_FLAG_FFT_SHIFT 0x00000008 +#define DSPL_FLAG_PSD_TWOSIDED DSPL_FLAG_FFT_SHIFT + @@ -918,6 +922,22 @@ DECLARE_FUNC(int, fft, double* COMMA fft_t* COMMA complex_t* ); /*----------------------------------------------------------------------------*/ +DECLARE_FUNC(int, fft_abs, double* x + COMMA int n + COMMA fft_t* pfft + COMMA double fs + COMMA int flag + COMMA double* mag + COMMA double* freq); +/*----------------------------------------------------------------------------*/ +DECLARE_FUNC(int, fft_abs_cmplx, complex_t* x + COMMA int n + COMMA fft_t* pfft + COMMA double fs + COMMA int flag + COMMA double* mag + COMMA double* freq); +/*----------------------------------------------------------------------------*/ DECLARE_FUNC(int, fft_cmplx, complex_t* COMMA int COMMA fft_t* @@ -1269,6 +1289,49 @@ DECLARE_FUNC(int, polyval_cmplx, complex_t* COMMA int COMMA complex_t*); /*----------------------------------------------------------------------------*/ +DECLARE_FUNC(int, psd_bartlett, double* x + COMMA int n + COMMA int nfft + COMMA fft_t* pfft + COMMA double fs + COMMA int flag + COMMA double* ppsd + COMMA double* pfrq); +/*----------------------------------------------------------------------------*/ +DECLARE_FUNC(int, psd_periodogram, double* x + COMMA int n + COMMA int win_type + COMMA double win_param + COMMA fft_t* pfft + COMMA double fs + COMMA int flag + COMMA double* ppsd + COMMA double* pfrq); +/*----------------------------------------------------------------------------*/ +DECLARE_FUNC(int, psd_welch, double* x + COMMA int n + COMMA int win_type + COMMA double win_param + COMMA int npsd + COMMA int noverlap + COMMA fft_t* pfft + COMMA double fs + COMMA int flag + COMMA double* ppsd + COMMA double* pfrq); +/*----------------------------------------------------------------------------*/ +DECLARE_FUNC(int, psd_welch_cmplx, complex_t* x + COMMA int n + COMMA int win_type + COMMA double win_param + COMMA int npsd + COMMA int noverlap + COMMA fft_t* pfft + COMMA double fs + COMMA int flag + COMMA double* ppsd + COMMA double* pfrq); +/*----------------------------------------------------------------------------*/ DECLARE_FUNC(int, randb, double* x COMMA int n COMMA random_t* prnd);