diff --git a/src/lasp/dsp/lasp_biquadbank.cpp b/src/lasp/dsp/lasp_biquadbank.cpp index 115f6e3..09ec834 100644 --- a/src/lasp/dsp/lasp_biquadbank.cpp +++ b/src/lasp/dsp/lasp_biquadbank.cpp @@ -41,6 +41,45 @@ SeriesBiquad::SeriesBiquad(const vd &filter_coefs) { "Filter coefficients should have fourth element (a0) equal to 1.0"); } } + +SeriesBiquad SeriesBiquad::firstOrderHighPass(const d fs, const d cuton_Hz) { + + if(fs <= 0) { + throw rte("Invalid sampling frequency: " + std::to_string(fs) + " [Hz]"); + } + if(cuton_Hz <= 0) { + throw rte("Invalid cuton frequency: " + std::to_string(cuton_Hz) + " [Hz]"); + } + if(cuton_Hz >= 0.98*fs/2) { + throw rte("Invalid cuton frequency. We limit this to 0.98* fs / 2. Given value" + std::to_string(cuton_Hz) + " [Hz]"); + } + + const d tau = 1/(2*arma::datum::pi*cuton_Hz); + const d facnum = 2*fs*tau/(1+2*fs*tau); + const d facden = (1-2*fs*tau)/(1+2*fs*tau); + + vd coefs(6); + // b0 + coefs(0) = facnum; + // b1 + coefs(1) = -facnum; + // b2 + coefs(2) = 0; + + // a0 + coefs(3) = 1; + + // a1 + coefs(4) = facden; + + // a2 + coefs(5) = 0; + + return SeriesBiquad(coefs); + +} + + std::unique_ptr SeriesBiquad::clone() const { // sos.as_col() concatenates all columns, exactly what we want. return std::make_unique(sos.as_col()); diff --git a/src/lasp/dsp/lasp_biquadbank.h b/src/lasp/dsp/lasp_biquadbank.h index c37df12..1426cb5 100644 --- a/src/lasp/dsp/lasp_biquadbank.h +++ b/src/lasp/dsp/lasp_biquadbank.h @@ -39,6 +39,17 @@ public: virtual ~SeriesBiquad() override {} void reset() override final; std::unique_ptr clone() const override final; + + /** + * @brief Create a SeriesBiquad object for a first order high-pass filter + * + * @param fs Sampling frequency [Hz] + * @param cuton_Hz Cuton-frequency [Hz] + * + * @return SeriesBiquad object with a single biquad, corresponding to a first + * order high pass filter. + */ + static SeriesBiquad firstOrderHighPass(const d fs, const d cuton_Hz); }; /**