From 60c0b5c11f51a9855e03540ed13dca71526c1a5c Mon Sep 17 00:00:00 2001 From: Vincent Samy Date: Tue, 29 Oct 2019 15:03:13 +0900 Subject: [PATCH] Fix some stuffs. --- include/BaseFilter.h | 5 +- include/BaseFilter.tpp | 34 ++++++------- include/GenericFilter.h | 17 ++++--- include/GenericFilter.tpp | 11 +++-- include/differentiators.h | 85 +++++++++++++++++++++++++------- include/{math.h => math_utils.h} | 0 6 files changed, 103 insertions(+), 49 deletions(-) rename include/{math.h => math_utils.h} (100%) diff --git a/include/BaseFilter.h b/include/BaseFilter.h index 9d398ff..fae084e 100644 --- a/include/BaseFilter.h +++ b/include/BaseFilter.h @@ -30,7 +30,6 @@ #include "gsl/gsl_assert.h" #include "type_checks.h" #include "typedefs.h" -#include #include namespace difi { @@ -58,7 +57,7 @@ public: void resetFilter() noexcept { derived().resetFilter(); }; /*!< \brief Return the filter type */ - FilterType type() const noexcept { return (m_center == 0 ? Type::Forward : Type::Centered); } + FilterType type() const noexcept { return (m_center == 0 ? FilterType::Forward : FilterType::Centered); } /*! \brief Get digital filter coefficients. * * It will automatically resize the given vectors. @@ -101,7 +100,7 @@ protected: * \param bCoeff Numerator coefficients of the filter. * \return True if the filter status is set on READY. */ - bool checkCoeffs(const vectX_t& aCoeff, const vectX_t& bCoeff, Type type); + bool checkCoeffs(const vectX_t& aCoeff, const vectX_t& bCoeff, FilterType type); private: /*! \brief Default uninitialized constructor. */ diff --git a/include/BaseFilter.tpp b/include/BaseFilter.tpp index 35d9b2a..5872077 100644 --- a/include/BaseFilter.tpp +++ b/include/BaseFilter.tpp @@ -31,20 +31,20 @@ namespace difi { // Public functions -template -void BaseFilter::setType(Type type) +template +void BaseFilter::setType(FilterType type) { - Expects(type == Type::Centered ? m_bCoeff.size() > 2 && m_bCoeff.size() % 2 == 1 : true); - m_center = (type == Type::Forward ? 0 : (m_bCoeff.size() - 1) / 2); + Expects(type == FilterType::Centered ? m_bCoeff.size() > 2 && m_bCoeff.size() % 2 == 1 : true); + m_center = (type == FilterType::Forward ? 0 : (m_bCoeff.size() - 1) / 2); } -template +template template -void BaseFilter::setCoeffs(T2&& aCoeff, T2&& bCoeff) +void BaseFilter::setCoeffs(T2&& aCoeff, T2&& bCoeff) { static_assert(std::is_convertible_v>, "The coefficients types should be convertible to vectX_t"); - Expects(checkCoeffs(aCoeff, bCoeff, (m_center == 0 ? Type::Forward : Type::Centered))); + Expects(checkCoeffs(aCoeff, bCoeff, (m_center == 0 ? FilterType::Forward : FilterType::Centered))); m_aCoeff = aCoeff; m_bCoeff = bCoeff; normalizeCoeffs(); @@ -52,8 +52,8 @@ void BaseFilter::setCoeffs(T2&& aCoeff, T2&& bCoeff) m_isInitialized = true; } -template -void BaseFilter::getCoeffs(vectX_t& aCoeff, vectX_t& bCoeff) const noexcept +template +void BaseFilter::getCoeffs(vectX_t& aCoeff, vectX_t& bCoeff) const noexcept { aCoeff = m_aCoeff; bCoeff = m_bCoeff; @@ -61,22 +61,22 @@ void BaseFilter::getCoeffs(vectX_t& aCoeff, vectX_t& bCoeff) const noex // Protected functions -template -BaseFilter::BaseFilter(const vectX_t& aCoeff, const vectX_t& bCoeff, FilterType type) +template +BaseFilter::BaseFilter(const vectX_t& aCoeff, const vectX_t& bCoeff, FilterType type) : m_aCoeff(aCoeff) , m_bCoeff(bCoeff) , m_filteredData(aCoeff.size()) , m_rawData(bCoeff.size()) { Expects(checkCoeffs(aCoeff, bCoeff, type)); - m_center = (type == Type::Forward ? 0 : (bCoeff.size() - 1) / 2); + m_center = (type == FilterType::Forward ? 0 : (bCoeff.size() - 1) / 2); normalizeCoeffs(); resetFilter(); m_isInitialized = true; } -template -void BaseFilter::normalizeCoeffs() +template +void BaseFilter::normalizeCoeffs() { T a0 = m_aCoeff(0); if (std::abs(a0 - T(1)) < std::numeric_limits::epsilon()) @@ -86,10 +86,10 @@ void BaseFilter::normalizeCoeffs() m_bCoeff /= a0; } -template -bool BaseFilter::checkCoeffs(const vectX_t& aCoeff, const vectX_t& bCoeff, FilterType type) +template +bool BaseFilter::checkCoeffs(const vectX_t& aCoeff, const vectX_t& bCoeff, FilterType type) { - bool centering = (type == Type::Centered ? (bCoeff.size() % 2 == 1) : true); + bool centering = (type == FilterType::Centered ? (bCoeff.size() % 2 == 1) : true); return aCoeff.size() > 0 && std::abs(aCoeff[0]) > std::numeric_limits::epsilon() && bCoeff.size() > 0 && centering; } diff --git a/include/GenericFilter.h b/include/GenericFilter.h index 9eb9566..8653d6e 100644 --- a/include/GenericFilter.h +++ b/include/GenericFilter.h @@ -32,7 +32,7 @@ namespace difi { template -class GenericFilter : BaseFilter> { +class GenericFilter : public BaseFilter> { public: /*! \brief Filter a new data. * @@ -49,17 +49,17 @@ public: */ vectX_t filter(const vectX_t& data); - void resetFilter() noexcept + void resetFilter() noexcept; protected: GenericFilter() = default; - GenericFilter(const vectX_t& aCoeff, const vectX_t& bCoeff, Type type = Type::Forward) + GenericFilter(const vectX_t& aCoeff, const vectX_t& bCoeff, FilterType type = FilterType::Forward) : BaseFilter(aCoeff, bCoeff, type) {} }; template -class TVGenericFilter : BaseFilter> { +class TVGenericFilter : public BaseFilter> { public: /*! \brief Filter a new data. * @@ -76,16 +76,21 @@ public: */ vectX_t filter(const vectX_t& data, const vectX_t& time); + void resetFilter() noexcept; + protected: - GenericFilter() = default; - GenericFilter(const vectX_t& aCoeff, const vectX_t& bCoeff, Type type = Type::Forward) + TVGenericFilter() = default; + TVGenericFilter(int order, const vectX_t& aCoeff, const vectX_t& bCoeff, FilterType type = FilterType::Forward) : BaseFilter(aCoeff, bCoeff, type) + , m_order(order) { + Expects(order >= 1); m_timers.resize(m_bCoeffs.size()); m_timeDiffs.resize(m_bCoeffs.size()); } private: + int m_order = 1; vectX_t m_timers; vectX_t m_timeDiffs; }; diff --git a/include/GenericFilter.tpp b/include/GenericFilter.tpp index 0e5b936..d843bca 100644 --- a/include/GenericFilter.tpp +++ b/include/GenericFilter.tpp @@ -78,13 +78,16 @@ T TVGenericFilter::stepFilter(const T& data, const T& time) if (m_center == 0) { for (Eigen::Index i = m_rawData.size() - 1; i > 0; --i) m_timeDiffs(i) = m_timeDiffs(i - 1); - m_timeDiffs(0) = time - m_timers(1); + m_timeDiffs(0) = std::pow(time - m_timers(1), m_order); } else { const Eigen::Index S = data.size() - 1; const Eigen::Index M = S / 2; m_timeDiffs(M) = T(1); - for (Eigen::Index i = 0; i < M; ++i) - m_timeDiffs(i) = m_timers(i) - m_timers(S - i); + for (Eigen::Index i = 1; i < M; ++i) { + const T diff = std::pow(m_timers(M - i) - m_timers(M + i), m_order); + m_timeDiffs(M + i) = diff; + m_timeDiffs(M - i) = diff; + } } m_rawData(0) = data; m_filteredData(0) = 0; @@ -104,7 +107,7 @@ vectX_t TVGenericFilter::filter(const vectX_t& data, const vectX_t& } template -void GenericFilter::resetFilter() noexcept +void TVGenericFilter::resetFilter() noexcept { m_filteredData.setZero(m_aCoeff.size()); m_rawData.setZero(m_bCoeff.size()); diff --git a/include/differentiators.h b/include/differentiators.h index a4f4f91..0b5544e 100644 --- a/include/differentiators.h +++ b/include/differentiators.h @@ -27,7 +27,7 @@ #pragma once #include "DigitalFilter.h" -#include "math.h" +#include "math_utils.h" #include // Read this if you are an adulator of the math god: https://arxiv.org/pdf/1709.08321.pdf @@ -106,6 +106,19 @@ template vectN_t GetFHNRCoeffs() { return vectN_t{ T(52 template vectN_t GetFHNRCoeffs() { return vectN_t{ T(56), T(26), T(-2), T(-17), T(-30), T(-30), T(-28), T(-13), T(4), T(34) } / T(220); } template vectN_t GetFHNRCoeffs() { return vectN_t{ T(320), T(206), T(-8), T(-47), T(-186), T(-150), T(-214), T(-103), T(-92), T(94), T(180) } / T(1540); } +template typename Foo> vectN_t GetForwardISDCoeffs() +{ + vectN_t v{}; + const vectN_t v0 = Foo(); + for (Eigen::Index k = 0; k < N; ++k) + v(k) = k * v0(k); + return v(k); +} +// Forward Noise-Robust differentiators for irregular space data +template vectN_t GetFNRISDCoeffs() { return GetForwardISDCoeffs(); } +// Forward Hybrid Noise-Robust differentiators for irregular space data +template vectN_t GetFHNRISDCoeffs() { return GetForwardISDCoeffs(); } + // Centered Noise-Robust differentiators (tangency at 2nd order): http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ template vectN_t GetCNR2Coeffs() @@ -125,16 +138,20 @@ template vectN_t GetCNR4Coeffs() { return vectN_t{ T(-2 template vectN_t GetCNR4Coeffs() { return vectN_t{ T(-11), T(-32), T(39), T(256), T(322) } / T(1536); } // Centered Noise-Robust differentiators for irregular space data: http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ -template typename Foo> vectN_t GetCNRISDoeffs() +template typename Foo> vectN_t GetCNRISDCoeffs() { + constexpr const size_t M = (N - 1) / 2; vectN_t v{}; const vectN_t v0 = Foo(); - for (Eigen::Index k = 0; k < N; ++k) - v(k) = T(2) * k * v0(k); + v(M) = 0; + for (Eigen::Index k = 1; k < M; ++k) { + v(M - k) = T(2) * k * v0(M - k); + v(M + k) = T(2) * k * v0(M + k); + } return v(k); } -template vectN_t GetCNR2ISDCoeffs() { return GetCNRISDoeffs(); } -template vectN_t GetCNR4ISDCoeffs() { return GetCNRISDoeffs(); } +template vectN_t GetCNR2ISDCoeffs() { return GetCNRISDCoeffs(); } +template vectN_t GetCNR4ISDCoeffs() { return GetCNRISDCoeffs(); } /* * Second order differentiators @@ -170,10 +187,11 @@ vectN_t GetSOCNRCoeffs() constexpr const T Den = pow(size_t(2), N - 3); vectN_t v{}; vectN_t s = GetSONRBaseCoeffs(); - for (size_t k = 0; k < M; ++k) - v(k) = s(M - k); - for (size_t k = 0; k < M; ++k) + v(M) = s(0); + for (size_t k = 1; k < M; ++k) { v(M + k) = s(k); + v(M - k) = s(k); + } v /= Den; return v; @@ -195,11 +213,11 @@ vectN_t GetSOCNRISDCoeffs() constexpr const alpha = [&s](size_t k) -> T { return T(4) * k * k * s(k) }; - for (size_t k = 0; k < M; ++k) - v(k) = alpha(M - k); v(M) = -T(2) * alpha(0); - for (size_t k = 1; k < M; ++k) + for (size_t k = 1; k < M; ++k) { v(M + k) = alpha(k); + v(M - k) = alpha(k); + } v /= Den; return v; @@ -239,26 +257,55 @@ public: T timestep() const noexcept { return std::pow(bCoeff()(0) / CoeffGetter()(0), T(1) / Order); } }; +template typename CoeffGetter> +class TVForwardDifferentiator : public TVGenericFilter { + static_assert(Order >= 1); + +public: + TVForwardDifferentiator() + : TVGenericFilter(Order, {T(1)}, CoeffGetter()) + {} +}; + +template typename CoeffGetter> +class TVCentralDifferentiator : public TVGenericFilter { + static_assert(Order >= 1); + +public: + TVCentralDifferentiator() + : TVGenericFilter(Order, {T(1)}, CoeffGetter(), Type::Centered) + {} +}; + } // namespace details +// Forward differentiators +template using ForwardNoiseRobustDiff = ForwardDifferentiator; +template using ForwardHybridNoiseRobustDiff = ForwardDifferentiator; +// Time-Varying forward differentiators +template using TVForwardNoiseRobustDiff = TVForwardDifferentiator; +template using TVForwardHybridNoiseRobustDiff = TVForwardDifferentiator; + // Central differentiators template using CentralDiff = CentralDifferentiator; template using LowNoiseLanczosDiff = CentralDifferentiator; template using SuperLowNoiseLanczosDiff = CentralDifferentiator; template using CenteredNoiseRobust2Diff = CentralDifferentiator; template using CenteredNoiseRobust4Diff = CentralDifferentiator; +// Time-Varying central differentiators +template using TVCenteredNoiseRobust2Diff = TVCentralDifferentiator; +template using TVCenteredNoiseRobust4Diff = TVCentralDifferentiator; -// Forward differentiators -template using ForwardNoiseRobustDiff = ForwardDifferentiator; -template using ForwardHybridNoiseRobustDiff = ForwardDifferentiator; -// TODO: time-variable differentiators +// Second-order forward differentiators +template using ForwardSecondOrderDiff = ForwardDifferentiator; +// Second-order Time-Varying forward differentiators +template using TVForwardSecondOrderDiff = TVForwardDifferentiator; // Second-order central differentiators template using CenteredSecondOrderDiff = CentralDifferentiator; - -// Second-order central differentiators -template using ForwardSecondOrderDiff = ForwardDifferentiator; +// Second-order Time-Varying forward differentiators +template using TVCenteredSecondOrderDiff = TVCentralDifferentiator; } // namespace difi \ No newline at end of file diff --git a/include/math.h b/include/math_utils.h similarity index 100% rename from include/math.h rename to include/math_utils.h