kopia lustrzana https://github.com/vsamy/DiFipp
Fix some stuffs.
Vincent Samy
rodzic
1242b85c67
commit
60c0b5c11f
|
|
@ -30,7 +30,6 @@
|
|||
#include "gsl/gsl_assert.h"
|
||||
#include "type_checks.h"
|
||||
#include "typedefs.h"
|
||||
#include <stddef.h>
|
||||
#include <string>
|
||||
|
||||
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<T>& aCoeff, const vectX_t<T>& bCoeff, Type type);
|
||||
bool checkCoeffs(const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff, FilterType type);
|
||||
|
||||
private:
|
||||
/*! \brief Default uninitialized constructor. */
|
||||
|
|
|
|||
|
|
@ -31,20 +31,20 @@ namespace difi {
|
|||
|
||||
// Public functions
|
||||
|
||||
template <typename T>
|
||||
void BaseFilter<T>::setType(Type type)
|
||||
template <typename T, typename Derived>
|
||||
void BaseFilter<T, Derived>::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 <typename T>
|
||||
template <typename T, typename Derived>
|
||||
template <typename T2>
|
||||
void BaseFilter<T>::setCoeffs(T2&& aCoeff, T2&& bCoeff)
|
||||
void BaseFilter<T, Derived>::setCoeffs(T2&& aCoeff, T2&& bCoeff)
|
||||
{
|
||||
static_assert(std::is_convertible_v<T2, vectX_t<T>>, "The coefficients types should be convertible to vectX_t<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<T>::setCoeffs(T2&& aCoeff, T2&& bCoeff)
|
|||
m_isInitialized = true;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void BaseFilter<T>::getCoeffs(vectX_t<T>& aCoeff, vectX_t<T>& bCoeff) const noexcept
|
||||
template <typename T, typename Derived>
|
||||
void BaseFilter<T, Derived>::getCoeffs(vectX_t<T>& aCoeff, vectX_t<T>& bCoeff) const noexcept
|
||||
{
|
||||
aCoeff = m_aCoeff;
|
||||
bCoeff = m_bCoeff;
|
||||
|
|
@ -61,22 +61,22 @@ void BaseFilter<T>::getCoeffs(vectX_t<T>& aCoeff, vectX_t<T>& bCoeff) const noex
|
|||
|
||||
// Protected functions
|
||||
|
||||
template <typename T>
|
||||
BaseFilter<T>::BaseFilter(const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff, FilterType type)
|
||||
template <typename T, typename Derived>
|
||||
BaseFilter<T, Derived>::BaseFilter(const vectX_t<T>& aCoeff, const vectX_t<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 <typename T>
|
||||
void BaseFilter<T>::normalizeCoeffs()
|
||||
template <typename T, typename Derived>
|
||||
void BaseFilter<T, Derived>::normalizeCoeffs()
|
||||
{
|
||||
T a0 = m_aCoeff(0);
|
||||
if (std::abs(a0 - T(1)) < std::numeric_limits<T>::epsilon())
|
||||
|
|
@ -86,10 +86,10 @@ void BaseFilter<T>::normalizeCoeffs()
|
|||
m_bCoeff /= a0;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool BaseFilter<T>::checkCoeffs(const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff, FilterType type)
|
||||
template <typename T, typename Derived>
|
||||
bool BaseFilter<T, Derived>::checkCoeffs(const vectX_t<T>& aCoeff, const vectX_t<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<T>::epsilon() && bCoeff.size() > 0 && centering;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@
|
|||
namespace difi {
|
||||
|
||||
template <typename T>
|
||||
class GenericFilter : BaseFilter<T, GenericFilter<T>> {
|
||||
class GenericFilter : public BaseFilter<T, GenericFilter<T>> {
|
||||
public:
|
||||
/*! \brief Filter a new data.
|
||||
*
|
||||
|
|
@ -49,17 +49,17 @@ public:
|
|||
*/
|
||||
vectX_t<T> filter(const vectX_t<T>& data);
|
||||
|
||||
void resetFilter() noexcept
|
||||
void resetFilter() noexcept;
|
||||
|
||||
protected:
|
||||
GenericFilter() = default;
|
||||
GenericFilter(const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff, Type type = Type::Forward)
|
||||
GenericFilter(const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff, FilterType type = FilterType::Forward)
|
||||
: BaseFilter(aCoeff, bCoeff, type)
|
||||
{}
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
class TVGenericFilter : BaseFilter<T, GenericFilter<T>> {
|
||||
class TVGenericFilter : public BaseFilter<T, GenericFilter<T>> {
|
||||
public:
|
||||
/*! \brief Filter a new data.
|
||||
*
|
||||
|
|
@ -76,16 +76,21 @@ public:
|
|||
*/
|
||||
vectX_t<T> filter(const vectX_t<T>& data, const vectX_t<T>& time);
|
||||
|
||||
void resetFilter() noexcept;
|
||||
|
||||
protected:
|
||||
GenericFilter() = default;
|
||||
GenericFilter(const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff, Type type = Type::Forward)
|
||||
TVGenericFilter() = default;
|
||||
TVGenericFilter(int order, const vectX_t<T>& aCoeff, const vectX_t<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<T> m_timers;
|
||||
vectX_t<T> m_timeDiffs;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -78,13 +78,16 @@ T TVGenericFilter<T>::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<T> TVGenericFilter<T>::filter(const vectX_t<T>& data, const vectX_t<T>&
|
|||
}
|
||||
|
||||
template <typename T>
|
||||
void GenericFilter<T>::resetFilter() noexcept
|
||||
void TVGenericFilter<T>::resetFilter() noexcept
|
||||
{
|
||||
m_filteredData.setZero(m_aCoeff.size());
|
||||
m_rawData.setZero(m_bCoeff.size());
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@
|
|||
|
||||
#pragma once
|
||||
#include "DigitalFilter.h"
|
||||
#include "math.h"
|
||||
#include "math_utils.h"
|
||||
#include <array>
|
||||
|
||||
// Read this if you are an adulator of the math god: https://arxiv.org/pdf/1709.08321.pdf
|
||||
|
|
@ -106,6 +106,19 @@ template <typename T> vectN_t<T, 8> GetFHNRCoeffs() { return vectN_t<T, 8>{ T(52
|
|||
template <typename T> vectN_t<T, 9> GetFHNRCoeffs() { return vectN_t<T, 9>{ T(56), T(26), T(-2), T(-17), T(-30), T(-30), T(-28), T(-13), T(4), T(34) } / T(220); }
|
||||
template <typename T> vectN_t<T, 10> GetFHNRCoeffs() { return vectN_t<T, 10>{ 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 T, size_t N, template<class, class> typename Foo> vectN_t<T, N> GetForwardISDCoeffs()
|
||||
{
|
||||
vectN_t<T, N> v{};
|
||||
const vectN_t<T, N> v0 = Foo<T, NCoeffs>();
|
||||
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 <typename T, size_t N> vectN_t<T, N> GetFNRISDCoeffs() { return GetForwardISDCoeffs<T, N, GetFNRCoeffs>(); }
|
||||
// Forward Hybrid Noise-Robust differentiators for irregular space data
|
||||
template <typename T, size_t N> vectN_t<T, N> GetFHNRISDCoeffs() { return GetForwardISDCoeffs<T, N, GetFHNRCoeffs>(); }
|
||||
|
||||
// Centered Noise-Robust differentiators (tangency at 2nd order): http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
|
||||
template <typename T, size_t N>
|
||||
vectN_t<T, N> GetCNR2Coeffs()
|
||||
|
|
@ -125,16 +138,20 @@ template <typename T> vectN_t<T, 4> GetCNR4Coeffs() { return vectN_t<T, 4>{ T(-2
|
|||
template <typename T> vectN_t<T, 5> GetCNR4Coeffs() { return vectN_t<T, 5>{ 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 T, size_t N, template<class, class> typename Foo> vectN_t<T, N> GetCNRISDoeffs()
|
||||
template <typename T, size_t N, template<class, class> typename Foo> vectN_t<T, N> GetCNRISDCoeffs()
|
||||
{
|
||||
constexpr const size_t M = (N - 1) / 2;
|
||||
vectN_t<T, N> v{};
|
||||
const vectN_t<T, N> v0 = Foo<T, NCoeffs>();
|
||||
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 <typename T, size_t N> vectN_t<T, N> GetCNR2ISDCoeffs() { return GetCNRISDoeffs<T, N, GetCSNR2Coeffs>(); }
|
||||
template <typename T, size_t N> vectN_t<T, N> GetCNR4ISDCoeffs() { return GetCNRISDoeffs<T, N, GetCSNR4Coeffs>(); }
|
||||
template <typename T, size_t N> vectN_t<T, N> GetCNR2ISDCoeffs() { return GetCNRISDCoeffs<T, N, GetCNR2Coeffs>(); }
|
||||
template <typename T, size_t N> vectN_t<T, N> GetCNR4ISDCoeffs() { return GetCNRISDCoeffs<T, N, GetCNR4Coeffs>(); }
|
||||
|
||||
/*
|
||||
* Second order differentiators
|
||||
|
|
@ -170,10 +187,11 @@ vectN_t<T, N> GetSOCNRCoeffs()
|
|||
constexpr const T Den = pow(size_t(2), N - 3);
|
||||
vectN_t<T, N> v{};
|
||||
vectN_t<T, N> s = GetSONRBaseCoeffs<T, N>();
|
||||
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<T, N> 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<T, N>()(0), T(1) / Order); }
|
||||
};
|
||||
|
||||
template <typename T, size_t N, int Order, template<class, class> typename CoeffGetter>
|
||||
class TVForwardDifferentiator : public TVGenericFilter<T> {
|
||||
static_assert(Order >= 1);
|
||||
|
||||
public:
|
||||
TVForwardDifferentiator()
|
||||
: TVGenericFilter<T>(Order, {T(1)}, CoeffGetter<T, N>())
|
||||
{}
|
||||
};
|
||||
|
||||
template <typename T, size_t N, int Order, template<class, class> typename CoeffGetter>
|
||||
class TVCentralDifferentiator : public TVGenericFilter<T> {
|
||||
static_assert(Order >= 1);
|
||||
|
||||
public:
|
||||
TVCentralDifferentiator()
|
||||
: TVGenericFilter<T>(Order, {T(1)}, CoeffGetter<T, N>(), Type::Centered)
|
||||
{}
|
||||
};
|
||||
|
||||
} // namespace details
|
||||
|
||||
// Forward differentiators
|
||||
template <typename T, size_t N> using ForwardNoiseRobustDiff = ForwardDifferentiator<T, N, 1, details::GetFNRCoeffs>;
|
||||
template <typename T, size_t N> using ForwardHybridNoiseRobustDiff = ForwardDifferentiator<T, N, 1, details::GetFHNRCoeffs>;
|
||||
// Time-Varying forward differentiators
|
||||
template <typename T, size_t N> using TVForwardNoiseRobustDiff = TVForwardDifferentiator<T, N, 1, details::GetFNRISDCoeffs>;
|
||||
template <typename T, size_t N> using TVForwardHybridNoiseRobustDiff = TVForwardDifferentiator<T, N, 1, details::GetFHNRISDCoeffs>;
|
||||
|
||||
// Central differentiators
|
||||
template <typename T, size_t N> using CentralDiff = CentralDifferentiator<T, N, 1, details::GetCDCoeffs>;
|
||||
template <typename T, size_t N> using LowNoiseLanczosDiff = CentralDifferentiator<T, N, 1, details::GetLNLCoeffs>;
|
||||
template <typename T, size_t N> using SuperLowNoiseLanczosDiff = CentralDifferentiator<T, N, 1, details::GetSLNLCoeffs>;
|
||||
template <typename T, size_t N> using CenteredNoiseRobust2Diff = CentralDifferentiator<T, N, 1, details::GetCNR2Coeffs>;
|
||||
template <typename T, size_t N> using CenteredNoiseRobust4Diff = CentralDifferentiator<T, N, 1, details::GetCNR4Coeffs>;
|
||||
// Time-Varying central differentiators
|
||||
template <typename T, size_t N> using TVCenteredNoiseRobust2Diff = TVCentralDifferentiator<T, N, 1, details::GetCNR2ISDCoeffs>;
|
||||
template <typename T, size_t N> using TVCenteredNoiseRobust4Diff = TVCentralDifferentiator<T, N, 1, details::GetCNR4ISDCoeffs>;
|
||||
|
||||
// Forward differentiators
|
||||
template <typename T, size_t N> using ForwardNoiseRobustDiff = ForwardDifferentiator<T, N, 1, details::GetFNRCoeffs>;
|
||||
template <typename T, size_t N> using ForwardHybridNoiseRobustDiff = ForwardDifferentiator<T, N, 1, details::GetFHNRCoeffs>;
|
||||
|
||||
// TODO: time-variable differentiators
|
||||
// Second-order forward differentiators
|
||||
template <typename T, size_t N> using ForwardSecondOrderDiff = ForwardDifferentiator<T, N, 2, details::GetSOFNRCoeffs>;
|
||||
// Second-order Time-Varying forward differentiators
|
||||
template <typename T, size_t N> using TVForwardSecondOrderDiff = TVForwardDifferentiator<T, N, 2, details::GetSOFNRISDCoeffs>;
|
||||
|
||||
// Second-order central differentiators
|
||||
template <typename T, size_t N> using CenteredSecondOrderDiff = CentralDifferentiator<T, N, 2, details::GetSOCNRCoeffs>;
|
||||
|
||||
// Second-order central differentiators
|
||||
template <typename T, size_t N> using ForwardSecondOrderDiff = ForwardDifferentiator<T, N, 2, details::GetSOFNRCoeffs>;
|
||||
// Second-order Time-Varying forward differentiators
|
||||
template <typename T, size_t N> using TVCenteredSecondOrderDiff = TVCentralDifferentiator<T, N, 2, details::GetSOCNRISDCoeffs>;
|
||||
|
||||
|
||||
} // namespace difi
|
||||
Ładowanie…
Reference in New Issue