lasp/beamforming/c/ps.c

182 lines
4.7 KiB
C
Raw Normal View History

2018-01-29 15:14:50 +00:00
// ps.c
//
// Author: J.A. de Jong -ASCEE
//
// Description:
//
//////////////////////////////////////////////////////////////////////
#define TRACERPLUS (-5)
2018-01-29 15:14:50 +00:00
#include "ps.h"
#include "fft.h"
#include "ascee_alloc.h"
#include "ascee_alg.h"
2018-01-29 15:14:50 +00:00
#include "ascee_assert.h"
typedef struct PowerSpectra_s {
vd window;
d win_pow; /**< The power of the window */
Fft* fft; /**< Fft routines storage */
} PowerSpectra;
PowerSpectra* PowerSpectra_alloc(const us nfft,
const us nchannels,
const WindowType wt) {
fsTRACE(15);
2018-01-29 15:14:50 +00:00
int rv;
/* Check nfft */
if(nfft % 2 != 0) {
WARN("nfft should be even");
return NULL;
}
/* ALlocate space */
Fft* fft = Fft_alloc(nfft);
2018-01-29 15:14:50 +00:00
if(fft == NULL) {
WARN("Fft allocation failed");
return NULL;
}
PowerSpectra* ps = a_malloc(sizeof(PowerSpectra));
if(!ps) {
WARN("Allocation of PowerSpectra memory failed");
Fft_free(fft);
return NULL;
}
ps->fft = fft;
/* Allocate vectors and matrices */
ps->window = vd_alloc(nfft);
rv = window_create(wt,&ps->window,&ps->win_pow);
check_overflow_vx(ps->window);
2018-01-29 15:14:50 +00:00
if(rv!=0) {
WARN("Error creating window function, continuing anyway");
}
feTRACE(15);
2018-01-29 15:14:50 +00:00
return ps;
}
void PowerSpectra_free(PowerSpectra* ps) {
fsTRACE(15);
2018-01-29 15:14:50 +00:00
Fft_free(ps->fft);
vd_free(&ps->window);
a_free(ps);
feTRACE(15);
2018-01-29 15:14:50 +00:00
}
void PowerSpectra_compute(const PowerSpectra* ps,
2018-01-29 15:14:50 +00:00
const dmat * timedata,
cmat * result) {
fsTRACE(15);
2018-01-29 15:14:50 +00:00
dbgassert(ps && timedata && result,NULLPTRDEREF);
const us nchannels = timedata->n_rows;
2018-01-29 15:14:50 +00:00
const us nfft = Fft_nfft(ps->fft);
uVARTRACE(15,nchannels);
const d win_pow = ps->win_pow;
dVARTRACE(15,win_pow);
/* Sanity checks for the matrices */
dbgassert(timedata->n_cols == nchannels,"timedata n_cols "
"should be equal to nchannels");
dbgassert(timedata->n_rows == nfft,"timedata n_rows "
"should be equal to nfft");
dbgassert(result->n_rows == nfft/2+1,"result n_rows "
"should be equal to nfft/2+1");
dbgassert(result->n_cols == nchannels*nchannels,"result n_cols "
"should be equal to nchannels*nchannels");
/* Multiply time data with the window and store result in
* timedata_work. */
dmat timedata_work = dmat_alloc(nfft,nchannels);
for(us i=0;i<nchannels;i++) {
vd column = dmat_column((dmat*) timedata,i);
vd column_work = dmat_column(&timedata_work,i);
vd_elem_prod(&column_work,&column,&ps->window);
vd_free(&column);
vd_free(&column_work);
2018-01-29 15:14:50 +00:00
}
check_overflow_xmat(timedata_work);
2018-01-29 15:14:50 +00:00
/* print_dmat(&timedata_work); */
/* Compute fft of the time data */
cmat fft_work = cmat_alloc(nfft/2+1,nchannels);
2018-01-29 15:14:50 +00:00
Fft_fft(ps->fft,
&timedata_work,
&fft_work);
dmat_free(&timedata_work);
TRACE(15,"fft done");
2018-01-29 15:14:50 +00:00
/* Scale fft such that power is easily comxputed */
const c scale_fac = d_sqrt(2/win_pow)/nfft;
cmat_scale(&fft_work,scale_fac);
TRACE(15,"scale done");
for(us i=0;i< nchannels;i++) {
2018-01-29 15:14:50 +00:00
/* Multiply DC term with 1/sqrt(2) */
*getcmatval(&fft_work,0,i) *= 1/d_sqrt(2.)+0*I;
/* Multiply Nyquist term with 1/sqrt(2) */
*getcmatval(&fft_work,nfft/2,i) *= 1/d_sqrt(2.)+0*I;
}
check_overflow_xmat(fft_work);
2018-01-29 15:14:50 +00:00
/* print_cmat(&fft_work); */
TRACE(15,"Nyquist and DC correction done");
2018-01-29 15:14:50 +00:00
vc j_vec_conj = vc_alloc(nfft/2+1);
2018-01-29 15:14:50 +00:00
/* Compute Cross-power spectra and store result */
for(us i =0; i<nchannels;i++) {
for(us j=0;j<nchannels;j++) {
2018-01-29 15:14:50 +00:00
/* The indices here are important. This is also how it
* is documented */
vc res = cmat_column(result,i+j*nchannels);
check_overflow_vx(res);
vc i_vec = cmat_column(&fft_work,i);
vc j_vec = cmat_column(&fft_work,j);
check_overflow_xmat(fft_work);
2018-01-29 15:14:50 +00:00
/* Compute the conjugate of spectra j */
vc_conj(&j_vec_conj,&j_vec);
check_overflow_xmat(fft_work);
/* Compute the element-wise product of the two vectors and
* store the result as the result */
vc_hadamard(&res,&i_vec,&j_vec_conj);
vc_free(&i_vec);
vc_free(&j_vec);
vc_free(&res);
2018-01-29 15:14:50 +00:00
}
}
check_overflow_xmat(*result);
check_overflow_xmat(*timedata);
cmat_free(&fft_work);
vc_free(&j_vec_conj);
feTRACE(15);
2018-01-29 15:14:50 +00:00
}
//////////////////////////////////////////////////////////////////////