From 3ff2f09bdb9f0f55040058c11889c7759970572f Mon Sep 17 00:00:00 2001 From: Vincent Samy Date: Tue, 18 Dec 2018 18:23:09 +0900 Subject: [PATCH] Add BandReject --- include/Butterworth.h | 19 +++---- include/Butterworth.tpp | 96 +++++++++++++++++++------------- tests/ButterworthFilterTests.cpp | 32 +++++++++-- tests/GenericFilterTests.cpp | 2 +- 4 files changed, 93 insertions(+), 56 deletions(-) diff --git a/include/Butterworth.h b/include/Butterworth.h index 8b5595b..058be42 100644 --- a/include/Butterworth.h +++ b/include/Butterworth.h @@ -10,6 +10,7 @@ namespace fratio { // https://www.dsprelated.com/showarticle/1119.php // https://www.dsprelated.com/showarticle/1135.php // https://www.dsprelated.com/showarticle/1128.php +// https://www.dsprelated.com/showarticle/1131.php template class Butterworth : public DigitalFilter { public: @@ -28,26 +29,24 @@ public: public: Butterworth(Type type = Type::LowPass); Butterworth(int order, T fc, T fs, Type type = Type::LowPass); - Butterworth(int order, T bw, T fs, T fCenter, Type type = Type::BandPass); + Butterworth(int order, T fLower, T fUpper, T fs, Type type = Type::BandPass); - void setFilterParameters(int order, T fc, T fs, T fCenter = T(0)); + void setFilterParameters(int order, T fc, T fs); + void setFilterParameters(int order, T f0, T fLimit, T fs); private: - void initialize(int order, T fc, T fs, T fCenter = T(0)); // fc = bw for bandPass filter + void initialize(int order, T f0, T fLimit, T fs); // f0 = fc for LowPass/HighPass filter void computeDigitalRep(T fc); - void computeBandDigitalRep(T bw, T fCenter); + void computeBandDigitalRep(T fLower, T fUpper); std::complex generateAnalogPole(int k, T fpw); - std::pair, std::complex> generateBandAnalogPole(int k, T fpw1, T fpw2); - vectXc_t generateAnalogZeros(); - void scaleAmplitude(Eigen::Ref> aCoeff, Eigen::Ref> bCoeff); + std::pair, std::complex> generateBandAnalogPole(int k, T fpw0, T bw); + vectXc_t generateAnalogZeros(T f0 = T()); + void scaleAmplitude(const vectX_t& aCoeff, Eigen::Ref> bCoeff, const std::complex& bpS = T()); private: Type m_type; int m_order; - T m_fc; - T m_bw; T m_fs; - T m_fCenter; vectXc_t m_poles; }; diff --git a/include/Butterworth.tpp b/include/Butterworth.tpp index c9e32b6..e3347e8 100644 --- a/include/Butterworth.tpp +++ b/include/Butterworth.tpp @@ -13,41 +13,47 @@ template Butterworth::Butterworth(int order, T fc, T fs, Type type) : m_type(type) { - initialize(order, fc, fs); + initialize(order, fc, 0, fs); } template -Butterworth::Butterworth(int order, T fc, T fs, T fCenter, Type type) +Butterworth::Butterworth(int order, T fLower, T fUpper, T fs, Type type) : m_type(type) { - initialize(order, fc, fs, fCenter); + initialize(order, fLower, fUpper, fs); } template -void Butterworth::setFilterParameters(int order, T fc, T fs, T fCenter) +void Butterworth::setFilterParameters(int order, T fc, T fs) { - initialize(order, fc, fs, fCenter); + initialize(order, fc, 0, fs); } template -void Butterworth::initialize(int order, T fc, T fs, T fCenter) +void Butterworth::setFilterParameters(int order, T f0, T fLimit, T fs) +{ + initialize(order, f0, fLimit, fs); +} + +template +void Butterworth::initialize(int order, T f0, T fLimit, T fs) { if (order <= 0) { m_status = FilterStatus::BAD_ORDER_SIZE; return; } - if (fc <= 0 || fs <= 0) { + if (f0 <= 0 || fs <= 0) { m_status = FilterStatus::BAD_FREQUENCY_VALUE; return; } - if ((m_type == Type::BandPass || m_type == Type::BandReject) && fCenter - fc / 2. <= 0) { + if ((m_type == Type::BandPass || m_type == Type::BandReject) && f0 >= fLimit) { m_status = FilterStatus::BAD_BAND_FREQUENCY; return; } - if (m_fc > m_fs / 2.) { + if ((m_type == Type::LowPass || m_type == Type::HighPass) && f0 > fs / 2.) { m_status = FilterStatus::BAD_CUTOFF_FREQUENCY; return; } @@ -55,9 +61,9 @@ void Butterworth::initialize(int order, T fc, T fs, T fCenter) m_order = order; m_fs = fs; if (m_type == Type::LowPass || m_type == Type::HighPass) - computeDigitalRep(fc); + computeDigitalRep(f0); else - computeBandDigitalRep(fc, fCenter); // For band-like filters + computeBandDigitalRep(f0, fLimit); // For band-like filters resetFilter(); } @@ -65,9 +71,8 @@ void Butterworth::initialize(int order, T fc, T fs, T fCenter) template void Butterworth::computeDigitalRep(T fc) { - m_fc = fc; // Continuous pre-warped frequency - T fpw = (m_fs / PI) * std::tan(PI * m_fc / m_fs); + T fpw = (m_fs / PI) * std::tan(PI * fc / m_fs); // Compute poles vectXc_t poles(m_order); @@ -92,24 +97,21 @@ void Butterworth::computeDigitalRep(T fc) } template -void Butterworth::computeBandDigitalRep(T bw, T fCenter) +void Butterworth::computeBandDigitalRep(T fLower, T fUpper) { - m_bw = bw; - m_fCenter = fCenter; - T f1 = m_fCenter - m_bw / T(2); - T f2 = m_fCenter + m_bw / T(2); - T fpw1 = (m_fs / PI) * std::tan(PI * f1 / m_fs); - T fpw2 = (m_fs / PI) * std::tan(PI * f2 / m_fs); + T fpw1 = (m_fs / PI) * std::tan(PI * fLower / m_fs); + T fpw2 = (m_fs / PI) * std::tan(PI * fUpper / m_fs); + T fpw0 = std::sqrt(fpw1 * fpw2); vectXc_t poles(2 * m_order); std::pair, std::complex> analogPoles; for (int k = 0; k < m_order; ++k) { - analogPoles = generateBandAnalogPole(k + 1, fpw1, fpw2); + analogPoles = generateBandAnalogPole(k + 1, fpw0, fpw2 - fpw1); BilinearTransform>::SToZ(m_fs, analogPoles.first, poles(2 * k)); BilinearTransform>::SToZ(m_fs, analogPoles.second, poles(2 * k + 1)); } - vectXc_t zeros = generateAnalogZeros(); + vectXc_t zeros = generateAnalogZeros(fpw0); vectXc_t a = VietaAlgo>::polyCoeffFromRoot(poles); vectXc_t b = VietaAlgo>::polyCoeffFromRoot(zeros); vectX_t aCoeff(2 * m_order + 1); @@ -119,15 +121,11 @@ void Butterworth::computeBandDigitalRep(T bw, T fCenter) bCoeff(i) = b(i).real(); } - std::complex s = std::exp(std::complex(T(0), T(2) * PI * std::sqrt(f1 * f2) / m_fs)); - std::complex num(b(0)); - std::complex denum(a(0)); - for (int i = 1; i < 2 * m_order + 1; ++i) { - num = num * s + b(i); - denum = denum * s + a(i); - } - std::complex hf0 = num / denum; - bCoeff *= T(1) / std::abs(hf0); // bCoeff *= 1 / abs(num / denum) + if (m_type == Type::BandPass) + scaleAmplitude(aCoeff, bCoeff, std::exp(std::complex(T(0), T(2) * PI * std::sqrt(fLower * fUpper) / m_fs))); + else + scaleAmplitude(aCoeff, bCoeff); + setCoeffs(std::move(aCoeff), std::move(bCoeff)); } @@ -149,7 +147,7 @@ std::complex Butterworth::generateAnalogPole(int k, T fpw1) } template -std::pair, std::complex> Butterworth::generateBandAnalogPole(int k, T fpw1, T fpw2) +std::pair, std::complex> Butterworth::generateBandAnalogPole(int k, T fpw0, T bw) { auto thetaK = [pi = PI, order = m_order](int k) -> T { return (2 * k - 1) * pi / (2 * order); @@ -157,28 +155,35 @@ std::pair, std::complex> Butterworth::generateBandAnalogPo std::complex analogPole(-std::sin(thetaK(k)), std::cos(thetaK(k))); std::pair, std::complex> poles; + std::complex s0 = T(2) * PI * fpw0; + std::complex s = T(0.5) * bw / fpw0; switch (m_type) { case Type::BandReject: + s /= analogPole; + poles.first = s0 * (s + std::complex(T(0), T(1)) * std::sqrt(T(1) - s * s)); + poles.second = s0 * (s - std::complex(T(0), T(1)) * std::sqrt(T(1) - s * s)); return poles; case Type::BandPass: - default: { - std::complex fpw0 = std::sqrt(fpw1 * fpw2); - std::complex s = T(0.5) * (fpw2 - fpw1) * analogPole / fpw0; - poles.first = T(2) * PI * fpw0 * (s + std::complex(T(0), T(1)) * std::sqrt(std::complex(T(1), T(0)) - s * s)); - poles.second = T(2) * PI * fpw0 * (s - std::complex(T(0), T(1)) * std::sqrt(std::complex(T(1), T(0)) - s * s)); + default: + s *= analogPole; + poles.first = s0 * (s + std::complex(T(0), T(1)) * std::sqrt(T(1) - s * s)); + poles.second = s0 * (s - std::complex(T(0), T(1)) * std::sqrt(T(1) - s * s)); return poles; } - } } template -vectXc_t Butterworth::generateAnalogZeros() +vectXc_t Butterworth::generateAnalogZeros(T fpw0) { switch (m_type) { case Type::HighPass: return vectXc_t::Constant(m_order, std::complex(1)); case Type::BandPass: return (vectXc_t(2 * m_order) << vectXc_t::Constant(m_order, std::complex(-1)), vectXc_t::Constant(m_order, std::complex(1))).finished(); + case Type::BandReject: { + T w0 = T(2) * std::atan2(PI * fpw0, m_fs); // 2 * atan2(fpw0, 4)?? + return (vectXc_t(2 * m_order) << vectXc_t::Constant(m_order, std::exp(std::complex(0, fpw0))), vectXc_t::Constant(m_order, std::exp(std::complex(0, -fpw0)))).finished(); + } case Type::LowPass: default: return vectXc_t::Constant(m_order, std::complex(-1)); @@ -186,7 +191,7 @@ vectXc_t Butterworth::generateAnalogZeros() } template -void Butterworth::scaleAmplitude(Eigen::Ref> aCoeff, Eigen::Ref> bCoeff) +void Butterworth::scaleAmplitude(const vectX_t& aCoeff, Eigen::Ref> bCoeff, const std::complex& bpS) { T num = 0; T denum = 0; @@ -203,6 +208,17 @@ void Butterworth::scaleAmplitude(Eigen::Ref> aCoeff, Eigen::Ref numC(bCoeff(0)); + std::complex denumC(aCoeff(0)); + for (int i = 1; i < 2 * m_order + 1; ++i) { + denumC = denumC * bpS + aCoeff(i); + numC = numC * bpS + bCoeff(i); + } + num = std::abs(denumC); + denum = std::abs(numC); + } break; + case Type::BandReject: case Type::LowPass: default: num = aCoeff.sum(); diff --git a/tests/ButterworthFilterTests.cpp b/tests/ButterworthFilterTests.cpp index 286fda2..d6ed840 100644 --- a/tests/ButterworthFilterTests.cpp +++ b/tests/ButterworthFilterTests.cpp @@ -15,8 +15,10 @@ struct System { int order = 5; T fc = 10; T fs = 100; - T bw = 10; - T fCenter = 10; + T fLower = 5; + T fUpper = 15; + T f0 = 10; + T fU = 11; // LP fratio::vectX_t lpACoeffRes = (fratio::vectX_t(6) << 1, -2.975422109745684, 3.806018119320413, -2.545252868330468, 0.881130075437837, -0.125430622155356).finished(); fratio::vectX_t lpBCoeffRes = (fratio::vectX_t(6) << 0.001282581078961, 0.006412905394803, 0.012825810789607, 0.012825810789607, 0.006412905394803, 0.001282581078961).finished(); @@ -29,6 +31,10 @@ struct System { fratio::vectX_t bpACoeffRes = (fratio::vectX_t(11) << 1, -6.784299264603903, 21.577693329895588, -42.338550072279737, 56.729081385507655, -54.208087151300411, 37.399203252161037, -18.397491390111661, 6.180883710485754, -1.283022311577260, 0.125430622155356).finished(); fratio::vectX_t bpBCoeffRes = (fratio::vectX_t(11) << 0.001282581078963, 0, -0.006412905394817, 0, 0.012825810789633, 0, -0.012825810789633, 0, 0.006412905394817, 0, -0.001282581078963).finished(); fratio::vectX_t bpResults = (fratio::vectX_t(8) << 0.001282581078963, 0.011266576028733, 0.046195520115810, 0.116904647483408, 0.200574194600111, 0.232153315136604, 0.141350142008155, -0.086403129422609).finished(); + // BR + fratio::vectX_t brACoeffRes = (fratio::vectX_t(11) << 1.000000000000000, -6.784299264603897, 21.577693329895553, -42.338550072279631, 56.729081385507484, -54.208087151300205, 37.399203252160873, -18.397491390111572, 6.180883710485723, -1.283022311577253, 0.125430622155356).finished(); + fratio::vectX_t brBCoeffRes = (fratio::vectX_t(11) << 0.354164181088899, -3.012700469326103, 12.021845263663774, -29.490886190815772, 49.130136704563000, -58.004276868015168, 49.130136704563000, -29.490886190815772, 12.021845263663774, -3.012700469326103, 0.354164181088899).finished(); + fratio::vectX_t brResults = (fratio::vectX_t(8) << 0.354164181088899, 0.098383686162154, 0.084355149987331, 0.375555141082278, 0.735622022349639, 1.008089442365644, 1.229578363722674, 1.537000959441760).finished(); }; DISABLE_CONVERSION_WARNING_END @@ -67,7 +73,7 @@ BOOST_FIXTURE_TEST_CASE(BUTTERWORTH_HP_FILTER_DOUBLE, System) BOOST_FIXTURE_TEST_CASE(BUTTERWORTH_BP_FILTER_FLOAT, System) { - auto bf = fratio::Butterworthf(order, bw, fs, fCenter); + auto bf = fratio::Butterworthf(order, fLower, fUpper, fs); BOOST_REQUIRE_EQUAL(bf.aOrder(), bf.bOrder()); test_coeffs(bpACoeffRes, bpBCoeffRes, bf, std::numeric_limits::epsilon() * 1000); test_results(bpResults, data, bf, std::numeric_limits::epsilon() * 10000); @@ -75,8 +81,24 @@ BOOST_FIXTURE_TEST_CASE(BUTTERWORTH_BP_FILTER_FLOAT, System) BOOST_FIXTURE_TEST_CASE(BUTTERWORTH_BP_FILTER_DOUBLE, System) { - auto bf = fratio::Butterworthd(order, bw, fs, fCenter); + auto bf = fratio::Butterworthd(order, fLower, fUpper, fs); BOOST_REQUIRE_EQUAL(bf.aOrder(), bf.bOrder()); test_coeffs(bpACoeffRes, bpBCoeffRes, bf, std::numeric_limits::epsilon() * 1000); test_results(bpResults, data, bf, std::numeric_limits::epsilon() * 10000); -} \ No newline at end of file +} + +BOOST_FIXTURE_TEST_CASE(BUTTERWORTH_BR_FILTER_FLOAT, System) +{ + auto bf = fratio::Butterworthf(order, fLower, fUpper, fs, fratio::Butterworthf::Type::BandReject); + BOOST_REQUIRE_EQUAL(bf.aOrder(), bf.bOrder()); + test_coeffs(brACoeffRes, brBCoeffRes, bf, std::numeric_limits::epsilon() * 10); + test_results(brResults, data, bf, std::numeric_limits::epsilon() * 10); +} + +BOOST_FIXTURE_TEST_CASE(BUTTERWORTH_BR_FILTER_DOUBLE, System) +{ + auto bf = fratio::Butterworthd(order, fLower, fUpper, fs, fratio::Butterworthd::Type::BandReject); + BOOST_REQUIRE_EQUAL(bf.aOrder(), bf.bOrder()); + test_coeffs(brACoeffRes, brBCoeffRes, bf, std::numeric_limits::epsilon() * 10); + test_results(brResults, data, bf, std::numeric_limits::epsilon() * 10); +} diff --git a/tests/GenericFilterTests.cpp b/tests/GenericFilterTests.cpp index d8d97ca..f14ccb6 100644 --- a/tests/GenericFilterTests.cpp +++ b/tests/GenericFilterTests.cpp @@ -22,6 +22,6 @@ BOOST_AUTO_TEST_CASE(FILTER_FAILURES) BOOST_REQUIRE(mad.status() == fratio::FilterStatus::BAD_ORDER_SIZE); auto bfd = fratio::Butterworthd(0, 10, 100); BOOST_REQUIRE(bfd.status() == fratio::FilterStatus::BAD_ORDER_SIZE); - bfd = fratio::Butterworthd(5, 10, 100, 3); + bfd = fratio::Butterworthd(5, 5, 6, 100); BOOST_REQUIRE(bfd.status() == fratio::FilterStatus::BAD_BAND_FREQUENCY); }