diff --git a/cpp_src/dsp/lasp_avpowerspectra.cpp b/cpp_src/dsp/lasp_avpowerspectra.cpp index 100fd27..60b7cf1 100644 --- a/cpp_src/dsp/lasp_avpowerspectra.cpp +++ b/cpp_src/dsp/lasp_avpowerspectra.cpp @@ -10,7 +10,8 @@ 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) { + : nfft(window.size()), _fft(nfft), _window(window) +{ d win_pow = arma::sum(window % window) / window.size(); @@ -21,7 +22,8 @@ PowerSpectra::PowerSpectra(const vd &window) DEBUGTRACE_PRINT(win_pow); } -ccube PowerSpectra::compute(const dmat &input) { +ccube PowerSpectra::compute(const dmat &input) +{ /// Run very often. Silence this one. /* DEBUGTRACE_ENTER; */ @@ -34,11 +36,13 @@ ccube PowerSpectra::compute(const dmat &input) { 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++) { + 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++) { + for (us j = 0; j < input.n_cols; j++) + { output.slice(j).col(i) = _scale_fac * (rfft.col(i) % arma::conj(rfft.col(j))); @@ -52,37 +56,46 @@ ccube PowerSpectra::compute(const dmat &input) { AvPowerSpectra::AvPowerSpectra(const us nfft, const Window::WindowType w, const d overlap_percentage, const d time_constant_times_fs) - : _ps(nfft, w) { + : _ps(nfft, w) +{ DEBUGTRACE_ENTER; - if (overlap_percentage >= 100 || overlap_percentage < 0) { + 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) { + 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) { + if (time_constant_times_fs < 0) + { _mode = Mode::Averaging; - } else if (time_constant_times_fs == 0) { + } + else if (time_constant_times_fs == 0) + { _mode = Mode::Spectrogram; - } else { + } + else + { _mode = Mode::Leaking; - _alpha = d_exp(-static_cast((nfft - _overlap_keep)/time_constant_times_fs)); + _alpha = d_exp(-static_cast((nfft - _overlap_keep) / time_constant_times_fs)); DEBUGTRACE_PRINT(_alpha); } } -void AvPowerSpectra::reset() { +void AvPowerSpectra::reset() +{ _timeBuf.reset(); _est.reset(); - n_averages=0; - + _n_averages = 0; } -ccube AvPowerSpectra::compute(const dmat &timedata) { +ccube AvPowerSpectra::compute(const dmat &timedata) +{ DEBUGTRACE_ENTER; DEBUGTRACE_PRINT(timedata.n_rows); @@ -91,32 +104,42 @@ ccube AvPowerSpectra::compute(const dmat &timedata) { _timeBuf.push(timedata); bool run_once = false; - while (_timeBuf.size() >= _ps.nfft) { + while (_timeBuf.size() >= _ps.nfft) + { DEBUGTRACE_PRINT((int)_mode); dmat samples = _timeBuf.pop(_ps.nfft, _overlap_keep); - switch (_mode) { + 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) { + _n_averages++; + if (_n_averages == 1) + { _est = _ps.compute(samples); - } else { - _est = (static_cast(n_averages - 1) / n_averages) * _est + - _ps.compute(samples) / n_averages; + + } + else + { + _est = (static_cast(_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)) { + if (arma::size(_est) == arma::size(0, 0, 0)) + { _est = _ps.compute(samples); - } else { + } + else + { _est = _alpha * _est + (1 - _alpha) * _ps.compute(samples); } break; @@ -128,6 +151,135 @@ ccube AvPowerSpectra::compute(const dmat &timedata) { /// Othewise, we return an empty ccube. return run_once ? _est : ccube(); } -ccube AvPowerSpectra::get_est() const { +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) + : _nfft(nfft), _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((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); + } + + d threshold = arma::mean(arma::real(temp.slice(0).col(0))); + + for (us f=0; f < nfreq; f++) + { + if (real(temp(f, 0, 0)) > threshold) + { + _n_averages[f]++; + if (_n_averages[f] == 1) + { + _est.row(f) = temp.row(f); + // _est.subcube(f, 0, 0, f, ncols - 1, ncols - 1) = temp.subcube(f, 0, 0, f, ncols - 1, ncols - 1); + } + else + { + _est.row(f) = (static_cast(_n_averages[f] - 1) / _n_averages[f]) * _est.row(f) + + temp.row(f) / _n_averages[f]; + // _est.subcube(f, 0, 0, f, ncols - 1, ncols - 1) = (static_cast(_n_averages[f] - 1) / _n_averages[f]) * _est.subcube(f, 0, 0, f, ncols - 1, ncols - 1) + + // temp.subcube(f, 0, 0, f, ncols - 1, ncols - 1) / _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; } diff --git a/cpp_src/dsp/lasp_avpowerspectra.h b/cpp_src/dsp/lasp_avpowerspectra.h index a90e5b9..d972493 100644 --- a/cpp_src/dsp/lasp_avpowerspectra.h +++ b/cpp_src/dsp/lasp_avpowerspectra.h @@ -81,7 +81,7 @@ class AvPowerSpectra { Mode _mode; d _alpha; // Only valid in case of 'Leaking' - us n_averages = 0; // Only valid in case of 'Averaging' + us _n_averages = 0; // Only valid in case of 'Averaging' PowerSpectra _ps; /** @@ -165,3 +165,107 @@ public: us exactOverlapSamples() const { return _ps.nfft - _overlap_keep; } }; /** @} */ + + +/** + * @brief Estimate cross-power spectra using Welch' method of spectral + * estimation. The exact amount of overlap in Welch' method is rounded up to a + * certain amount of samples. This class is specifically for sweeps and will ignore + * fft blocks where the auto-power of the "reference" signal is below the average + * when in averaging mode. + */ +class AvSweepPowerSpectra { + + enum class Mode { + Averaging = 0, // Averaging all time date + Leaking = 1, // Exponential weighting of an "instantaneous cps" + Spectrogram = 2 // Instantenous spectrum, no averaging + }; + + Mode _mode; + d _alpha; // Only valid in case of 'Leaking' + us _nfft; + vd _n_averages { 1, arma::fill::zeros}; // Only valid in case of 'Averaging' + + PowerSpectra _ps; + /** + * @brief Current estimate of cross-spectral density + */ + ccube _est; + + /** + * @brief Buffer of storage of time data. + */ + TimeBuffer _timeBuf; + /** + * @brief The amount of samples to keep in the overlap + */ + us _overlap_keep; + +public: + /** + * @brief Initalize averaged power spectra computer. If a time constant is + * given > 0, it is used in a kind of exponential weighting. + * + * @param nfft The fft length + * @param w The window type. + * @param overlap_percentage A number 0 < overlap_percentage <= 100. It + * determines the amount of overlap used in Welch' method. A typical value is + * 50 %, i.e. 50. + * @param fs_tau Value should either be < 0, indicating that the + * estimate is averages over all time data. + * For a value = 0 the instantaneous power spectrum is returned, which can be + * interpreted as the spectrogram. For a value > 0 a exponential forgetting is + * used, where the value is used as the time constant such that the decay + * follows approximately the trend exp(-n/fs_tau), where n is the + * sample number in the power spectra. To choose 'fast' time weighting, set + * time_constant to the value of fs*0.125, where fs denotes the sampling + * frequency. + **/ + AvSweepPowerSpectra(const us nfft = 2048, + const Window::WindowType w = Window::WindowType::Hann, + const d overlap_percentage = 50., + const d fs_tau = -1); + + AvSweepPowerSpectra(const AvSweepPowerSpectra &) = delete; + AvSweepPowerSpectra &operator=(const AvSweepPowerSpectra &) = delete; + + /** + * @brief Reset to empty state. Clears the time buffer and sets estimator to + * empty. + */ + void reset(); + + /** + * @brief Compute an update of the power spectra based on given time data. + * Note that the number of channels is determined from the first time this + * function is called. If a later call has an incompatible number of + * channels, a runtime error is thrown. + * + * @param timedata + * + * @return a copy of the latest estimate of the power spectra. an + * update is only given if the amount of new time data is enough to compute a + * new estimate. if no new estimate is available, it returns an empty ccube. + * Note that the latest available estimate can be obtained using get_est(). + * */ + ccube compute(const dmat &timedata); + + /** + * @brief Returns the latest estimate of cps (cross-power spectra. + * + * @return a copy of the latest estimate of the power spectra. an + * update is only given if the amount of new time data is enough to compute a + * new estimate. If no estimate is available, it returns an empty ccube. + * */ + ccube get_est() const; + + /** + * @brief The overlap is rounded to a certain amount of time samples. This + * function returns that value. + * + * @return The amount of samples in overlapping. + */ + us exactOverlapSamples() const { return _ps.nfft - _overlap_keep; } +}; +/** @} */ diff --git a/cpp_src/pybind11/lasp_dsp_pybind.cpp b/cpp_src/pybind11/lasp_dsp_pybind.cpp index 906c6a4..d9bc9ca 100644 --- a/cpp_src/pybind11/lasp_dsp_pybind.cpp +++ b/cpp_src/pybind11/lasp_dsp_pybind.cpp @@ -129,6 +129,29 @@ void init_dsp(py::module &m) { return CubeToNpy(est); }); + /// AvSweepPowerSpectra + py::class_ asps(m, "AvSweepPowerSpectra"); + asps.def(py::init(), + py::arg("nfft") = 2048, + py::arg("windowType") = Window::WindowType::Hann, + py::arg("overlap_percentage") = 50.0, + py::arg("time_constant") = -1); + + asps.def("compute", [](AvSweepPowerSpectra &asps, dpyarray timedata) { + std::optional res; + dmat timedata_mat = NpyToMat(timedata); + { + py::gil_scoped_release release; + res = asps.compute(timedata_mat); + } + + return CubeToNpy(res.value_or(ccube(0, 0, 0))); + }); + asps.def("get_est", [](const AvSweepPowerSpectra &sps) { + ccube est = sps.get_est(); + return CubeToNpy(est); + }); + py::class_ slm(m, "cppSLM"); slm.def_static("fromBiquads", [](const d fs, const d Lref, const us ds, diff --git a/python_src/lasp/lasp_measurement.py b/python_src/lasp/lasp_measurement.py index 1843360..ab61e53 100644 --- a/python_src/lasp/lasp_measurement.py +++ b/python_src/lasp/lasp_measurement.py @@ -15,7 +15,7 @@ from scipy.io import wavfile import os, time, wave, logging from .lasp_common import SIQtys, Qty, getFreq from .lasp_version import LASP_VERSION_MAJOR, LASP_VERSION_MINOR -from .lasp_cpp import Window, DaqChannel, AvPowerSpectra +from .lasp_cpp import Window, DaqChannel, AvPowerSpectra, AvSweepPowerSpectra from typing import List from functools import lru_cache from .lasp_config import ones @@ -814,9 +814,11 @@ class Measurement: channels = list(range(self.nchannels)) nchannels = len(channels) - aps = AvPowerSpectra(nfft, window, overlap) - freq = getFreq(self.samplerate, nfft) + # aps = AvPowerSpectra(nfft, window, overlap) + aps = AvSweepPowerSpectra(nfft, window, overlap) + freq = getFreq(self.samplerate, nfft) + for data in self.iterData(channels, **kwargs): CS = aps.compute(data) @@ -1005,7 +1007,8 @@ class Measurement: # Convert range to [-1, 1] # TODO: this is wrong for float data where full scale > 1 - sensone = np.ones_like(self.sensitivity) + sensone = np.ones(data.shape[1]) + data = scaleBlockSens(data, sensone) if dtype == "int16" or dtype == "int32":