300 lines
7.1 KiB
C++
300 lines
7.1 KiB
C++
/* #define DEBUGTRACE_ENABLED */
|
|
#include "lasp_avpowerspectra.h"
|
|
#include "debugtrace.hpp"
|
|
#include "lasp_mathtypes.h"
|
|
#include <stdexcept>
|
|
|
|
using rte = std::runtime_error;
|
|
|
|
PowerSpectra::PowerSpectra(const us nfft, const Window::WindowType w)
|
|
: PowerSpectra(Window::create(w, nfft)) {}
|
|
|
|
PowerSpectra::PowerSpectra(const vd &window)
|
|
: nfft(window.size()), _fft(nfft), _window(window)
|
|
{
|
|
|
|
d win_pow = arma::sum(window % window) / window.size();
|
|
|
|
/* Scale fft such that power is easily computed */
|
|
_scale_fac = 2.0 / (win_pow * (d)nfft * (d)nfft);
|
|
|
|
DEBUGTRACE_PRINT(nfft);
|
|
DEBUGTRACE_PRINT(win_pow);
|
|
}
|
|
|
|
ccube PowerSpectra::compute(const dmat &input)
|
|
{
|
|
|
|
/// Run very often. Silence this one.
|
|
/* DEBUGTRACE_ENTER; */
|
|
|
|
dmat input_tmp = input;
|
|
|
|
// Multiply each column of the inputs element-wise with the window.
|
|
input_tmp.each_col() %= _window;
|
|
|
|
cmat rfft = _fft.fft(input_tmp);
|
|
|
|
ccube output(rfft.n_rows, input.n_cols, input.n_cols);
|
|
for (us i = 0; i < input.n_cols; i++)
|
|
{
|
|
/// This one can be run in parallel without any problem. Note that it is
|
|
/// the inner loop that is run in parallel.
|
|
#pragma omp parallel for
|
|
for (us j = 0; j < input.n_cols; j++)
|
|
{
|
|
output.slice(j).col(i) =
|
|
_scale_fac * (rfft.col(i) % arma::conj(rfft.col(j)));
|
|
|
|
output.slice(j).col(i)(0) /= 2;
|
|
output.slice(j).col(i)(nfft / 2) /= 2;
|
|
}
|
|
}
|
|
return output;
|
|
}
|
|
|
|
AvPowerSpectra::AvPowerSpectra(const us nfft, const Window::WindowType w,
|
|
const d overlap_percentage,
|
|
const d time_constant_times_fs)
|
|
: _ps(nfft, w)
|
|
{
|
|
|
|
DEBUGTRACE_ENTER;
|
|
if (overlap_percentage >= 100 || overlap_percentage < 0)
|
|
{
|
|
throw rte("Overlap percentage should be >= 0 and < 100");
|
|
}
|
|
|
|
_overlap_keep = (nfft * overlap_percentage) / 100;
|
|
DEBUGTRACE_PRINT(_overlap_keep);
|
|
if (_overlap_keep >= nfft)
|
|
{
|
|
throw rte("Overlap is too high. Results in no jump. Please "
|
|
"choose a smaller overlap percentage or a higher nfft");
|
|
}
|
|
if (time_constant_times_fs < 0)
|
|
{
|
|
_mode = Mode::Averaging;
|
|
}
|
|
else if (time_constant_times_fs == 0)
|
|
{
|
|
_mode = Mode::Spectrogram;
|
|
}
|
|
else
|
|
{
|
|
_mode = Mode::Leaking;
|
|
_alpha = d_exp(-static_cast<d>((nfft - _overlap_keep) / time_constant_times_fs));
|
|
DEBUGTRACE_PRINT(_alpha);
|
|
}
|
|
}
|
|
void AvPowerSpectra::reset()
|
|
{
|
|
_timeBuf.reset();
|
|
_est.reset();
|
|
_n_averages = 0;
|
|
}
|
|
|
|
ccube AvPowerSpectra::compute(const dmat &timedata)
|
|
{
|
|
|
|
DEBUGTRACE_ENTER;
|
|
DEBUGTRACE_PRINT(timedata.n_rows);
|
|
DEBUGTRACE_PRINT(_ps.nfft);
|
|
|
|
_timeBuf.push(timedata);
|
|
|
|
bool run_once = false;
|
|
while (_timeBuf.size() >= _ps.nfft)
|
|
{
|
|
|
|
DEBUGTRACE_PRINT((int)_mode);
|
|
|
|
dmat samples = _timeBuf.pop(_ps.nfft, _overlap_keep);
|
|
|
|
switch (_mode)
|
|
{
|
|
case (Mode::Spectrogram):
|
|
DEBUGTRACE_PRINT("Spectrogram mode");
|
|
_est = _ps.compute(samples);
|
|
|
|
break;
|
|
case (Mode::Averaging):
|
|
DEBUGTRACE_PRINT("Averaging mode");
|
|
_n_averages++;
|
|
if (_n_averages == 1)
|
|
{
|
|
_est = _ps.compute(samples);
|
|
|
|
}
|
|
else
|
|
{
|
|
_est = (static_cast<d>(_n_averages - 1) / _n_averages) * _est +
|
|
_ps.compute(samples) / _n_averages;
|
|
}
|
|
break;
|
|
case (Mode::Leaking):
|
|
DEBUGTRACE_PRINT("Leaking mode");
|
|
if (arma::size(_est) == arma::size(0, 0, 0))
|
|
{
|
|
_est = _ps.compute(samples);
|
|
}
|
|
else
|
|
{
|
|
_est = _alpha * _est + (1 - _alpha) * _ps.compute(samples);
|
|
}
|
|
break;
|
|
} // end switch mode
|
|
run_once = true;
|
|
}
|
|
|
|
/// Return a copy of current estimator in case we have done one update.
|
|
/// Othewise, we return an empty ccube.
|
|
return run_once ? _est : ccube();
|
|
}
|
|
ccube AvPowerSpectra::get_est() const
|
|
{
|
|
return _est;
|
|
}
|
|
|
|
AvSweepPowerSpectra::AvSweepPowerSpectra(const us nfft, const Window::WindowType w,
|
|
const d overlap_percentage,
|
|
const d time_constant_times_fs,
|
|
const vd *A)
|
|
: _nfft(nfft), _ps(nfft, w)
|
|
{
|
|
|
|
DEBUGTRACE_ENTER;
|
|
if (overlap_percentage >= 100 || overlap_percentage < 0)
|
|
{
|
|
throw rte("Overlap percentage should be >= 0 and < 100");
|
|
}
|
|
if(A == nullptr) {
|
|
_A = vd(nfft, arma::fill::ones);
|
|
} else {
|
|
if(A->n_elem != (nfft / 2 + 1)) {
|
|
throw rte("Invalid length given for amplitude coefficients");
|
|
}
|
|
_A = arma::square(*A);
|
|
}
|
|
|
|
_overlap_keep = (nfft * overlap_percentage) / 100;
|
|
DEBUGTRACE_PRINT(_overlap_keep);
|
|
if (_overlap_keep >= nfft)
|
|
{
|
|
throw rte("Overlap is too high. Results in no jump. Please "
|
|
"choose a smaller overlap percentage or a higher nfft");
|
|
}
|
|
if (time_constant_times_fs < 0)
|
|
{
|
|
_mode = Mode::Averaging;
|
|
}
|
|
else if (time_constant_times_fs == 0)
|
|
{
|
|
_mode = Mode::Spectrogram;
|
|
}
|
|
else
|
|
{
|
|
_mode = Mode::Leaking;
|
|
_alpha = d_exp(-static_cast<d>((nfft - _overlap_keep) / time_constant_times_fs));
|
|
DEBUGTRACE_PRINT(_alpha);
|
|
}
|
|
}
|
|
void AvSweepPowerSpectra::reset()
|
|
{
|
|
_timeBuf.reset();
|
|
_est.reset();
|
|
_n_averages.reset();
|
|
}
|
|
|
|
ccube AvSweepPowerSpectra::compute(const dmat &timedata)
|
|
{
|
|
|
|
DEBUGTRACE_ENTER;
|
|
DEBUGTRACE_PRINT(timedata.n_rows);
|
|
DEBUGTRACE_PRINT(_ps.nfft);
|
|
|
|
_timeBuf.push(timedata);
|
|
|
|
bool run_once = false;
|
|
while (_timeBuf.size() >= _ps.nfft)
|
|
{
|
|
|
|
DEBUGTRACE_PRINT((int)_mode);
|
|
|
|
dmat samples = _timeBuf.pop(_ps.nfft, _overlap_keep);
|
|
|
|
us ncols = timedata.n_cols;
|
|
|
|
switch (_mode)
|
|
{
|
|
case (Mode::Spectrogram):
|
|
DEBUGTRACE_PRINT("Spectrogram mode");
|
|
_est = _ps.compute(samples);
|
|
break;
|
|
case (Mode::Averaging):
|
|
{
|
|
DEBUGTRACE_PRINT("Averaging mode");
|
|
|
|
ccube temp = _ps.compute(samples);
|
|
us nfreq = arma::size(temp)[0];
|
|
|
|
if (_est.size() == 0) {
|
|
// Initialize empty
|
|
_est = ccube(nfreq, ncols, ncols, arma::fill::zeros);
|
|
_n_averages = vd(nfreq, arma::fill::zeros);
|
|
}
|
|
|
|
vd corr_pow = arma::real(temp.slice(0).col(0)) / _A;
|
|
|
|
d threshold = pow(10.0, log10(arma::max(corr_pow)*arma::median(corr_pow))/2.0);
|
|
// d threshold = 1e-13;
|
|
|
|
for (us f=0; f < nfreq; f++)
|
|
{
|
|
if (real(temp(f, 0, 0)) / _A(f) > threshold)
|
|
{
|
|
_n_averages[f]++;
|
|
if (_n_averages[f] == 1)
|
|
{
|
|
_est.row(f) = temp.row(f);
|
|
}
|
|
else
|
|
{
|
|
_est.row(f) = (static_cast<d>(_n_averages[f] - 1) / _n_averages[f]) * _est.row(f) +
|
|
temp.row(f) / _n_averages[f];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
|
|
case (Mode::Leaking):
|
|
DEBUGTRACE_PRINT("Leaking mode");
|
|
if (arma::size(_est) == arma::size(0, 0, 0))
|
|
{
|
|
_est = _ps.compute(samples);
|
|
}
|
|
else
|
|
{
|
|
_est = _alpha * _est + (1 - _alpha) * _ps.compute(samples);
|
|
}
|
|
break;
|
|
} // end switch mode
|
|
run_once = true;
|
|
}
|
|
|
|
/// Return a copy of current estimator in case we have done one update.
|
|
/// Othewise, we return an empty ccube.
|
|
return run_once ? _est : ccube();
|
|
}
|
|
|
|
ccube AvSweepPowerSpectra::get_est() const
|
|
{
|
|
return _est;
|
|
}
|
|
|
|
vd AvSweepPowerSpectra::get_A() const
|
|
{
|
|
return _A;
|
|
}
|