kopia lustrzana https://github.com/vsamy/DiFipp
Time-fixed central filter pass.
rodzic
934a0ed17f
commit
d049e600d0
|
@ -13,11 +13,12 @@ AllowShortBlocksOnASingleLine: true
|
|||
AllowShortCaseLabelsOnASingleLine: true
|
||||
AllowShortFunctionsOnASingleLine: All
|
||||
AllowShortIfStatementsOnASingleLine: false
|
||||
AllowShortLambdasOnASingleLine: All
|
||||
AllowShortLoopsOnASingleLine: false
|
||||
AlwaysBreakAfterDefinitionReturnType: None
|
||||
AlwaysBreakAfterReturnType: None
|
||||
AlwaysBreakBeforeMultilineStrings: false
|
||||
AlwaysBreakTemplateDeclarations: MultiLine
|
||||
AlwaysBreakTemplateDeclarations: No
|
||||
BinPackArguments: true
|
||||
BinPackParameters: true
|
||||
BraceWrapping:
|
||||
|
|
|
@ -36,6 +36,7 @@ namespace difi {
|
|||
|
||||
// TODO: noexcept(Function of gsl variable)
|
||||
// TODO: constructor with universal refs
|
||||
// TODO: setCoeffs with universal refs
|
||||
|
||||
/*! \brief Low-level filter.
|
||||
*
|
||||
|
@ -57,7 +58,13 @@ public:
|
|||
void resetFilter() noexcept { derived().resetFilter(); };
|
||||
|
||||
/*!< \brief Return the filter type */
|
||||
FilterType type() const noexcept { return (m_center == 0 ? FilterType::Backward : FilterType::Centered); }
|
||||
FilterType type() const noexcept { return m_type; }
|
||||
/*! \brief Get how far back is the filtered value.
|
||||
* The filtered value is computed at time \f$tc=t - center()*\Delta T\f$ where \f$t\f$ is the current time
|
||||
* \f$\Delta T\f$ is your sampling time.
|
||||
* \note For FilterType::Backward filter, the function returns 0.
|
||||
*/
|
||||
Eigen::Index center() const noexcept { return (m_type == FilterType::Backward ? 0 : ((m_bCoeff.size() - 1) / 2)); }
|
||||
/*! \brief Get digital filter coefficients.
|
||||
*
|
||||
* It will automatically resize the given vectors.
|
||||
|
@ -75,8 +82,6 @@ public:
|
|||
Eigen::Index bOrder() const noexcept { return m_bCoeff.size(); }
|
||||
/*! \brief Return the initialization state of the filter0 */
|
||||
bool isInitialized() const noexcept { return m_isInitialized; }
|
||||
|
||||
protected:
|
||||
/*! \brief Set type of filter (one-sided or centered)
|
||||
*
|
||||
* \param type The filter type.
|
||||
|
@ -89,8 +94,9 @@ protected:
|
|||
* \param aCoeff Denominator coefficients of the filter in decreasing order.
|
||||
* \param bCoeff Numerator coefficients of the filter in decreasing order.
|
||||
*/
|
||||
template <typename T2>
|
||||
void setCoeffs(T2&& aCoeff, T2&& bCoeff);
|
||||
void setCoeffs(const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff);
|
||||
|
||||
protected:
|
||||
/*! \brief Normalized the filter coefficients such that aCoeff(0) = 1. */
|
||||
void normalizeCoeffs();
|
||||
/*! \brief Check for bad coefficients.
|
||||
|
@ -100,7 +106,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, FilterType type);
|
||||
bool checkCoeffs(const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff);
|
||||
|
||||
private:
|
||||
/*! \brief Default uninitialized constructor. */
|
||||
|
@ -108,7 +114,7 @@ private:
|
|||
/*! \brief Constructor.
|
||||
* \param aCoeff Denominator coefficients of the filter in decreasing order.
|
||||
* \param bCoeff Numerator coefficients of the filter in decreasing order.
|
||||
* \param center
|
||||
* \param type Type of the filter.
|
||||
*/
|
||||
BaseFilter(const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff, FilterType type = FilterType::Backward);
|
||||
/*! \brief Default destructor. */
|
||||
|
@ -118,7 +124,7 @@ private:
|
|||
const Derived& derived() const noexcept { return *static_cast<const Derived*>(this); }
|
||||
|
||||
private:
|
||||
Eigen::Index m_center = 0; /*!< Center of the filter. 0 is a one-sided filter. Default is 0. */
|
||||
FilterType m_type; /*!< Type of filter. Default is FilterType::Backward. */
|
||||
bool m_isInitialized = false; /*!< Initialization state of the filter. Default is false */
|
||||
vectX_t<T> m_aCoeff; /*!< Denominator coefficients of the filter */
|
||||
vectX_t<T> m_bCoeff; /*!< Numerator coefficients of the filter */
|
||||
|
|
|
@ -35,16 +35,13 @@ template <typename T, typename Derived>
|
|||
void BaseFilter<T, Derived>::setType(FilterType type)
|
||||
{
|
||||
Expects(type == FilterType::Centered ? m_bCoeff.size() > 2 && m_bCoeff.size() % 2 == 1 : true);
|
||||
m_center = (type == FilterType::Backward ? 0 : (m_bCoeff.size() - 1) / 2);
|
||||
m_type = type;
|
||||
}
|
||||
|
||||
template <typename T, typename Derived>
|
||||
template <typename T2>
|
||||
void BaseFilter<T, Derived>::setCoeffs(T2&& aCoeff, T2&& bCoeff)
|
||||
void BaseFilter<T, Derived>::setCoeffs(const vectX_t<T>& aCoeff, const vectX_t<T>& 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 ? FilterType::Backward : FilterType::Centered)));
|
||||
Expects(checkCoeffs(aCoeff, bCoeff));
|
||||
m_aCoeff = aCoeff;
|
||||
m_bCoeff = bCoeff;
|
||||
normalizeCoeffs();
|
||||
|
@ -65,11 +62,11 @@ 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_type(type)
|
||||
, m_filteredData(aCoeff.size())
|
||||
, m_rawData(bCoeff.size())
|
||||
{
|
||||
Expects(checkCoeffs(aCoeff, bCoeff, type));
|
||||
m_center = (type == FilterType::Backward ? 0 : (bCoeff.size() - 1) / 2);
|
||||
Expects(checkCoeffs(aCoeff, bCoeff));
|
||||
normalizeCoeffs();
|
||||
resetFilter();
|
||||
m_isInitialized = true;
|
||||
|
@ -87,9 +84,9 @@ void BaseFilter<T, Derived>::normalizeCoeffs()
|
|||
}
|
||||
|
||||
template <typename T, typename Derived>
|
||||
bool BaseFilter<T, Derived>::checkCoeffs(const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff, FilterType type)
|
||||
bool BaseFilter<T, Derived>::checkCoeffs(const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff)
|
||||
{
|
||||
bool centering = (type == FilterType::Centered ? (bCoeff.size() % 2 == 1) : true);
|
||||
bool centering = (m_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;
|
||||
}
|
||||
|
||||
|
|
|
@ -45,41 +45,12 @@ public:
|
|||
/*! \brief Constructor.
|
||||
* \param aCoeff Denominator coefficients of the filter in decreasing order.
|
||||
* \param bCoeff Numerator coefficients of the filter in decreasing order.
|
||||
* \param type Type of the filter.
|
||||
*/
|
||||
DigitalFilter(const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff)
|
||||
: GenericFilter<T>(aCoeff, bCoeff)
|
||||
DigitalFilter(const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff, FilterType type = FilterType::Backward)
|
||||
: GenericFilter<T>(aCoeff, bCoeff, type)
|
||||
{
|
||||
}
|
||||
|
||||
void setCoefficients(vectX_t<T>&& aCoeff, vectX_t<T>&& bCoeff)
|
||||
{
|
||||
setCoeffs(std::forward(aCoeff), std::forward(bCoeff));
|
||||
}
|
||||
};
|
||||
|
||||
/*! \brief Basic centered digital filter.
|
||||
*
|
||||
* This filter allows you to set any centered digital filter based on its coefficients.
|
||||
* \tparam T Floating type.
|
||||
*/
|
||||
template <typename T>
|
||||
class CenteredDigitalFilter : public GenericFilter<T> {
|
||||
public:
|
||||
/*! \brief Default uninitialized constructor. */
|
||||
CenteredDigitalFilter() = default;
|
||||
/*! \brief Constructor.
|
||||
* \param aCoeff Denominator coefficients of the filter in decreasing order.
|
||||
* \param bCoeff Numerator coefficients of the filter in decreasing order.
|
||||
*/
|
||||
CenteredDigitalFilter(const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff)
|
||||
: GenericFilter<T>(aCoeff, bCoeff, Type::Centered)
|
||||
{
|
||||
}
|
||||
void setCoefficients(vectX_t<T>&& aCoeff, vectX_t<T>&& bCoeff)
|
||||
{
|
||||
setCoeffs(std::forward(aCoeff), std::forward(bCoeff));
|
||||
setType(Type::Centered);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace difi
|
|
@ -59,7 +59,7 @@ protected:
|
|||
};
|
||||
|
||||
template <typename T>
|
||||
class TVGenericFilter : public BaseFilter<T, GenericFilter<T>> {
|
||||
class TVGenericFilter : public BaseFilter<T, TVGenericFilter<T>> {
|
||||
public:
|
||||
/*! \brief Filter a new data.
|
||||
*
|
||||
|
@ -67,7 +67,7 @@ public:
|
|||
* \param data New data to filter.
|
||||
* \return Filtered data.
|
||||
*/
|
||||
T stepFilter(const T& data, const T& time);
|
||||
T stepFilter(const T& time, const T& data);
|
||||
/*! \brief Filter a signal.
|
||||
*
|
||||
* Filter all data given by the signal.
|
||||
|
@ -80,17 +80,17 @@ public:
|
|||
|
||||
protected:
|
||||
TVGenericFilter() = default;
|
||||
TVGenericFilter(int order, const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff, FilterType type = FilterType::Backward)
|
||||
TVGenericFilter(size_t differentialOrder, const vectX_t<T>& aCoeff, const vectX_t<T>& bCoeff, FilterType type = FilterType::Backward)
|
||||
: BaseFilter(aCoeff, bCoeff, type)
|
||||
, m_order(order)
|
||||
, m_diffOrder(differentialOrder)
|
||||
{
|
||||
Expects(order >= 1);
|
||||
m_timers.resize(m_bCoeffs.size());
|
||||
m_timeDiffs.resize(m_bCoeffs.size());
|
||||
Expects(differentialOrder >= 1);
|
||||
m_timers.resize(m_bCoeff.size());
|
||||
m_timeDiffs.resize(m_bCoeff.size());
|
||||
}
|
||||
|
||||
private:
|
||||
int m_order = 1;
|
||||
size_t m_diffOrder = 1;
|
||||
vectX_t<T> m_timers;
|
||||
vectX_t<T> m_timeDiffs;
|
||||
};
|
||||
|
|
|
@ -40,8 +40,8 @@ T GenericFilter<T>::stepFilter(const T& data)
|
|||
|
||||
m_rawData(0) = data;
|
||||
m_filteredData(0) = 0;
|
||||
m_filteredData(m_center) = m_bCoeff.dot(m_rawData) - m_aCoeff.dot(m_filteredData);
|
||||
return m_filteredData(m_center);
|
||||
m_filteredData(0) = m_bCoeff.dot(m_rawData) - m_aCoeff.dot(m_filteredData);
|
||||
return m_filteredData(0);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
|
@ -62,7 +62,7 @@ void GenericFilter<T>::resetFilter() noexcept
|
|||
}
|
||||
|
||||
template <typename T>
|
||||
T TVGenericFilter<T>::stepFilter(const T& data, const T& time)
|
||||
T TVGenericFilter<T>::stepFilter(const T& time, const T& data)
|
||||
{
|
||||
Expects(m_isInitialized);
|
||||
|
||||
|
@ -75,24 +75,28 @@ T TVGenericFilter<T>::stepFilter(const T& data, const T& time)
|
|||
m_filteredData(i) = m_filteredData(i - 1);
|
||||
|
||||
m_timers(0) = time;
|
||||
if (m_center == 0) {
|
||||
switch (m_type) {
|
||||
case FilterType::Backward:
|
||||
for (Eigen::Index i = m_rawData.size() - 1; i > 0; --i)
|
||||
m_timeDiffs(i) = m_timeDiffs(i - 1);
|
||||
m_timeDiffs(0) = std::pow(time - m_timers(1), m_order);
|
||||
} else {
|
||||
const Eigen::Index S = m_rawData.size() - 1;
|
||||
const Eigen::Index M = S / 2;
|
||||
m_timeDiffs(0) = std::pow(time - m_timers(1), m_diffOrder);
|
||||
break;
|
||||
case FilterType::Centered: {
|
||||
const Eigen::Index M = (m_rawData.size() - 1) / 2;
|
||||
m_timeDiffs(M) = T(1);
|
||||
for (Eigen::Index i = 1; i < M; ++i) {
|
||||
const T diff = std::pow(m_timers(M - i) - m_timers(M + i), m_order);
|
||||
const T diff = std::pow(m_timers(M - i) - m_timers(M + i), m_diffOrder);
|
||||
m_timeDiffs(M + i) = diff;
|
||||
m_timeDiffs(M - i) = diff;
|
||||
}
|
||||
break;
|
||||
}
|
||||
default: std::invalid_argument("Given FilterType lacks the implementation.");
|
||||
}
|
||||
m_rawData(0) = data;
|
||||
m_filteredData(0) = 0;
|
||||
m_filteredData(m_center) = (m_bCoeff.cwiseQuotient(m_timeDiffs)).dot(m_rawData) - m_aCoeff.dot(m_filteredData);
|
||||
return m_filteredData(m_center);
|
||||
m_filteredData(0) = (m_bCoeff.cwiseQuotient(m_timeDiffs)).dot(m_rawData) - m_aCoeff.dot(m_filteredData);
|
||||
return m_filteredData(0);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
|
@ -102,7 +106,7 @@ vectX_t<T> TVGenericFilter<T>::filter(const vectX_t<T>& data, const vectX_t<T>&
|
|||
Expects(data.size() == time.size());
|
||||
vectX_t<T> results(data.size());
|
||||
for (Eigen::Index i = 0; i < data.size(); ++i)
|
||||
results(i) = stepFilter(data(i), time(i));
|
||||
results(i) = stepFilter(time(i), data(i));
|
||||
return results;
|
||||
}
|
||||
|
||||
|
@ -112,7 +116,7 @@ void TVGenericFilter<T>::resetFilter() noexcept
|
|||
m_filteredData.setZero(m_aCoeff.size());
|
||||
m_rawData.setZero(m_bCoeff.size());
|
||||
m_timers.setZero(m_bCoeff.size());
|
||||
m_timerDiffs.setZero(m_bCoeff.size());
|
||||
m_timeDiffs.setZero(m_bCoeff.size());
|
||||
}
|
||||
|
||||
} // namespace difi
|
|
@ -45,10 +45,12 @@ public:
|
|||
MovingAverage() = default;
|
||||
/*! \brief Constructor.
|
||||
* \param windowSize Size of the moving average window.
|
||||
* \param type Type of the filter.
|
||||
*/
|
||||
MovingAverage(int windowSize)
|
||||
MovingAverage(int windowSize, FilterType type = FilterType::Backward)
|
||||
{
|
||||
setWindowSize(windowSize);
|
||||
setType(type);
|
||||
}
|
||||
/*! \brief Set the size of the moving average window. */
|
||||
void setWindowSize(int windowSize)
|
||||
|
@ -60,32 +62,4 @@ public:
|
|||
int windowSize() const noexcept { return bOrder(); }
|
||||
};
|
||||
|
||||
/*! \brief Centered moving average digital filter.
|
||||
*
|
||||
* This is a specialization of a digital filter in order to use a centered moving average.
|
||||
* \tparam T Floating type.
|
||||
*/
|
||||
template <typename T>
|
||||
class CenteredMovingAverage : public DigitalFilter<T> {
|
||||
public:
|
||||
/*! \brief Default uninitialized constructor. */
|
||||
CenteredMovingAverage() = default;
|
||||
/*! \brief Constructor.
|
||||
* \param windowSize Size of the moving average window.
|
||||
*/
|
||||
CenteredMovingAverage(int windowSize)
|
||||
{
|
||||
setWindowSize(windowSize);
|
||||
}
|
||||
/*! \brief Set the size of the moving average window. */
|
||||
void setWindowSize(int windowSize)
|
||||
{
|
||||
Expects(windowSize > 2 && windowSize % 2 == 1);
|
||||
setCoeffs(vectX_t<T>::Constant(1, T(1)), vectX_t<T>::Constant(windowSize, T(1) / windowSize));
|
||||
setType(Type::Centered);
|
||||
}
|
||||
/*! \brief Get the size of the moving average window. */
|
||||
int windowSize() const noexcept { return bOrder(); }
|
||||
};
|
||||
|
||||
} // namespace difi
|
||||
|
|
|
@ -28,7 +28,6 @@
|
|||
#pragma once
|
||||
#include "DigitalFilter.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
|
||||
|
||||
|
@ -40,103 +39,150 @@ namespace details {
|
|||
// N: Number of points
|
||||
|
||||
// Central differentiators: http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/central-differences/
|
||||
template <typename T, size_t N> struct GetCDCoeffs { vectN_t<T, N> operator()() const; };
|
||||
template <typename T> struct GetCDCoeffs<T, 3> { vectN_t<T, 3> operator()() const { return vectN_t<T, 3>{ T(1), T(0), T(-1) } / T(2); } };
|
||||
template <typename T> struct GetCDCoeffs<T, 5> { vectN_t<T, 5> operator()() const { return vectN_t<T, 5>{ T(-1), T(8), T(0), T(8), T(1) } / T(12); } };
|
||||
template <typename T> struct GetCDCoeffs<T, 7> { vectN_t<T, 7> operator()() const { return vectN_t<T, 7>{ T(1), T(-9), T(45), T(0), T(-45), T(9), T(-1) } / T(60); } };
|
||||
template <typename T> struct GetCDCoeffs<T, 9> { vectN_t<T, 9> operator()() const { return vectN_t<T, 9>{ T(-3), T(32), T(-168), T(672), T(0), T(-672), T(168), T(-32), T(3) } / T(840); } };
|
||||
template <typename T, size_t N> struct GetCDCoeffs {
|
||||
vectN_t<T, N> operator()() const;
|
||||
};
|
||||
template <typename T> struct GetCDCoeffs<T, 3> {
|
||||
vectN_t<T, 3> operator()() const { return (vectN_t<T, 3>() << T(1), T(0), T(-1)).finished() / T(2); }
|
||||
};
|
||||
template <typename T> struct GetCDCoeffs<T, 5> {
|
||||
vectN_t<T, 5> operator()() const { return (vectN_t<T, 5>() << T(-1), T(8), T(0), T(8), T(1)).finished() / T(12); }
|
||||
};
|
||||
template <typename T> struct GetCDCoeffs<T, 7> {
|
||||
vectN_t<T, 7> operator()() const { return (vectN_t<T, 7>() << T(1), T(-9), T(45), T(0), T(-45), T(9), T(-1)).finished() / T(60); }
|
||||
};
|
||||
template <typename T> struct GetCDCoeffs<T, 9> {
|
||||
vectN_t<T, 9> operator()() const { return (vectN_t<T, 9>() << T(-3), T(32), T(-168), T(672), T(0), T(-672), T(168), T(-32), T(3)).finished() / T(840); }
|
||||
};
|
||||
|
||||
// Low-noise Lanczos differentiators: http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/lanczos-low-noise-differentiators/
|
||||
template <typename T, size_t N>
|
||||
struct GetLNLCoeffs {
|
||||
vectN_t<T, N> operator()() const
|
||||
{
|
||||
static_assert(N % 2 == 1. "'N' must be odd.");
|
||||
constexpr T GetCoeff = [](size_t n, size_t k) {
|
||||
const T m = (n - 1) / 2;
|
||||
return T(3) * k / (m * (m + 1) * (2 * m + 1));
|
||||
};
|
||||
vectN_t<T, N> operator()() const
|
||||
{
|
||||
static_assert(N > 2 && N % 2 == 1, "'N' must be odd.");
|
||||
constexpr const size_t M = (N - 1) / 2;
|
||||
constexpr const size_t Den = M * (M + 1) * (2 * M + 1);
|
||||
|
||||
vectN_t<T, N> v{};
|
||||
for (Eigen::Index k = 0; k < N; ++k)
|
||||
v(k) = GetCoeff(N, k);
|
||||
return v;
|
||||
}
|
||||
vectN_t<T, N> v{};
|
||||
v(M) = T(0);
|
||||
for (size_t k = 0; k < M; ++k) {
|
||||
v(k) = T(3) * (M - k) / static_cast<T>(Den);
|
||||
v(N - k - 1) = -v(k);
|
||||
}
|
||||
return v;
|
||||
}
|
||||
};
|
||||
template <typename T> struct GetLNLCoeffs<T, 5> { vectN_t<T, 5> operator()() const { return vectN_t<T, 5>{ T(2), T(1), T(0), T(-1), T(-2) } / T(10); } };
|
||||
template <typename T> struct GetLNLCoeffs<T, 7> { vectN_t<T, 7> operator()() const { return vectN_t<T, 7>{ T(3), T(2), T(1), T(0), T(-1), T(-2), T(-3) } / T(28); } };
|
||||
template <typename T> struct GetLNLCoeffs<T, 9> { vectN_t<T, 9> operator()() const { return vectN_t<T, 9>{ T(4), T(3), T(2), T(1), T(0), T(-1), T(-2), T(-3), T(-4) } / T(60); } };
|
||||
template <typename T> struct GetLNLCoeffs<T, 11> { vectN_t<T, 11> operator()() const { return vectN_t<T, 11>{ T(5), T(4), T(3), T(2), T(1), T(0), T(-1), T(-2), T(-3), T(-4), T(-5) } / T(110); } };
|
||||
|
||||
// Super Low-noise Lanczos differentiators: http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/lanczos-low-noise-differentiators/
|
||||
template <typename T, size_t N> struct GetSLNLCoeffs { vectN_t<T, N> operator()() const; };
|
||||
template <typename T> struct GetSLNLCoeffs<T, 7> { vectN_t<T, 7> operator()() const { return vectN_t<T, 7>{ T(-22), T(67), T(58), T(0), T(-58), T(-67), T(22) } / T(252); } };
|
||||
template <typename T> struct GetSLNLCoeffs<T, 9> { vectN_t<T, 9> operator()() const { return vectN_t<T, 9>{ T(-86), T(142), T(193), T(126), T(0), T(-126), T(-193), T(-142), T(86) } / T(1188); } };
|
||||
template <typename T> struct GetSLNLCoeffs<T, 11> { vectN_t<T, 11> operator()() const { return vectN_t<T, 11>{ T(-300), T(294), T(532), T(503), T(296), T(0), T(-296), T(-503), T(-532), T(-294), T(300) } / T(5148); } };
|
||||
template <typename T, size_t N> struct GetSLNLCoeffs {
|
||||
vectN_t<T, N> operator()() const;
|
||||
};
|
||||
template <typename T> struct GetSLNLCoeffs<T, 7> {
|
||||
vectN_t<T, 7> operator()() const { return (vectN_t<T, 7>() << T(-22), T(67), T(58), T(0), T(-58), T(-67), T(22)).finished() / T(252); }
|
||||
};
|
||||
template <typename T> struct GetSLNLCoeffs<T, 9> {
|
||||
vectN_t<T, 9> operator()() const { return (vectN_t<T, 9>() << T(-86), T(142), T(193), T(126), T(0), T(-126), T(-193), T(-142), T(86)).finished() / T(1188); }
|
||||
};
|
||||
template <typename T> struct GetSLNLCoeffs<T, 11> {
|
||||
vectN_t<T, 11> operator()() const { return (vectN_t<T, 11>() << T(-300), T(294), T(532), T(503), T(296), T(0), T(-296), T(-503), T(-532), T(-294), T(300)).finished() / T(5148); }
|
||||
};
|
||||
|
||||
// Backward Noise-Robust differentiators; http://www.holoborodko.com/pavel/wp-content/uploads/OneSidedNoiseRobustDifferentiators.pdf
|
||||
template <typename T, size_t N>
|
||||
struct GetFNRCoeffs {
|
||||
vectN_t<T, N> operator()() const {
|
||||
constexpr const int BinCoeff = N - 2;
|
||||
vectN_t<T, N> v{};
|
||||
v(0) = T(1);
|
||||
v(N - 1) = T(-1);
|
||||
for (int i = 1; i < N - 1; ++i)
|
||||
v(i) = (Binomial<T>(N, i) - Binomial<T>(N, i - 1)) / std::pow(T(2), T(BinCoeff));
|
||||
vectN_t<T, N> operator()() const
|
||||
{
|
||||
static_assert(N >= 2, "N should be greater than 2");
|
||||
constexpr const size_t BinCoeff = N - 2;
|
||||
constexpr const size_t M = N / 2;
|
||||
|
||||
return v;
|
||||
vectN_t<T, N> v{};
|
||||
v(0) = T(1);
|
||||
v(M) = T(0);
|
||||
v(N - 1) = T(-1);
|
||||
for (size_t i = 1; i < M; ++i) {
|
||||
v(i) = Binomial<T>(BinCoeff, i) - Binomial<T>(BinCoeff, i - 1);
|
||||
v(N - i - 1) = -v(i);
|
||||
}
|
||||
|
||||
v /= std::pow(T(2), T(BinCoeff));
|
||||
return v;
|
||||
}
|
||||
};
|
||||
template <typename T> struct GetFNRCoeffs<T, 3> { vectN_t<T, 3> operator()() const { return vectN_t<T, 3>{ T(1), T(0), T(-1) } / T(2); } };
|
||||
template <typename T> struct GetFNRCoeffs<T, 4> { vectN_t<T, 4> operator()() const { return vectN_t<T, 4>{ T(1), T(1), T(-1), T(-1) } / T(4); } };
|
||||
template <typename T> struct GetFNRCoeffs<T, 5> { vectN_t<T, 5> operator()() const { return vectN_t<T, 5>{ T(1), T(2), T(0), T(-2), T(-1) } / T(8); } };
|
||||
template <typename T> struct GetFNRCoeffs<T, 6> { vectN_t<T, 6> operator()() const { return vectN_t<T, 6>{ T(1), T(3), T(2), T(-2), T(-3), T(-1) } / T(16); } };
|
||||
template <typename T> struct GetFNRCoeffs<T, 7> { vectN_t<T, 7> operator()() const { return vectN_t<T, 7>{ T(1), T(4), T(5), T(0), T(-5), T(-4), T(-1) } / T(32); } };
|
||||
template <typename T> struct GetFNRCoeffs<T, 8> { vectN_t<T, 8> operator()() const { return vectN_t<T, 8>{ T(1), T(5), T(9), T(5), T(-5), T(-9), T(-5), T(-1) } / T(64); } };
|
||||
template <typename T> struct GetFNRCoeffs<T, 9> { vectN_t<T, 9> operator()() const { return vectN_t<T, 9>{ T(1), T(6), T(14), T(14), T(0), T(-14), T(-14), T(-6), T(-1) } / T(128); } };
|
||||
template <typename T> struct GetFNRCoeffs<T, 10> { vectN_t<T, 10> operator()() const { return vectN_t<T, 10>{ T(1), T(7), T(20), T(28), T(14), T(-14), T(-28), T(-20), T(-7), T(-1) } / T(256); } };
|
||||
template <typename T> struct GetFNRCoeffs<T, 11> { vectN_t<T, 11> operator()() const { return vectN_t<T, 11>{ T(1), T(8), T(27), T(48), T(42), T(0), T(-42), T(-48), T(-27), T(-8), T(-1) } / T(512); } };
|
||||
|
||||
// Backward Hybrid Noise-Robust differentiators; http://www.holoborodko.com/pavel/wp-content/uploads/OneSidedNoiseRobustDifferentiators.pdf
|
||||
template <typename T, size_t N> struct GetFHNRCoeffs { vectN_t<T, N> operator()() const; };
|
||||
template <typename T> struct GetFHNRCoeffs<T, 4> { vectN_t<T, 4> operator()() const { return vectN_t<T, 4>{ T(2), T(-1), T(-2), T(1) } / T(2); } };
|
||||
template <typename T> struct GetFHNRCoeffs<T, 5> { vectN_t<T, 5> operator()() const { return vectN_t<T, 5>{ T(7), T(1), T(-10), T(-1), T(3) } / T(10); } };
|
||||
template <typename T> struct GetFHNRCoeffs<T, 6> { vectN_t<T, 6> operator()() const { return vectN_t<T, 6>{ T(16), T(1), T(-10), T(-10), T(-6), T(9) } / T(28); } };
|
||||
template <typename T> struct GetFHNRCoeffs<T, 7> { vectN_t<T, 7> operator()() const { return vectN_t<T, 7>{ T(12), T(5), T(-8), T(-6), T(-10), T(1), T(6) } / T(28); } };
|
||||
template <typename T> struct GetFHNRCoeffs<T, 8> { vectN_t<T, 8> operator()() const { return vectN_t<T, 8>{ T(22), T(7), T(-6), T(-11), T(-14), T(-9), T(-2), T(13) } / T(60); } };
|
||||
template <typename T> struct GetFHNRCoeffs<T, 9> { vectN_t<T, 9> operator()() const { return vectN_t<T, 9>{ T(52), T(29), T(-14), T(-17), T(-40), T(-23), T(-26), T(11), T(28) } / T(180); } };
|
||||
template <typename T> struct GetFHNRCoeffs<T, 10> { vectN_t<T, 10> operator()() const { return vectN_t<T, 10>{ 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> struct GetFHNRCoeffs<T, 11> { vectN_t<T, 11> operator()() const { return vectN_t<T, 11>{ 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> struct GetFHNRCoeffs {
|
||||
vectN_t<T, N> operator()() const;
|
||||
};
|
||||
template <typename T> struct GetFHNRCoeffs<T, 4> {
|
||||
vectN_t<T, 4> operator()() const { return (vectN_t<T, 4>() << T(2), T(-1), T(-2), T(1)).finished() / T(2); }
|
||||
};
|
||||
template <typename T> struct GetFHNRCoeffs<T, 5> {
|
||||
vectN_t<T, 5> operator()() const { return (vectN_t<T, 5>() << T(7), T(1), T(-10), T(-1), T(3)).finished() / T(10); }
|
||||
};
|
||||
template <typename T> struct GetFHNRCoeffs<T, 6> {
|
||||
vectN_t<T, 6> operator()() const { return (vectN_t<T, 6>() << T(16), T(1), T(-10), T(-10), T(-6), T(9)).finished() / T(28); }
|
||||
};
|
||||
template <typename T> struct GetFHNRCoeffs<T, 7> {
|
||||
vectN_t<T, 7> operator()() const { return (vectN_t<T, 7>() << T(12), T(5), T(-8), T(-6), T(-10), T(1), T(6)).finished() / T(28); }
|
||||
};
|
||||
template <typename T> struct GetFHNRCoeffs<T, 8> {
|
||||
vectN_t<T, 8> operator()() const { return (vectN_t<T, 8>() << T(22), T(7), T(-6), T(-11), T(-14), T(-9), T(-2), T(13)).finished() / T(60); }
|
||||
};
|
||||
template <typename T> struct GetFHNRCoeffs<T, 9> {
|
||||
vectN_t<T, 9> operator()() const { return (vectN_t<T, 9>() << T(52), T(29), T(-14), T(-17), T(-40), T(-23), T(-26), T(11), T(28)).finished() / T(180); }
|
||||
};
|
||||
template <typename T> struct GetFHNRCoeffs<T, 10> {
|
||||
vectN_t<T, 10> operator()() const { return (vectN_t<T, 10>() << T(56), T(26), T(-2), T(-17), T(-30), T(-30), T(-28), T(-13), T(4), T(34)).finished() / T(220); }
|
||||
};
|
||||
template <typename T> struct GetFHNRCoeffs<T, 11> {
|
||||
vectN_t<T, 11> operator()() const { return (vectN_t<T, 11>() << T(320), T(206), T(-8), T(-47), T(-186), T(-150), T(-214), T(-103), T(-92), T(94), T(180)).finished() / T(1540); }
|
||||
};
|
||||
template <typename T> struct GetFHNRCoeffs<T, 16> {
|
||||
vectN_t<T, 16> operator()() const { return (vectN_t<T, 16>() << T(322), T(217), T(110), T(35), T(-42), T(-87), T(-134), T(-149), T(-166), T(-151), T(-138), T(-93), T(-50), T(28), T(98), T(203)).finished() / T(2856); }
|
||||
};
|
||||
|
||||
template <typename T, size_t N, typename ForwardCoeffs> vectN_t<T, N> GetForwardISDCoeffs()
|
||||
template <typename T, size_t N, typename BackwardCoeffs> vectN_t<T, N> GetBackwardISDCoeffs()
|
||||
{
|
||||
vectN_t<T, N> v{};
|
||||
const vectN_t<T, N> v0 = ForwardCoeffs{}();
|
||||
const vectN_t<T, N> v0 = BackwardCoeffs{}();
|
||||
for (Eigen::Index k = 0; k < N; ++k)
|
||||
v(k) = k * v0(k);
|
||||
return v(k);
|
||||
}
|
||||
// Backward Noise-Robust differentiators for irregular space data
|
||||
template <typename T, size_t N> struct GetFNRISDCoeffs { vectN_t<T, N> operator()() const { return GetForwardISDCoeffs<T, N, GetFNRCoeffs<T, N>>(); } };
|
||||
template <typename T, size_t N> struct GetFNRISDCoeffs {
|
||||
vectN_t<T, N> operator()() const { return GetBackwardISDCoeffs<T, N, GetFNRCoeffs<T, N>>(); }
|
||||
};
|
||||
// Backward Hybrid Noise-Robust differentiators for irregular space data
|
||||
template <typename T, size_t N> struct GetFHNRISDCoeffs { vectN_t<T, N> operator()() const { return GetForwardISDCoeffs<T, N, GetFHNRCoeffs<T, N>>(); } };
|
||||
template <typename T, size_t N> struct GetFHNRISDCoeffs {
|
||||
vectN_t<T, N> operator()() const { return GetBackwardISDCoeffs<T, N, GetFHNRCoeffs<T, N>>(); }
|
||||
};
|
||||
|
||||
// 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>
|
||||
struct GetCNR2Coeffs {
|
||||
vectN_t<T, N> operator()() const
|
||||
{
|
||||
static_assert(N % 2 == 1. "'N' must be odd.");
|
||||
return GetFNRCoeffs<T, N>{}(); // Same coefficients
|
||||
}
|
||||
vectN_t<T, N> operator()() const
|
||||
{
|
||||
static_assert(N % 2 == 1., "'N' must be odd.");
|
||||
return GetFNRCoeffs<T, N>{}(); // Same coefficients
|
||||
}
|
||||
};
|
||||
|
||||
// Centered Noise-Robust differentiators (tangency at 4th order): http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
|
||||
template <typename T, size_t N> struct GetCNR4Coeffs { vectN_t<T, N> operator()() const; };
|
||||
template <typename T> struct GetCNR4Coeffs<T, 7> { vectN_t<T, 7> operator()() const { return vectN_t<T, 7>{ T(-5), T(12), T(39), T(0), T(-39), T(-12), T(5) } / T(96); } };
|
||||
template <typename T> struct GetCNR4Coeffs<T, 9> { vectN_t<T, 9> operator()() const { return vectN_t<T, 9>{ T(-2), T(-1), T(16), T(27), T(0), T(-27), T(-16), T(1), T(2) } / T(96); } };
|
||||
template <typename T> struct GetCNR4Coeffs<T, 11> { vectN_t<T, 11> operator()() const { return vectN_t<T, 11>{ T(-11), T(-32), T(39), T(256), T(322), T(0), T(-322), T(-256), T(-39), T(32), T(11) } / T(1536); } };
|
||||
template <typename T, size_t N> struct GetCNR4Coeffs {
|
||||
vectN_t<T, N> operator()() const;
|
||||
};
|
||||
template <typename T> struct GetCNR4Coeffs<T, 7> {
|
||||
vectN_t<T, 7> operator()() const { return (vectN_t<T, 7>() << T(-5), T(12), T(39), T(0), T(-39), T(-12), T(5)).finished() / T(96); }
|
||||
};
|
||||
template <typename T> struct GetCNR4Coeffs<T, 9> {
|
||||
vectN_t<T, 9> operator()() const { return (vectN_t<T, 9>() << T(-2), T(-1), T(16), T(27), T(0), T(-27), T(-16), T(1), T(2)).finished() / T(96); }
|
||||
};
|
||||
template <typename T> struct GetCNR4Coeffs<T, 11> {
|
||||
vectN_t<T, 11> operator()() const { return (vectN_t<T, 11>() << T(-11), T(-32), T(39), T(256), T(322), T(0), T(-322), T(-256), T(-39), T(32), T(11)).finished() / 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, typename CNRCoeffs> vectN_t<T, N> GetCNRISDCoeffs()
|
||||
|
@ -149,10 +195,15 @@ template <typename T, size_t N, typename CNRCoeffs> vectN_t<T, N> GetCNRISDCoeff
|
|||
v(M - k) = T(2) * k * v0(M - k);
|
||||
v(M + k) = T(2) * k * v0(M + k);
|
||||
}
|
||||
return v(k);
|
||||
|
||||
return v;
|
||||
}
|
||||
template <typename T, size_t N> struct GetCNR2ISDCoeffs { vectN_t<T, N> operator()() const { return GetCNRISDCoeffs<T, N, GetCNR2Coeffs<T, N>>(); } };
|
||||
template <typename T, size_t N> struct GetCNR4ISDCoeffs { vectN_t<T, N> operator()() const { return GetCNRISDCoeffs<T, N, GetCNR4Coeffs<T, N>>(); } };
|
||||
template <typename T, size_t N> struct GetCNR2ISDCoeffs {
|
||||
vectN_t<T, N> operator()() const { return GetCNRISDCoeffs<T, N, GetCNR2Coeffs<T, N>>(); }
|
||||
};
|
||||
template <typename T, size_t N> struct GetCNR4ISDCoeffs {
|
||||
vectN_t<T, N> operator()() const { return GetCNRISDCoeffs<T, N, GetCNR4Coeffs<T, N>>(); }
|
||||
};
|
||||
|
||||
/*
|
||||
* Second order differentiators
|
||||
|
@ -163,109 +214,113 @@ constexpr T GetSONRBaseCoeff(size_t N, size_t M, size_t k)
|
|||
{
|
||||
if (k > M)
|
||||
return T(0);
|
||||
else if (k = M)
|
||||
else if (k == M)
|
||||
return T(1);
|
||||
else
|
||||
return ((T(2) * N - T(10)) * GetSONRBaseCoeff<T, N, M>(k + 1) - (N + T(2) * k + T(3) * GetSONRBaseCoeff<T, N, M>(k + 2))) / (N - T(2) * k - T(1));
|
||||
return ((T(2) * N - T(10)) * GetSONRBaseCoeff<T>(N, M, k + 1) - (N + T(2) * k + T(3)) * GetSONRBaseCoeff<T>(N, M, k + 2)) / (N - T(2) * k - T(1));
|
||||
}
|
||||
|
||||
template <typename T, size_t N> vectN_t<T, N> GetSONRBaseCoeffs()
|
||||
template <typename T, size_t N> vectN_t<T, (N - 1) / 2 + 1> GetSONRBaseCoeffs()
|
||||
{
|
||||
static_assert(N >= 5 && N % 2 == 1);
|
||||
static_assert(N >= 5 && N % 2 == 1, "N must be a odd number >= 5");
|
||||
constexpr const size_t M = (N - 1) / 2;
|
||||
vectN_t<T, M + 1> v{};
|
||||
vectN_t<T, M + 1> s{};
|
||||
for (size_t k = 0; k < M + 1; ++k)
|
||||
v(k) = GetSONRBaseCoeff(N, M, k);
|
||||
s(k) = GetSONRBaseCoeff<T>(N, M, k);
|
||||
|
||||
return v;
|
||||
return s;
|
||||
}
|
||||
|
||||
// Second-Order Centered Noise-Robust differentiator: http://www.holoborodko.com/pavel/downloads/NoiseRobustSecondDerivative.pdf
|
||||
template <typename T, size_t N>
|
||||
template <typename T, size_t N>
|
||||
struct GetSOCNRCoeffs {
|
||||
vectN_t<T, N> operator()() const
|
||||
{
|
||||
constexpr const size_t M = (N - 1) / 2;
|
||||
constexpr const T Den = pow(size_t(2), N - 3);
|
||||
vectN_t<T, N> v{};
|
||||
vectN_t<T, N> s = GetSONRBaseCoeffs<T, N>();
|
||||
v(M) = s(0);
|
||||
for (size_t k = 1; k < M; ++k) {
|
||||
v(M + k) = s(k);
|
||||
v(M - k) = s(k);
|
||||
}
|
||||
vectN_t<T, N> operator()() const
|
||||
{
|
||||
constexpr const size_t M = (N - 1) / 2;
|
||||
constexpr const T Den = pow(size_t(2), N - 3);
|
||||
vectN_t<T, N> v{};
|
||||
vectN_t<T, M + 1> s = GetSONRBaseCoeffs<T, N>();
|
||||
v(M) = s(0);
|
||||
for (size_t k = 1; k < M + 1; ++k) {
|
||||
v(M + k) = s(k);
|
||||
v(M - k) = s(k);
|
||||
}
|
||||
|
||||
v /= Den;
|
||||
return v;
|
||||
}
|
||||
v /= Den;
|
||||
return v;
|
||||
}
|
||||
};
|
||||
// Second-Order Backward Noise-Robust differentiator: http://www.holoborodko.com/pavel/downloads/NoiseRobustSecondDerivative.pdf
|
||||
template <typename T, size_t N> struct GetSOFNRCoeffs { vectN_t<T, N> operator()() const { return GetSOCNRCoeffs<T, N>(); } }; // Coefficients are the same.
|
||||
template <typename T, size_t N> struct GetSOFNRCoeffs {
|
||||
vectN_t<T, N> operator()() const { return GetSOCNRCoeffs<T, N>(); }
|
||||
}; // Coefficients are the same.
|
||||
|
||||
// Second-Order Centered Noise-Robust Irregular Space Data differentiator: http://www.holoborodko.com/pavel/downloads/NoiseRobustSecondDerivative.pdf
|
||||
template <typename T, size_t N>
|
||||
template <typename T, size_t N>
|
||||
struct GetSOCNRISDCoeffs {
|
||||
vectN_t<T, N> operator()() const
|
||||
{
|
||||
constexpr const size_t M = (N - 1) / 2;
|
||||
constexpr const T Den = pow(size_t(2), N - 3);
|
||||
vectN_t<T, N> operator()() const
|
||||
{
|
||||
constexpr const size_t M = (N - 1) / 2;
|
||||
constexpr const T Den = pow(size_t(2), N - 3);
|
||||
|
||||
vectN_t<T, N> v{};
|
||||
vectN_t<T, N> s = GetSONRBaseCoeffs<T, N>();
|
||||
vectN_t<T, N> v{};
|
||||
vectN_t<T, N> s = GetSONRBaseCoeffs<T, N>();
|
||||
|
||||
constexpr const alpha = [&s](size_t k) -> T { return T(4) * k * k * s(k) };
|
||||
constexpr const alpha = [&s](size_t k) -> T { return T(4) * k * k * s(k) };
|
||||
|
||||
v(M) = -T(2) * alpha(0);
|
||||
for (size_t k = 1; k < M; ++k) {
|
||||
v(M + k) = alpha(k);
|
||||
v(M - k) = alpha(k);
|
||||
v(M) = -T(2) * alpha(0);
|
||||
for (size_t k = 1; k < M; ++k) {
|
||||
v(M + k) = alpha(k);
|
||||
v(M - k) = alpha(k);
|
||||
}
|
||||
|
||||
v /= Den;
|
||||
return v;
|
||||
}
|
||||
|
||||
v /= Den;
|
||||
return v;
|
||||
}
|
||||
};
|
||||
|
||||
// Second-Order Backward Noise-Robust Irregular Space Data differentiator: http://www.holoborodko.com/pavel/downloads/NoiseRobustSecondDerivative.pdf
|
||||
template <typename T, size_t N> struct GetSOFNRISDCoeffs { vectN_t<T, N> operator()() const { return GetSOCNRISDCoeffs<T, N>(); } }; // Same coefficients
|
||||
template <typename T, size_t N> struct GetSOFNRISDCoeffs {
|
||||
vectN_t<T, N> operator()() const { return GetSOCNRISDCoeffs<T, N>(); }
|
||||
}; // Same coefficients
|
||||
|
||||
/*
|
||||
* Differentiator Generator
|
||||
*/
|
||||
|
||||
template <typename T, size_t N, int Order, typename CoeffGetter>
|
||||
class ForwardDifferentiator : public GenericFilter<T> {
|
||||
class BackwardDifferentiator : public GenericFilter<T> {
|
||||
public:
|
||||
ForwardDifferentiator()
|
||||
: GenericFilter<T>({T(1)}, CoeffGetter{}())
|
||||
BackwardDifferentiator()
|
||||
: GenericFilter<T>(vectX_t<T>::Constant(1, T(1)), CoeffGetter{}())
|
||||
{}
|
||||
ForwardDifferentiator(T timestep)
|
||||
: GenericFilter<T>({T(1)}, CoeffGetter{}() / std::pow(timestep, Order))
|
||||
BackwardDifferentiator(T timestep)
|
||||
: GenericFilter<T>(vectX_t<T>::Constant(1, T(1)), CoeffGetter{}() / std::pow(timestep, Order))
|
||||
{}
|
||||
void setTimestep(T timestep) { setCoeffs({T(1)}, CoeffGetter{}() / std::pow(timestep, Order)); }
|
||||
void setTimestep(T timestep) { setCoeffs(vectX_t<T>::Constant(1, T(1)), CoeffGetter{}() / std::pow(timestep, Order)); }
|
||||
T timestep() const noexcept { return std::pow(bCoeff()(0) / CoeffGetter{}()(0), T(1) / Order); }
|
||||
};
|
||||
|
||||
template <typename T, size_t N, int Order, typename CoeffGetter>
|
||||
class CentralDifferentiator : public GenericFilter<T> {
|
||||
public:
|
||||
CentralDifferentiator()
|
||||
: GenericFilter<T>({T(1)}, CoeffGetter{}(), Type::Centered)
|
||||
CentralDifferentiator()
|
||||
: GenericFilter<T>(vectX_t<T>::Constant(1, T(1)), CoeffGetter{}(), FilterType::Centered)
|
||||
{}
|
||||
CentralDifferentiator(T timestep)
|
||||
: GenericFilter<T>({T(1)}, CoeffGetter{}() / std::pow(timestep, Order))
|
||||
: GenericFilter<T>(vectX_t<T>::Constant(1, T(1)), CoeffGetter{}() / std::pow(timestep, Order))
|
||||
{}
|
||||
void setTimestep(T timestep) { setCoeffs({T(1)}, CoeffGetter{}() / std::pow(timestep, Order)); }
|
||||
void setTimestep(T timestep) { setCoeffs(vectX_t<T>::Constant(1, T(1)), CoeffGetter{}() / std::pow(timestep, Order)); }
|
||||
T timestep() const noexcept { return std::pow(bCoeff()(0) / CoeffGetter{}()(0), T(1) / Order); }
|
||||
};
|
||||
|
||||
template <typename T, size_t N, int Order, typename CoeffGetter>
|
||||
class TVForwardDifferentiator : public TVGenericFilter<T> {
|
||||
class TVBackwardDifferentiator : public TVGenericFilter<T> {
|
||||
static_assert(Order >= 1, "Order must be greater or equal to 1");
|
||||
|
||||
public:
|
||||
TVForwardDifferentiator()
|
||||
: TVGenericFilter<T>(Order, {T(1)}, CoeffGetter{}())
|
||||
TVBackwardDifferentiator()
|
||||
: TVGenericFilter<T>(Order, vectX_t<T>::Constant(1, T(1)), CoeffGetter{}())
|
||||
{}
|
||||
};
|
||||
|
||||
|
@ -274,19 +329,19 @@ class TVCentralDifferentiator : public TVGenericFilter<T> {
|
|||
static_assert(Order >= 1, "Order must be greater or equal to 1");
|
||||
|
||||
public:
|
||||
TVCentralDifferentiator()
|
||||
: TVGenericFilter<T>(Order, {T(1)}, CoeffGetter{}(), Type::Centered)
|
||||
TVCentralDifferentiator()
|
||||
: TVGenericFilter<T>(Order, vectX_t<T>::Constant(1, T(1)), CoeffGetter{}(), FilterType::Centered)
|
||||
{}
|
||||
};
|
||||
|
||||
} // namespace details
|
||||
|
||||
// Backward differentiators
|
||||
template <typename T, size_t N> using ForwardNoiseRobustDiff = details::ForwardDifferentiator<T, N, 1, details::GetFNRCoeffs<T, N>>;
|
||||
template <typename T, size_t N> using ForwardHybridNoiseRobustDiff = details::ForwardDifferentiator<T, N, 1, details::GetFHNRCoeffs<T, N>>;
|
||||
template <typename T, size_t N> using BackwardNoiseRobustDiff = details::BackwardDifferentiator<T, N, 1, details::GetFNRCoeffs<T, N>>;
|
||||
template <typename T, size_t N> using BackwardHybridNoiseRobustDiff = details::BackwardDifferentiator<T, N, 1, details::GetFHNRCoeffs<T, N>>;
|
||||
// Time-Varying backward differentiators
|
||||
template <typename T, size_t N> using TVForwardNoiseRobustDiff = details::TVForwardDifferentiator<T, N, 1, details::GetFNRISDCoeffs<T, N>>;
|
||||
template <typename T, size_t N> using TVForwardHybridNoiseRobustDiff = details::TVForwardDifferentiator<T, N, 1, details::GetFHNRISDCoeffs<T, N>>;
|
||||
template <typename T, size_t N> using TVBackwardNoiseRobustDiff = details::TVBackwardDifferentiator<T, N, 1, details::GetFNRISDCoeffs<T, N>>;
|
||||
template <typename T, size_t N> using TVBackwardHybridNoiseRobustDiff = details::TVBackwardDifferentiator<T, N, 1, details::GetFHNRISDCoeffs<T, N>>;
|
||||
|
||||
// Central differentiators
|
||||
template <typename T, size_t N> using CentralDiff = details::CentralDifferentiator<T, N, 1, details::GetCDCoeffs<T, N>>;
|
||||
|
@ -298,16 +353,14 @@ template <typename T, size_t N> using CenteredNoiseRobust4Diff = details::Centra
|
|||
template <typename T, size_t N> using TVCenteredNoiseRobust2Diff = details::TVCentralDifferentiator<T, N, 1, details::GetCNR2ISDCoeffs<T, N>>;
|
||||
template <typename T, size_t N> using TVCenteredNoiseRobust4Diff = details::TVCentralDifferentiator<T, N, 1, details::GetCNR4ISDCoeffs<T, N>>;
|
||||
|
||||
|
||||
// Second-order backward differentiators
|
||||
template <typename T, size_t N> using ForwardSecondOrderDiff = details::ForwardDifferentiator<T, N, 2, details::GetSOFNRCoeffs<T, N>>;
|
||||
template <typename T, size_t N> using BackwardSecondOrderDiff = details::BackwardDifferentiator<T, N, 2, details::GetSOFNRCoeffs<T, N>>;
|
||||
// Second-order Time-Varying backward differentiators
|
||||
template <typename T, size_t N> using TVForwardSecondOrderDiff = details::TVForwardDifferentiator<T, N, 2, details::GetSOFNRISDCoeffs<T, N>>;
|
||||
template <typename T, size_t N> using TVBackwardSecondOrderDiff = details::TVBackwardDifferentiator<T, N, 2, details::GetSOFNRISDCoeffs<T, N>>;
|
||||
|
||||
// Second-order central differentiators
|
||||
template <typename T, size_t N> using CenteredSecondOrderDiff = details::CentralDifferentiator<T, N, 2, details::GetSOCNRCoeffs<T, N>>;
|
||||
// Second-order Time-Varying backward differentiators
|
||||
// Second-order Time-Varying central differentiators
|
||||
template <typename T, size_t N> using TVCenteredSecondOrderDiff = details::TVCentralDifferentiator<T, N, 2, details::GetSOCNRISDCoeffs<T, N>>;
|
||||
|
||||
|
||||
} // namespace difi
|
|
@ -28,35 +28,34 @@
|
|||
#pragma once
|
||||
#include <type_traits>
|
||||
|
||||
namespace difi
|
||||
{
|
||||
namespace difi {
|
||||
|
||||
// https://stackoverflow.com/questions/44718971/calculate-binomial-coffeficient-very-reliably
|
||||
template <typename T>
|
||||
constexpr T Binomial(int n, int k)
|
||||
constexpr T Binomial(size_t n, size_t k)
|
||||
{
|
||||
if (k > n)
|
||||
return T(0);
|
||||
else if (k == 0 || k == n)
|
||||
return T(1);
|
||||
else if (k == 1 || k == n-1)
|
||||
else if (k == 1 || k == n - 1)
|
||||
return T(n);
|
||||
else if ( 2 * k < n)
|
||||
return Binomial<T>(n-1, k - 1) * n / k;
|
||||
else if (2 * k < n)
|
||||
return Binomial<T>(n - 1, k - 1) * n / k;
|
||||
else
|
||||
return Binomial<T>(n-1, k) * n / (n - k);
|
||||
return Binomial<T>(n - 1, k) * n / (n - k);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr T pow(T n, T k)
|
||||
{
|
||||
static_assert(std::is_integral_v<T>);
|
||||
template <typename T>
|
||||
constexpr T pow(T n, T k)
|
||||
{
|
||||
static_assert(std::is_integral_v<T>, "For integral values only, eitherwise, use std::pow");
|
||||
if (n == 0)
|
||||
return (k == 0) ? 1 : 0;
|
||||
else if (k == 0 || k == 1)
|
||||
else if (k == 0)
|
||||
return T(1);
|
||||
else
|
||||
return n * pow(n, k - 1);
|
||||
return n * pow(n, k - 1);
|
||||
}
|
||||
|
||||
} // namespace difi
|
||||
|
|
|
@ -33,7 +33,7 @@ macro(addTest testName)
|
|||
add_executable(${testName} ${testName}.cpp)
|
||||
target_link_libraries(${testName} PRIVATE Catch2::Catch2)
|
||||
target_compile_definitions(${testName} PRIVATE CATCH_CONFIG_MAIN)
|
||||
target_include_directories(${testName} SYSTEM PUBLIC "${EIGEN3_INCLUDE_DIR}")
|
||||
target_include_directories(${testName} SYSTEM PRIVATE "${EIGEN3_INCLUDE_DIR}")
|
||||
# Adding a project configuration file (for MSVC only)
|
||||
generate_msvc_dot_user_file(${testName})
|
||||
|
||||
|
|
|
@ -43,6 +43,23 @@ TEST_CASE("Filter failures", "[fail]")
|
|||
auto df = difi::DigitalFilterd();
|
||||
// Filter data with uninitialized filter
|
||||
REQUIRE_THROWS_AS(df.stepFilter(10.), std::logic_error);
|
||||
// Change type before settings coeffs (The difi::FilterType::Centered needs to have odd number of bCoeffs)
|
||||
REQUIRE_THROWS_AS(df.setType(difi::FilterType::Centered), std::logic_error);
|
||||
// Set type to difi::FilterType::Backward is always ok.
|
||||
REQUIRE_NOTHROW(df.setType(difi::FilterType::Backward));
|
||||
// Set even number of bCoeffs
|
||||
REQUIRE_NOTHROW(df.setCoeffs(Eigen::VectorXd::Constant(2, 1), Eigen::VectorXd::Constant(2, 0)));
|
||||
// Check again
|
||||
REQUIRE_THROWS_AS(df.setType(difi::FilterType::Centered), std::logic_error);
|
||||
// Set odd number of bCoeffs
|
||||
REQUIRE_NOTHROW(df.setCoeffs(Eigen::VectorXd::Constant(2, 1), Eigen::VectorXd::Constant(3, 0)));
|
||||
// Check again
|
||||
REQUIRE_NOTHROW(df.setType(difi::FilterType::Centered));
|
||||
// Put even number of bCoeffs on a Centered filter
|
||||
REQUIRE_THROWS_AS(df.setCoeffs(Eigen::VectorXd::Constant(2, 1), Eigen::VectorXd::Constant(2, 0)), std::logic_error);
|
||||
// Put odd number of bCoeffs
|
||||
REQUIRE_NOTHROW(df.setCoeffs(Eigen::VectorXd::Constant(5, 2), Eigen::VectorXd::Constant(7, 3)));
|
||||
|
||||
// window <= 0
|
||||
REQUIRE_THROWS_AS(difi::MovingAveraged(0), std::logic_error);
|
||||
// order <= 0
|
||||
|
@ -54,4 +71,6 @@ TEST_CASE("Filter failures", "[fail]")
|
|||
|
||||
// Ok
|
||||
REQUIRE_NOTHROW(difi::DigitalFilterd(Eigen::VectorXd::Constant(2, 1), Eigen::VectorXd::Constant(2, 0)));
|
||||
// Bad type. Need odd number of bCoeffs
|
||||
REQUIRE_THROWS_AS(difi::DigitalFilterd(Eigen::VectorXd::Constant(2, 1), Eigen::VectorXd::Constant(2, 0), difi::FilterType::Centered), std::logic_error);
|
||||
}
|
||||
|
|
|
@ -0,0 +1,162 @@
|
|||
// Copyright (c) 2019, Vincent SAMY
|
||||
// All rights reserved.
|
||||
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
|
||||
// 1. Redistributions of source code must retain the above copyright notice,
|
||||
// this list of conditions and the following disclaimer.
|
||||
// 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
// this list of conditions and the following disclaimer in the documentation
|
||||
// and/or other materials provided with the distribution.
|
||||
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
|
||||
// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
// The views and conclusions contained in the software and documentation are those
|
||||
// of the authors and should not be interpreted as representing official policies,
|
||||
// either expressed or implied, of the FreeBSD Project.
|
||||
|
||||
#pragma once
|
||||
#include "catch_helper.h"
|
||||
#include "differentiators.h"
|
||||
#include "difi"
|
||||
#include <array>
|
||||
#include <catch2/catch.hpp>
|
||||
|
||||
using namespace difi;
|
||||
|
||||
template <typename T, typename Tuple, typename Derived>
|
||||
class DiffTester {
|
||||
friend Derived;
|
||||
static constexpr const size_t TSize = std::tuple_size_v<Tuple>;
|
||||
|
||||
public:
|
||||
DiffTester(const Tuple& filters, const vectX_t<T>& f, const vectX_t<T>& df)
|
||||
: m_filters(filters)
|
||||
, m_f(f)
|
||||
, m_df(df)
|
||||
{}
|
||||
|
||||
void run(const std::array<T, TSize>& eps) { testRunner<TSize - 1>(eps); }
|
||||
|
||||
private:
|
||||
template <typename Filter>
|
||||
void test(Filter& filt, T eps)
|
||||
{
|
||||
static_cast<Derived&>(*this).testFilter(filt, eps);
|
||||
}
|
||||
|
||||
template <size_t Index>
|
||||
void testRunner(const std::array<T, TSize>& eps)
|
||||
{
|
||||
test(std::get<Index>(m_filters), eps[Index]);
|
||||
testRunner<Index - 1>(eps);
|
||||
}
|
||||
|
||||
template <>
|
||||
void testRunner<0>(const std::array<T, TSize>& eps) { test(std::get<0>(m_filters), eps[0]); }
|
||||
|
||||
private:
|
||||
vectX_t<T> m_f;
|
||||
vectX_t<T> m_df;
|
||||
Tuple m_filters;
|
||||
};
|
||||
|
||||
template <typename T, typename Tuple>
|
||||
class TFDiffTester : public DiffTester<T, Tuple, TFDiffTester<T, Tuple>> {
|
||||
friend DiffTester<T, Tuple, TFDiffTester<T, Tuple>>;
|
||||
|
||||
public:
|
||||
TFDiffTester(const Tuple& filters, const vectX_t<T>& f, const vectX_t<T>& df)
|
||||
: DiffTester(filters, f, df)
|
||||
{}
|
||||
|
||||
void setFilterTimestep(T step) { setStep<TSize - 1>(step); }
|
||||
|
||||
private:
|
||||
template <typename Filter>
|
||||
void testFilter(Filter& filt, T eps)
|
||||
{
|
||||
for (int i = 0; i < 50; ++i) {
|
||||
auto value = filt.stepFilter(m_f(i)); // First initialize, some steps
|
||||
value = 2.;
|
||||
}
|
||||
|
||||
for (int i = 50; i < STEPS; ++i) {
|
||||
auto value = filt.stepFilter(m_f(i));
|
||||
REQUIRE_SMALL(std::abs(value - m_df(i - filt.center())), eps);
|
||||
}
|
||||
}
|
||||
|
||||
template <size_t Index>
|
||||
void setStep(T step)
|
||||
{
|
||||
std::get<Index>(m_filters).setTimestep(step);
|
||||
setStep<Index - 1>(step);
|
||||
}
|
||||
|
||||
template <>
|
||||
void setStep<0>(T step) { std::get<0>(m_filters).setTimestep(step); }
|
||||
};
|
||||
|
||||
template <typename T, typename Tuple>
|
||||
class TVDiffTester : public DiffTester<T, Tuple, TVDiffTester<T, Tuple>> {
|
||||
friend DiffTester<T, Tuple, TVDiffTester<T, Tuple>>;
|
||||
|
||||
public:
|
||||
TVDiffTester(const Tuple& filters, const vectX_t<T>& t, const vectX_t<T>& f, const vectX_t<T>& df)
|
||||
: DiffTester(filters, f, df)
|
||||
, m_time(t)
|
||||
{}
|
||||
|
||||
private:
|
||||
template <typename Filter>
|
||||
void testFilter(Filter& filt, T eps)
|
||||
{
|
||||
for (int i = 0; i < 50; ++i) {
|
||||
auto value = filt.stepFilter(m_time(i), m_f(i)); // First initialize, some steps
|
||||
value = 2.;
|
||||
}
|
||||
|
||||
for (int i = 50; i < STEPS; ++i) {
|
||||
auto value = filt.stepFilter(m_time(i), m_f(i));
|
||||
REQUIRE_SMALL(std::abs(value - m_df(i - filt.center())), eps);
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
vectX_t<T> m_time;
|
||||
};
|
||||
|
||||
template <typename T, size_t N>
|
||||
using central_list = std::tuple<CentralDiff<T, N>, LowNoiseLanczosDiff<T, N>, SuperLowNoiseLanczosDiff<T, N>, CenteredNoiseRobust2Diff<T, N>, CenteredNoiseRobust4Diff<T, N>>;
|
||||
|
||||
template <typename T, size_t N>
|
||||
TFDiffTester<T, central_list<T, N>> generateCTester(const vectX_t<T>& f, const vectX_t<T>& df)
|
||||
{
|
||||
return { central_list<T, N>{}, f, df };
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
TFDiffTester<T, std::tuple<CenteredSecondOrderDiff<T, N>>> generateC2OTester(const vectX_t<T>& f, const vectX_t<T>& df)
|
||||
{
|
||||
return { std::tuple<CenteredSecondOrderDiff<T, N>>{}, f, df };
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
using tv_central_list = std::tuple<TVCenteredNoiseRobust2Diff<T, N>, TVCenteredNoiseRobust4Diff<T, N>>;
|
||||
|
||||
template <typename T, size_t N>
|
||||
TVDiffTester<T, tv_central_list<T, N>> generateTVCTester(const vectX_t<T>& t, const vectX_t<T>& f, const vectX_t<T>& df)
|
||||
{
|
||||
return { tv_central_list<T, N>{}, t, f, df };
|
||||
}
|
|
@ -26,71 +26,129 @@
|
|||
// either expressed or implied, of the FreeBSD Project.
|
||||
|
||||
#pragma once
|
||||
#include "differentiators.h"
|
||||
#include "catch_helper.h"
|
||||
#include "diffTesters.h"
|
||||
#include "noisy_function_generator.h"
|
||||
#include <catch2/catch.hpp>
|
||||
#include <limits>
|
||||
|
||||
constexpr const int STEPS = 200;
|
||||
constexpr const int SIN_FREQUENCY = 60;
|
||||
constexpr const int STEPS = 500;
|
||||
constexpr const int SIN_AMPLITUDE = 100;
|
||||
constexpr const int SIN_FREQUENCY = 1;
|
||||
template <typename T> constexpr const std::array<T, 5> POLY_4 = { 2, -3, 3, 2, -5 };
|
||||
template <typename T> constexpr const std::array<T, 8> POLY_7 = { 10, -12, 5, 6, -9, -3, -2, 4 };
|
||||
|
||||
using namespace difi;
|
||||
|
||||
template <typename T>
|
||||
struct TestFun {
|
||||
vectX_t<T> f;
|
||||
vectX_t<T> d;
|
||||
int center;
|
||||
double eps;
|
||||
};
|
||||
|
||||
template <typename T, typename Filter>
|
||||
struct TestRunner {
|
||||
void operator()(const TestFun<T>& tf, Filter& f)
|
||||
{
|
||||
for (int i = 0; i < 50; ++i)
|
||||
f.step(tf.f(i)); // First initialize, some steps
|
||||
|
||||
for (int i = 20; i < STEPS; ++i)
|
||||
REQUIRE_SMALL(std::abs(f.step(tf.f(i)) - tf.d(i - tf.center)), tf.eps);
|
||||
}
|
||||
};
|
||||
|
||||
template <typename T, size_t Ind, typename... Ts>
|
||||
struct Tester {
|
||||
void operator()(const TestFun<T>& tf, std::tuple<Ts...> f)
|
||||
{
|
||||
TestRunner(tf, std::get<Ind>(f));
|
||||
Tester<T, Ind - 1, Ts...>(tf, f);
|
||||
}
|
||||
};
|
||||
|
||||
template <typename T, typename... Ts>
|
||||
struct Tester<T, 0, Ts...> {
|
||||
void operator()(const TestFun<T>& tf, std::tuple<Ts...> f)
|
||||
{
|
||||
TestRunner(tf, std::get<0>(f));
|
||||
}
|
||||
};
|
||||
|
||||
template <typename T, size_t N>
|
||||
using centralList = std::tuple<CentralDiff<T, N>, LowNoiseLanczosDiff<T, N>, SuperLowNoiseLanczosDiff<T, N>, CenteredNoiseRobust2Diff<T, N>, CenteredNoiseRobust4Diff<T, N>>;
|
||||
|
||||
TEMPLATE_TEST_CASE("Sinus time-fixed central derivative", "[sin][central]", float, double)
|
||||
template <size_t N>
|
||||
vectN_t<double, N> generateLNLCoeffs()
|
||||
{
|
||||
static_assert(N > 2 && N % 2 == 1, "N must be odd");
|
||||
vectN_t<double, N> lnl;
|
||||
const size_t M = (N - 1) / 2;
|
||||
for (size_t i = 0; i < M + 1; ++i) {
|
||||
lnl(i) = static_cast<double>(M - i);
|
||||
lnl(M + i) = -static_cast<double>(i);
|
||||
}
|
||||
|
||||
FunctionGenerator<TestType> fg = sinGenerator<TestType>(STEPS, SIN_FREQUENCY);
|
||||
|
||||
auto list7 = centralList<TestType, 7>{};
|
||||
TestFun<TestType> list7Param = {std::get<0>(fg), std::get<2>(fg), 3, std::numeric_limits<TestType>::epsilon() * 100};
|
||||
TestFun<TestType> list7NoisyParam = {std::get<1>(fg), std::get<2>(fg), 3, std::numeric_limits<TestType>::epsilon() * 100};
|
||||
auto list9 = centralList<TestType, 9>{};
|
||||
TestFun<TestType> list9Param = {std::get<0>(fg), std::get<2>(fg), 4, std::numeric_limits<TestType>::epsilon() * 100};
|
||||
TestFun<TestType> list9NoisyParam = {std::get<1>(fg), std::get<2>(fg), 4, std::numeric_limits<TestType>::epsilon() * 100};
|
||||
|
||||
// Check no noisy function
|
||||
|
||||
|
||||
// Check for noisy function
|
||||
|
||||
return lnl;
|
||||
}
|
||||
|
||||
template <size_t N>
|
||||
void checkCoeffs(const vectN_t<double, N>& coeffs, const vectN_t<double, N>& goodCoeffs)
|
||||
{
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
REQUIRE_SMALL(std::abs(coeffs(i) - goodCoeffs(i)), std::numeric_limits<double>::epsilon() * 5);
|
||||
}
|
||||
|
||||
TEST_CASE("Coefficient calculation", "[coeff]") // Some coeffs are computed, the rest are given
|
||||
{
|
||||
// LNL coeffs
|
||||
checkCoeffs<5>(details::GetLNLCoeffs<double, 5>{}(), generateLNLCoeffs<5>() / 10.);
|
||||
checkCoeffs<7>(details::GetLNLCoeffs<double, 7>{}(), generateLNLCoeffs<7>() / 28.);
|
||||
checkCoeffs<9>(details::GetLNLCoeffs<double, 9>{}(), generateLNLCoeffs<9>() / 60.);
|
||||
checkCoeffs<11>(details::GetLNLCoeffs<double, 11>{}(), generateLNLCoeffs<11>() / 110.);
|
||||
|
||||
// FNR coeffs
|
||||
checkCoeffs<3>(details::GetFNRCoeffs<double, 3>{}(), (vectN_t<double, 3>() << 1., 0., -1.).finished() / 2.);
|
||||
checkCoeffs<4>(details::GetFNRCoeffs<double, 4>{}(), (vectN_t<double, 4>() << 1., 1., -1., -1.).finished() / 4.);
|
||||
checkCoeffs<5>(details::GetFNRCoeffs<double, 5>{}(), (vectN_t<double, 5>() << 1., 2., 0., -2., -1.).finished() / 8.);
|
||||
checkCoeffs<6>(details::GetFNRCoeffs<double, 6>{}(), (vectN_t<double, 6>() << 1., 3., 2., -2., -3., -1.).finished() / 16.);
|
||||
checkCoeffs<7>(details::GetFNRCoeffs<double, 7>{}(), (vectN_t<double, 7>() << 1., 4., 5., 0., -5., -4., -1.).finished() / 32.);
|
||||
checkCoeffs<8>(details::GetFNRCoeffs<double, 8>{}(), (vectN_t<double, 8>() << 1., 5., 9., 5., -5., -9., -5., -1.).finished() / 64.);
|
||||
checkCoeffs<9>(details::GetFNRCoeffs<double, 9>{}(), (vectN_t<double, 9>() << 1., 6., 14., 14., 0., -14., -14., -6., -1.).finished() / 128.);
|
||||
checkCoeffs<10>(details::GetFNRCoeffs<double, 10>{}(), (vectN_t<double, 10>() << 1., 7., 20., 28., 14., -14., -28., -20., -7., -1.).finished() / 256.);
|
||||
checkCoeffs<11>(details::GetFNRCoeffs<double, 11>{}(), (vectN_t<double, 11>() << 1., 8., 27., 48., 42., 0., -42., -48., -27., -8., -1.).finished() / 512.);
|
||||
}
|
||||
|
||||
// TEST_CASE("Sinus time-fixed central derivative", "[sin][central][1st]")
|
||||
// {
|
||||
// double dt = 0.001;
|
||||
// auto sg = sinGenerator<double>(STEPS, SIN_AMPLITUDE, SIN_FREQUENCY, dt);
|
||||
// auto ct7 = generateCTester<double, 7>(std::get<0>(sg), std::get<1>(sg));
|
||||
// auto ct9 = generateCTester<double, 9>(std::get<0>(sg), std::get<1>(sg));
|
||||
|
||||
// ct7.setFilterTimestep(dt);
|
||||
// ct9.setFilterTimestep(dt);
|
||||
|
||||
// std::array<double, 5> eps = { 1e-10, 1e-1, 1e-6, 1e-1, 1e-6 }; // Value checked with MATLAB
|
||||
// ct7.run(eps);
|
||||
// ct9.run(eps);
|
||||
// }
|
||||
|
||||
// TEST_CASE("Polynome time-fixed central derivative", "[poly][central][1st]")
|
||||
// {
|
||||
// double dt = 0.001;
|
||||
// {
|
||||
// auto pg4 = polyGenerator<double>(STEPS, POLY_4<double>, dt);
|
||||
// auto ct7 = generateCTester<double, 7>(std::get<0>(pg4), std::get<1>(pg4));
|
||||
// auto ct9 = generateCTester<double, 9>(std::get<0>(pg4), std::get<1>(pg4));
|
||||
|
||||
// ct7.setFilterTimestep(dt);
|
||||
// ct9.setFilterTimestep(dt);
|
||||
|
||||
// std::array<double, 5> eps = { 1e-12, 1e-4, 1e-12, 1e-4, 1e-12 }; // Value checked with MATLAB
|
||||
// ct7.run(eps);
|
||||
// ct9.run(eps);
|
||||
// }
|
||||
// {
|
||||
// auto pg7 = polyGenerator<double>(STEPS, POLY_7<double>, dt);
|
||||
// auto ct7 = generateCTester<double, 7>(std::get<0>(pg7), std::get<1>(pg7));
|
||||
// auto ct9 = generateCTester<double, 9>(std::get<0>(pg7), std::get<1>(pg7));
|
||||
|
||||
// ct7.setFilterTimestep(dt);
|
||||
// ct9.setFilterTimestep(dt);
|
||||
|
||||
// std::array<double, 5> eps = { 1e-11, 1e-3, 1e-9, 1e-4, 1e-9 }; // Value checked with MATLAB
|
||||
// ct7.run(eps);
|
||||
// ct9.run(eps);
|
||||
// }
|
||||
// }
|
||||
|
||||
// TEST_CASE("2nd order sinus time-fixed center derivative", "[sin][center][2nd]")
|
||||
// {
|
||||
// double dt = 0.001;
|
||||
// auto sg = sinGenerator<double>(STEPS, SIN_AMPLITUDE, SIN_FREQUENCY, dt);
|
||||
// auto ct7 = generateC2OTester<double, 7>(std::get<0>(sg), std::get<2>(sg));
|
||||
// auto ct9 = generateC2OTester<double, 9>(std::get<0>(sg), std::get<2>(sg));
|
||||
// auto ct11 = generateC2OTester<double, 11>(std::get<0>(sg), std::get<2>(sg));
|
||||
|
||||
// ct7.setFilterTimestep(dt);
|
||||
// ct9.setFilterTimestep(dt);
|
||||
// ct11.setFilterTimestep(dt);
|
||||
|
||||
// std::array<double, 1> eps = { 2e-1 }; // Value checked with MATLAB
|
||||
// ct7.run(eps);
|
||||
// ct9.run(eps);
|
||||
// ct11.run(eps);
|
||||
// }
|
||||
|
||||
TEST_CASE("Sinus time-varying central derivative", "[tv][sin][central][1st]")
|
||||
{
|
||||
double dt = 0.001;
|
||||
auto sg = tvSinGenerator<double>(STEPS, SIN_AMPLITUDE, SIN_FREQUENCY, dt);
|
||||
auto ct7 = generateTVCTester<double, 7>(std::get<0>(sg), std::get<1>(sg), std::get<2>(sg));
|
||||
auto ct9 = generateTVCTester<double, 9>(std::get<0>(sg), std::get<1>(sg), std::get<2>(sg));
|
||||
|
||||
std::array<double, 2> eps = { 1e-1, 1e-1 }; // Value checked with MATLAB
|
||||
ct7.run(eps);
|
||||
ct9.run(eps);
|
||||
}
|
||||
|
|
|
@ -28,77 +28,170 @@
|
|||
#pragma once
|
||||
|
||||
#include "typedefs.h"
|
||||
#include <tuple>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <random>
|
||||
#include <tuple>
|
||||
|
||||
template <typename T>
|
||||
using FunctionGenerator = std::tuple<difi::vectX_t<T>, difi::vectX_t<T>, difi::vectX_t<T>>;
|
||||
|
||||
template <typename T>
|
||||
FunctionGenerator<T> sinGenerator(int nrSteps, T frequency, T dt = 0.001)
|
||||
FunctionGenerator<T> sinGenerator(int nrSteps, T amplitude, T frequency, T dt)
|
||||
{
|
||||
using namespace difi;
|
||||
|
||||
std::random_device rd{};
|
||||
std::mt19937 gen{rd()};
|
||||
|
||||
vectX_t<T> truth;
|
||||
vectX_t<T> noisy;
|
||||
vectX_t<T> derivative;
|
||||
vectX_t<T> f(nrSteps);
|
||||
vectX_t<T> df(nrSteps);
|
||||
vectX_t<T> ddf(nrSteps);
|
||||
|
||||
for (int i = 0; i < nrSteps; ++i) {
|
||||
// truth
|
||||
truth(i) = std::sin(2 * pi<T> * frequency * i * dt);
|
||||
// function
|
||||
f(i) = amplitude * std::sin(2 * pi<T> * frequency * i * dt);
|
||||
|
||||
// noisy
|
||||
std::normal_distribution<T> d{truth(i), T(0.01)};
|
||||
noisy(i) = truth(i) + d(gen);
|
||||
// derivative 1
|
||||
df(i) = 2 * pi<T> * frequency * amplitude * std::cos(2 * pi<T> * frequency * i * dt);
|
||||
|
||||
// derivative
|
||||
derivative(i) = 2 * pi<T> * frequency * i * std::cos(2 * pi<T> * frequency * i * dt);
|
||||
// derivative 2
|
||||
ddf(i) = -4 * pi<T> * pi<T> * frequency * frequency * amplitude * std::sin(2 * pi<T> * frequency * i * dt);
|
||||
}
|
||||
|
||||
return { truth, noisy, derivative };
|
||||
return { f, df, ddf };
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
FunctionGenerator<T> polyGenerator(int nrSteps, difi::vectX_t<T> coeffs, T dt = 0.001)
|
||||
template <typename T, size_t N>
|
||||
FunctionGenerator<T> polyGenerator(int nrSteps, std::array<T, N> coeffs, T dt)
|
||||
{
|
||||
using namespace difi;
|
||||
Expects(coeffs.size() >=2);
|
||||
std::random_device rd{};
|
||||
std::mt19937 gen{rd()};
|
||||
static_assert(N >= 2, "N must be superior to 20");
|
||||
|
||||
auto computePoly = [](const VectX_t<T>& coeffs, T time) {
|
||||
auto recursiveComputation = [time, &coeffs](int i, T result) {
|
||||
auto computePoly = [](const auto& coeffs, T time) {
|
||||
auto recursiveComputation = [time, &coeffs](size_t i, T result, const auto& self) -> T {
|
||||
if (i > 0)
|
||||
return recursiveComputation(i - 1, time * result + coeffs(i - 1));
|
||||
else
|
||||
return self(i - 1, time * result + coeffs[i - 1], self);
|
||||
else
|
||||
return result;
|
||||
};
|
||||
|
||||
return recursiveComputation(coeffs.size(), 0);
|
||||
return recursiveComputation(coeffs.size(), 0, recursiveComputation);
|
||||
};
|
||||
|
||||
vectX_t<T> derivativeCoeffs(coeffs.size() - 1);
|
||||
for (Eigen::Index i = 1; i < coeffs.size(); ++i)
|
||||
derivativeCoeffs(i - 1) = i * coeffs.tail(i);
|
||||
std::array<T, N - 1> dCoeffs;
|
||||
for (Eigen::Index i = 1; i < N; ++i)
|
||||
dCoeffs[i - 1] = i * coeffs[i];
|
||||
|
||||
vectX_t<T> truth;
|
||||
vectX_t<T> noisy;
|
||||
vectX_t<T> derivative;
|
||||
std::array<T, N - 2> ddCoeffs;
|
||||
for (Eigen::Index i = 1; i < N - 1; ++i)
|
||||
ddCoeffs[i - 1] = i * dCoeffs[i];
|
||||
|
||||
vectX_t<T> f(nrSteps);
|
||||
vectX_t<T> df(nrSteps);
|
||||
vectX_t<T> ddf(nrSteps);
|
||||
for (int i = 0; i < nrSteps; ++i) {
|
||||
// truth
|
||||
truth(i) = computePoly(coeffs, i * dt);
|
||||
// function
|
||||
f(i) = computePoly(coeffs, i * dt);
|
||||
|
||||
// noisy
|
||||
std::normal_distribution<T> d{truth(i), T(0.01)};
|
||||
noisy(i) = truth(i) + d(gen);
|
||||
// derivative 1
|
||||
df(i) = computePoly(dCoeffs, i * dt);
|
||||
|
||||
// derivative
|
||||
derivative(i) = computePoly(derivativeCoeffs, i * dt);
|
||||
// derivative 2
|
||||
ddf(i) = computePoly(ddCoeffs, i * dt);
|
||||
}
|
||||
|
||||
return { truth, noisy, derivative };
|
||||
}
|
||||
return { f, df, ddf };
|
||||
}
|
||||
|
||||
// TV sin generator
|
||||
|
||||
template <typename T>
|
||||
using TVFunctionGenerator = std::tuple<difi::vectX_t<T>, difi::vectX_t<T>, difi::vectX_t<T>, difi::vectX_t<T>>;
|
||||
|
||||
template <typename T>
|
||||
TVFunctionGenerator<T> tvSinGenerator(int nrSteps, T amplitude, T frequency, T meanDt)
|
||||
{
|
||||
using namespace difi;
|
||||
|
||||
vectX_t<T> t(nrSteps);
|
||||
vectX_t<T> f(nrSteps);
|
||||
vectX_t<T> df(nrSteps);
|
||||
vectX_t<T> ddf(nrSteps);
|
||||
|
||||
t(0) = T(0);
|
||||
f(0) = T(0);
|
||||
df(0) = 2 * pi<T> * frequency * amplitude;
|
||||
ddf(0) = T(0);
|
||||
for (int i = 1; i < nrSteps; ++i) {
|
||||
// time
|
||||
if (i % 3 == 0)
|
||||
t(i) = t(i - 1) + 0.9 * meanDt;
|
||||
else if (i % 3 == 1)
|
||||
t(i) = t(i - 1) + meanDt;
|
||||
else
|
||||
t(i) = t(i - 1) + 1.1 * meanDt;
|
||||
|
||||
// function
|
||||
f(i) = amplitude * std::sin(2 * pi<T> * frequency * t(i));
|
||||
|
||||
// derivative 1
|
||||
df(i) = 2 * pi<T> * frequency * amplitude * std::cos(2 * pi<T> * frequency * t(i));
|
||||
|
||||
// derivative 2
|
||||
ddf(i) = -4 * pi<T> * pi<T> * frequency * frequency * amplitude * std::sin(2 * pi<T> * frequency * t(i));
|
||||
}
|
||||
|
||||
return { t, f, df, ddf };
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
TVFunctionGenerator<T> tvPolyGenerator(int nrSteps, std::array<T, N> coeffs, T dt)
|
||||
{
|
||||
using namespace difi;
|
||||
static_assert(N >= 2, "N must be superior to 20");
|
||||
|
||||
auto computePoly = [](const auto& coeffs, T time) {
|
||||
auto recursiveComputation = [time, &coeffs](size_t i, T result, const auto& self) -> T {
|
||||
if (i > 0)
|
||||
return self(i - 1, time * result + coeffs[i - 1], self);
|
||||
else
|
||||
return result;
|
||||
};
|
||||
|
||||
return recursiveComputation(coeffs.size(), 0, recursiveComputation);
|
||||
};
|
||||
|
||||
std::array<T, N - 1> dCoeffs;
|
||||
for (Eigen::Index i = 1; i < N; ++i)
|
||||
dCoeffs[i - 1] = i * coeffs[i];
|
||||
|
||||
std::array<T, N - 2> ddCoeffs;
|
||||
for (Eigen::Index i = 1; i < N - 1; ++i)
|
||||
ddCoeffs[i - 1] = i * dCoeffs[i];
|
||||
|
||||
vectX_t<T> t(nrSteps);
|
||||
vectX_t<T> f(nrSteps);
|
||||
vectX_t<T> df(nrSteps);
|
||||
vectX_t<T> ddf(nrSteps);
|
||||
t(0) = T(0);
|
||||
f(0) = computePoly(coeffs, t(0));
|
||||
df(0) = computePoly(dCoeffs, t(0));
|
||||
ddf(0) = computePoly(ddCoeffs, t(0));
|
||||
for (int i = 1; i < nrSteps; ++i) {
|
||||
// time
|
||||
if (i % 3 == 0)
|
||||
t(i) = t(i - 1) + 0.9 * meanDt;
|
||||
else if (i % 3 == 1)
|
||||
t(i) = t(i - 1) + meanDt;
|
||||
else
|
||||
t(i) = t(i - 1) + 1.1 * meanDt;
|
||||
|
||||
// function
|
||||
f(i) = computePoly(coeffs, t(i));
|
||||
|
||||
// derivative 1
|
||||
df(i) = computePoly(dCoeffs, t(i));
|
||||
|
||||
// derivative 2
|
||||
ddf(i) = computePoly(ddCoeffs, t(i));
|
||||
}
|
||||
|
||||
return { t, f, df, ddf };
|
||||
}
|
||||
|
|
Ładowanie…
Reference in New Issue