mirror
/
DiFipp
kopia lustrzana https://github.com/vsamy/DiFipp
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

Add BandReject

topic/diffentiators
Vincent Samy 2018-12-18 18:23:09 +09:00
rodzic 3ce8bb3a4f
commit 3ff2f09bdb
4 zmienionych plików z 93 dodań i 56 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

19
include/Butterworth.h
Wyświetl plik

@ -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 <typename T>
class Butterworth : public DigitalFilter<T> {
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<T> generateAnalogPole(int k, T fpw);
std::pair<std::complex<T>, std::complex<T>> generateBandAnalogPole(int k, T fpw1, T fpw2);
vectXc_t<T> generateAnalogZeros();
void scaleAmplitude(Eigen::Ref<vectX_t<T>> aCoeff, Eigen::Ref<vectX_t<T>> bCoeff);
std::pair<std::complex<T>, std::complex<T>> generateBandAnalogPole(int k, T fpw0, T bw);
vectXc_t<T> generateAnalogZeros(T f0 = T());
void scaleAmplitude(const vectX_t<T>& aCoeff, Eigen::Ref<vectX_t<T>> bCoeff, const std::complex<T>& bpS = T());
private:
Type m_type;
int m_order;
T m_fc;
T m_bw;
T m_fs;
T m_fCenter;
vectXc_t<T> m_poles;
};

96
include/Butterworth.tpp
Wyświetl plik

@ -13,41 +13,47 @@ template <typename T>
Butterworth<T>::Butterworth(int order, T fc, T fs, Type type)
: m_type(type)
{
initialize(order, fc, fs);
initialize(order, fc, 0, fs);
}
template <typename T>
Butterworth<T>::Butterworth(int order, T fc, T fs, T fCenter, Type type)
Butterworth<T>::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 <typename T>
void Butterworth<T>::setFilterParameters(int order, T fc, T fs, T fCenter)
void Butterworth<T>::setFilterParameters(int order, T fc, T fs)
{
initialize(order, fc, fs, fCenter);
initialize(order, fc, 0, fs);
}
template <typename T>
void Butterworth<T>::initialize(int order, T fc, T fs, T fCenter)
void Butterworth<T>::setFilterParameters(int order, T f0, T fLimit, T fs)
{
initialize(order, f0, fLimit, fs);
}
template <typename T>
void Butterworth<T>::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<T>::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<T>::initialize(int order, T fc, T fs, T fCenter)
template <typename T>
void Butterworth<T>::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<T> poles(m_order);
@ -92,24 +97,21 @@ void Butterworth<T>::computeDigitalRep(T fc)
}
template <typename T>
void Butterworth<T>::computeBandDigitalRep(T bw, T fCenter)
void Butterworth<T>::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<T> poles(2 * m_order);
std::pair<std::complex<T>, std::complex<T>> analogPoles;
for (int k = 0; k < m_order; ++k) {
analogPoles = generateBandAnalogPole(k + 1, fpw1, fpw2);
analogPoles = generateBandAnalogPole(k + 1, fpw0, fpw2 - fpw1);
BilinearTransform<std::complex<T>>::SToZ(m_fs, analogPoles.first, poles(2 * k));
BilinearTransform<std::complex<T>>::SToZ(m_fs, analogPoles.second, poles(2 * k + 1));
}
vectXc_t<T> zeros = generateAnalogZeros();
vectXc_t<T> zeros = generateAnalogZeros(fpw0);
vectXc_t<T> a = VietaAlgo<std::complex<T>>::polyCoeffFromRoot(poles);
vectXc_t<T> b = VietaAlgo<std::complex<T>>::polyCoeffFromRoot(zeros);
vectX_t<T> aCoeff(2 * m_order + 1);
@ -119,15 +121,11 @@ void Butterworth<T>::computeBandDigitalRep(T bw, T fCenter)
bCoeff(i) = b(i).real();
}
std::complex<T> s = std::exp(std::complex<T>(T(0), T(2) * PI * std::sqrt(f1 * f2) / m_fs));
std::complex<T> num(b(0));
std::complex<T> 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<T> 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>(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<T> Butterworth<T>::generateAnalogPole(int k, T fpw1)
}
template <typename T>
std::pair<std::complex<T>, std::complex<T>> Butterworth<T>::generateBandAnalogPole(int k, T fpw1, T fpw2)
std::pair<std::complex<T>, std::complex<T>> Butterworth<T>::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<T>, std::complex<T>> Butterworth<T>::generateBandAnalogPo
std::complex<T> analogPole(-std::sin(thetaK(k)), std::cos(thetaK(k)));
std::pair<std::complex<T>, std::complex<T>> poles;
std::complex<T> s0 = T(2) * PI * fpw0;
std::complex<T> s = T(0.5) * bw / fpw0;
switch (m_type) {
case Type::BandReject:
s /= analogPole;
poles.first = s0 * (s + std::complex<T>(T(0), T(1)) * std::sqrt(T(1) - s * s));
poles.second = s0 * (s - std::complex<T>(T(0), T(1)) * std::sqrt(T(1) - s * s));
return poles;
case Type::BandPass:
default: {
std::complex<T> fpw0 = std::sqrt(fpw1 * fpw2);
std::complex<T> s = T(0.5) * (fpw2 - fpw1) * analogPole / fpw0;
poles.first = T(2) * PI * fpw0 * (s + std::complex<T>(T(0), T(1)) * std::sqrt(std::complex<T>(T(1), T(0)) - s * s));
poles.second = T(2) * PI * fpw0 * (s - std::complex<T>(T(0), T(1)) * std::sqrt(std::complex<T>(T(1), T(0)) - s * s));
default:
s *= analogPole;
poles.first = s0 * (s + std::complex<T>(T(0), T(1)) * std::sqrt(T(1) - s * s));
poles.second = s0 * (s - std::complex<T>(T(0), T(1)) * std::sqrt(T(1) - s * s));
return poles;
}
}
}
template <typename T>
vectXc_t<T> Butterworth<T>::generateAnalogZeros()
vectXc_t<T> Butterworth<T>::generateAnalogZeros(T fpw0)
{
switch (m_type) {
case Type::HighPass:
return vectXc_t<T>::Constant(m_order, std::complex<T>(1));
case Type::BandPass:
return (vectXc_t<T>(2 * m_order) << vectXc_t<T>::Constant(m_order, std::complex<T>(-1)), vectXc_t<T>::Constant(m_order, std::complex<T>(1))).finished();
case Type::BandReject: {
T w0 = T(2) * std::atan2(PI * fpw0, m_fs); // 2 * atan2(fpw0, 4)??
return (vectXc_t<T>(2 * m_order) << vectXc_t<T>::Constant(m_order, std::exp(std::complex<T>(0, fpw0))), vectXc_t<T>::Constant(m_order, std::exp(std::complex<T>(0, -fpw0)))).finished();
}
case Type::LowPass:
default:
return vectXc_t<T>::Constant(m_order, std::complex<T>(-1));
@ -186,7 +191,7 @@ vectXc_t<T> Butterworth<T>::generateAnalogZeros()
}
template <typename T>
void Butterworth<T>::scaleAmplitude(Eigen::Ref<vectX_t<T>> aCoeff, Eigen::Ref<vectX_t<T>> bCoeff)
void Butterworth<T>::scaleAmplitude(const vectX_t<T>& aCoeff, Eigen::Ref<vectX_t<T>> bCoeff, const std::complex<T>& bpS)
{
T num = 0;
T denum = 0;
@ -203,6 +208,17 @@ void Butterworth<T>::scaleAmplitude(Eigen::Ref<vectX_t<T>> aCoeff, Eigen::Ref<ve
}
}
break;
case Type::BandPass: {
std::complex<T> numC(bCoeff(0));
std::complex<T> 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();

32
tests/ButterworthFilterTests.cpp
Wyświetl plik

@ -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<T> lpACoeffRes = (fratio::vectX_t<T>(6) << 1, -2.975422109745684, 3.806018119320413, -2.545252868330468, 0.881130075437837, -0.125430622155356).finished();
fratio::vectX_t<T> lpBCoeffRes = (fratio::vectX_t<T>(6) << 0.001282581078961, 0.006412905394803, 0.012825810789607, 0.012825810789607, 0.006412905394803, 0.001282581078961).finished();
@ -29,6 +31,10 @@ struct System {
fratio::vectX_t<T> bpACoeffRes = (fratio::vectX_t<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<T> bpBCoeffRes = (fratio::vectX_t<T>(11) << 0.001282581078963, 0, -0.006412905394817, 0, 0.012825810789633, 0, -0.012825810789633, 0, 0.006412905394817, 0, -0.001282581078963).finished();
fratio::vectX_t<T> bpResults = (fratio::vectX_t<T>(8) << 0.001282581078963, 0.011266576028733, 0.046195520115810, 0.116904647483408, 0.200574194600111, 0.232153315136604, 0.141350142008155, -0.086403129422609).finished();
// BR
fratio::vectX_t<T> brACoeffRes = (fratio::vectX_t<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<T> brBCoeffRes = (fratio::vectX_t<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<T> brResults = (fratio::vectX_t<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<double>)
BOOST_FIXTURE_TEST_CASE(BUTTERWORTH_BP_FILTER_FLOAT, System<float>)
{
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<float>::epsilon() * 1000);
test_results(bpResults, data, bf, std::numeric_limits<float>::epsilon() * 10000);
@ -75,8 +81,24 @@ BOOST_FIXTURE_TEST_CASE(BUTTERWORTH_BP_FILTER_FLOAT, System<float>)
BOOST_FIXTURE_TEST_CASE(BUTTERWORTH_BP_FILTER_DOUBLE, System<double>)
{
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<double>::epsilon() * 1000);
test_results(bpResults, data, bf, std::numeric_limits<double>::epsilon() * 10000);
}
}
BOOST_FIXTURE_TEST_CASE(BUTTERWORTH_BR_FILTER_FLOAT, System<float>)
{
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<float>::epsilon() * 10);
test_results(brResults, data, bf, std::numeric_limits<float>::epsilon() * 10);
}
BOOST_FIXTURE_TEST_CASE(BUTTERWORTH_BR_FILTER_DOUBLE, System<double>)
{
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<double>::epsilon() * 10);
test_results(brResults, data, bf, std::numeric_limits<double>::epsilon() * 10);
}

2
tests/GenericFilterTests.cpp
Wyświetl plik

@ -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);
}