diff --git a/include/Butterworth.h b/include/Butterworth.h index 1ee1361..b76d687 100644 --- a/include/Butterworth.h +++ b/include/Butterworth.h @@ -9,6 +9,9 @@ namespace fratio { // https://www.dsprelated.com/showarticle/1119.php template class Butterworth : public GenericFilter { +public: + T PI = static_cast(M_PI); + public: enum class Type { LowPass, @@ -27,14 +30,15 @@ private: void initialize(size_t order, T fc, T fs); void computeDigitalRep(); void updateCoeffSize(); - void transformFilter(); + std::complex generateAnalogPole(T fpw, size_t k); + std::vector> generateAnalogZeros(); + void scaleAmplitude(); private: Type m_type; size_t m_order; T m_fc; T m_fs; - std::vector> m_poles; }; } // namespace fratio diff --git a/include/Butterworth.inl b/include/Butterworth.inl index d80bc71..f97971d 100644 --- a/include/Butterworth.inl +++ b/include/Butterworth.inl @@ -38,7 +38,6 @@ void Butterworth::initialize(size_t order, T fc, T fs) m_order = order; m_fc = fc; m_fs = fs; - m_poles.resize(order); updateCoeffSize(); computeDigitalRep(); } @@ -46,38 +45,26 @@ void Butterworth::initialize(size_t order, T fc, T fs) template void Butterworth::computeDigitalRep() { - T pi = static_cast(M_PI); // Continuous pre-warped frequency - T fpw = (m_fs / pi) * std::tan(pi * m_fc / m_fs); - T scaleFactor = 2 * pi * fpw; - - auto thetaK = [pi, order = m_order](size_t k) -> T { - return (2 * k - 1) * pi / (2 * order); - }; + T fpw = (m_fs / PI) * std::tan(PI * m_fc / m_fs); // Compute poles - std::complex scalePole; + std::complex analogPole; + std::vector> poles(m_order); for (size_t k = 1; k <= m_order; ++k) { - scalePole = scaleFactor * std::complex(-std::sin(thetaK(k)), std::cos(thetaK(k))); - BilinearTransform>::SToZ(m_fs, scalePole, m_poles[k - 1]); + analogPole = generateAnalogPole(fpw, k); + BilinearTransform>::SToZ(m_fs, analogPole, poles[k - 1]); } - std::vector> numPoles(m_order, std::complex(-1)); - std::vector> a = VietaAlgo>::polyCoeffFromRoot(m_poles); - std::vector> b = VietaAlgo>::polyCoeffFromRoot(numPoles); - T norm = 0; - T sumB = 0; + std::vector> zeros = generateAnalogZeros(); + std::vector> a = VietaAlgo>::polyCoeffFromRoot(poles); + std::vector> b = VietaAlgo>::polyCoeffFromRoot(zeros); for (size_t i = 0; i < m_order + 1; ++i) { m_aCoeff[i] = a[i].real(); m_bCoeff[i] = b[i].real(); - norm += m_aCoeff[i]; - sumB += m_bCoeff[i]; } - norm /= sumB; - for (auto& b : m_bCoeff) - b *= norm; - + scaleAmplitude(); checkCoeff(m_aCoeff, m_bCoeff); } @@ -90,16 +77,69 @@ void Butterworth::updateCoeffSize() } template -void Butterworth::transformFilter() +std::complex Butterworth::generateAnalogPole(T fpw, size_t k) +{ + T scaleFactor = 2 * PI * fpw; + + auto thetaK = [pi = PI, order = m_order](size_t k) -> T { + return (2 * k - 1) * pi / (2 * order); + }; + + std::complex analogPole(-std::sin(thetaK(k)), std::cos(thetaK(k))); + switch (m_type) { + case Type::HighPass: + return scaleFactor / analogPole; + + case Type::LowPass: + default: + return scaleFactor * analogPole; + } +} + +template +std::vector> Butterworth::generateAnalogZeros() { switch (m_type) { case Type::HighPass: + return std::vector>(m_order, std::complex(1)); + + case Type::LowPass: + default: + return std::vector>(m_order, std::complex(-1)); + } +} + +template +void Butterworth::scaleAmplitude() +{ + T scale = 0; + T sumB = 0; + + switch (m_type) { + case Type::HighPass: + for (size_t i = 0; i < m_order + 1; ++i) { + if (i % 2 == 0) { + scale += m_aCoeff[i]; + sumB += m_bCoeff[i]; + } else { + scale -= m_aCoeff[i]; + sumB -= m_bCoeff[i]; + } + } break; case Type::LowPass: default: + for (size_t i = 0; i < m_order + 1; ++i) { + scale += m_aCoeff[i]; + sumB += m_bCoeff[i]; + } break; } + + scale /= sumB; + for (auto& b : m_bCoeff) + b *= scale; } } // namespace fratio \ No newline at end of file