From 3b927b81743edcd5337d8c8d71c4b3d931ead449 Mon Sep 17 00:00:00 2001 From: Vincent Samy Date: Tue, 29 Oct 2019 13:27:15 +0900 Subject: [PATCH] Add differentiators. --- include/differentiators.h | 264 ++++++++++++++++++++++++++++++++++++++ include/math.h | 62 +++++++++ include/typedefs.h | 3 + 3 files changed, 329 insertions(+) create mode 100644 include/differentiators.h create mode 100644 include/math.h diff --git a/include/differentiators.h b/include/differentiators.h new file mode 100644 index 0000000..cd5ac92 --- /dev/null +++ b/include/differentiators.h @@ -0,0 +1,264 @@ +// 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 "DigitalFilter.h" +#include "math.h" +#include + +// Read this if you are an adulator of the math god: https://arxiv.org/pdf/1709.08321.pdf + +namespace difi { + +namespace details { + +// T: type +// N: Number of points + +// Central differentiators: http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/central-differences/ +template vectN_t GetCDCoeffs(); +template vectN_t GetCDCoeffs() { return vectN_t{ T(1), T(0), T(-1) } / T(2); } +template vectN_t GetCDCoeffs() { return vectN_t{ T(-1), T(8), T(0), T(8), T(1) } / T(12); } +template vectN_t GetCDCoeffs() { return vectN_t{ T(1), T(-9), T(45), T(0), T(-45), T(9), T(-1) } / T(60); } +template vectN_t GetCDCoeffs() { return vectN_t{ T(-3), T(32), T(-168), T(672), T(0), T(-672), T(168), T(-32), T(3) } / T(840); } + +// Low-noise Lanczos differentiators: http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/lanczos-low-noise-differentiators/ +template +vectN_t GetLNLCoeffs() +{ + 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 v{}; + for (Eigen::Index k = 0; k < N; ++k) + v(k) = GetCoeff(N, k); + return v; +} +template vectN_t GetLNLCoeffs() { return vectN_t{ T(2), T(1), T(0), T(-1), T(-2) } / T(10); } +template vectN_t GetLNLCoeffs() { return vectN_t{ T(3), T(2), T(1), T(0), T(-1), T(-2), T(-3) } / T(28); } +template vectN_t GetLNLCoeffs() { return vectN_t{ T(4), T(3), T(2), T(1), T(0), T(-1), T(-2), T(-3), T(-4) } / T(60); } +template vectN_t GetLNLCoeffs() { return vectN_t{ 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 vectN_t GetSLNLCoeffs(); +template vectN_t GetSLNLCoeffs() { return vectN_t{ T(-22), T(67), T(58), T(0), T(-58), T(-67), T(22) } / T(252); } +template vectN_t GetSLNLCoeffs() { return vectN_t{ T(-86), T(142), T(193), T(126), T(0), T(-126), T(-193), T(-142), T(86) } / T(1188); } +template vectN_t GetSLNLCoeffs() { return vectN_t{ T(-300), T(294), T(532), T(503), T(296), T(0), T(-296), T(-503), T(-532), T(-294), T(300) } / T(5148); } + +// Forward Noise-Robust differentiators; http://www.holoborodko.com/pavel/wp-content/uploads/OneSidedNoiseRobustDifferentiators.pdf +template +vectN_t GetFNRCoeffs() +{ + constexpr const int BinCoeff = N - 2; + vectN_t v{}; + v(0) = T(1); + v(N - 1) = T(-1); + for (int i = 1; i < N - 1; ++i) + v(i) = (Binomial(N, i) - Binomial(N, i - 1)) / std::pow(T(2), T(BinCoeff)); + + return v; +} +template vectN_t GetFNRCoeffs() { return vectN_t{ T(1), T(0), T(-1) } / T(2); } +template vectN_t GetFNRCoeffs() { return vectN_t{ T(1), T(1), T(-1), T(-1) } / T(4); } +template vectN_t GetFNRCoeffs() { return vectN_t{ T(1), T(2), T(0), T(-2), T(-1) } / T(8); } +template vectN_t GetFNRCoeffs() { return vectN_t{ T(1), T(3), T(2), T(-2), T(-3), T(-1) } / T(16); } +template vectN_t GetFNRCoeffs() { return vectN_t{ T(1), T(4), T(5), T(0), T(-5), T(-4), T(-1) } / T(32); } +template vectN_t GetFNRCoeffs() { return vectN_t{ T(1), T(5), T(9), T(5), T(-5), T(-9), T(-5), T(-1) } / T(64); } +template vectN_t GetFNRCoeffs() { return vectN_t{ T(1), T(6), T(14), T(14), T(0), T(-14), T(-14), T(-6), T(-1) } / T(128); } +template vectN_t GetFNRCoeffs() { return vectN_t{ T(1), T(7), T(20), T(28), T(14), T(-14), T(-28), T(-20), T(-7), T(-1) } / T(256); } +template vectN_t GetFNRCoeffs() { return vectN_t{ T(1), T(8), T(27), T(48), T(42), T(0), T(-42), T(-48), T(-27), T(-8), T(-1) } / T(512); } + +// Forward Hybrid Noise-Robust differentiators; http://www.holoborodko.com/pavel/wp-content/uploads/OneSidedNoiseRobustDifferentiators.pdf +template vectN_t GetFHNRCoeffs(); +template vectN_t GetFHNRCoeffs() { return vectN_t{ T(2), T(-1), T(-2), T(1) } / T(2); } +template vectN_t GetFHNRCoeffs() { return vectN_t{ T(7), T(1), T(-10), T(-1), T(3) } / T(10); } +template vectN_t GetFHNRCoeffs() { return vectN_t{ T(16), T(1), T(-10), T(-10), T(-6), T(9) } / T(28); } +template vectN_t GetFHNRCoeffs() { return vectN_t{ T(12), T(5), T(-8), T(-6), T(-10), T(1), T(6) } / T(28); } +template vectN_t GetFHNRCoeffs() { return vectN_t{ T(22), T(7), T(-6), T(-11), T(-14), T(-9), T(-2), T(13) } / T(60); } +template vectN_t GetFHNRCoeffs() { return vectN_t{ T(52), T(29), T(-14), T(-17), T(-40), T(-23), T(-26), T(11), T(28) } / T(180); } +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); } + +// 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() +{ + static_assert(N % 2 == 1. "'N' must be odd."); + return GetFNRCoeffs(); // Same coefficients +} +template vectN_t GetCNR2Coeffs() { return vectN_t{ T(1), T(2), T(0), T(-2), T(-1) } / T(8); } +template vectN_t GetCNR2Coeffs() { return vectN_t{ T(1), T(4), T(5), T(0), T(-5), T(-4), T(-1) } / T(32); } +template vectN_t GetCNR2Coeffs() { return vectN_t{ T(1), T(6), T(14), T(14), T(0), T(-14), T(-14), T(-6), T(-1) } / T(128); } +template vectN_t GetCNR2Coeffs() { return vectN_t{ T(1), T(8), T(27), T(48), T(42), T(0), T(-42), T(-48), T(-27), T(-8), T(-1) } / T(512); } + +// Centered Noise-Robust differentiators (tangency at 4th order): http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ +template vectN_t GetCNR4Coeffs(); +template vectN_t GetCNR4Coeffs() { return vectN_t{ T(-5), T(12), T(39), T(0), T(-39), T(-12), T(5) } / T(96); } +template vectN_t GetCNR4Coeffs() { return vectN_t{ T(-2), T(-1), T(16), T(27) } / T(96); } +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() +{ + vectN_t v{}; + const vectN_t v0 = Foo(); + for (Eigen::Index k = 0; k < N; ++k) + v(k) = T(2) * k * v0(k); + return v(k); +} +template vectN_t GetCNR2ISDCoeffs() { return GetCNRISDoeffs(); } +template vectN_t GetCNR4ISDCoeffs() { return GetCNRISDoeffs(); } + +/* + * Second order differentiators + */ + +template +constexpr T GetSONRBaseCoeff(size_t N, size_t M, size_t k) +{ + if (k > M) + return T(0); + else if (k = M) + return T(1); + else + return ((T(2) * N - T(10)) * GetSONRBaseCoeff(k + 1) - (N + T(2) * k + T(3) * GetSONRBaseCoeff(k + 2))) / (N - T(2) * k - T(1)); +} + +template vectN_t GetSONRBaseCoeffs() +{ + static_assert(N >= 5 && N % 2 == 1); + constexpr const size_t M = (N - 1) / 2; + vectN_t v{}; + for (size_t k = 0; k < M + 1; ++k) + v(k) = GetSONRBaseCoeff(N, M, k); + + return v; +} + +// Second-Order Centered Noise-Robust differentiator: http://www.holoborodko.com/pavel/downloads/NoiseRobustSecondDerivative.pdf +template +vectN_t GetSOCNRCoeffs() +{ + constexpr const size_t M = (N - 1) / 2; + 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 + k) = s(k); + + v /= Den; + return v; +} + +// Second-Order Forward Noise-Robust differentiator: http://www.holoborodko.com/pavel/downloads/NoiseRobustSecondDerivative.pdf +template +vectN_t GetSOFNRCoeffs() { return GetSOCNRCoeffs(); } // Coefficients are the same. + +// Second-Order Centered Noise-Robust Irregular Space Data differentiator: http://www.holoborodko.com/pavel/downloads/NoiseRobustSecondDerivative.pdf +template +vectN_t GetSOCNRISDCoeffs() +{ + constexpr const size_t M = (N - 1) / 2; + constexpr const T Den = pow(size_t(2), N - 3); + + vectN_t v{}; + vectN_t s = GetSONRBaseCoeffs(); + + 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) + v(M + k) = alpha(k); + + v /= Den; + return v; +} + +// Second-Order Forward Noise-Robust Irregular Space Data differentiator: http://www.holoborodko.com/pavel/downloads/NoiseRobustSecondDerivative.pdf +template +vectN_t GetSOFNRISDCoeffs() { return GetSOCNRISDCoeffs(); } // Same coefficients + +/* + * Differentiator Generator + */ + +template typename CoeffGetter> +class ForwardDifferentiator : public GenericFilter { +public: + ForwardDifferentiator() + : GenericFilter({T(1)}, CoeffGetter()) + {} + ForwardDifferentiator(T timestep) + : GenericFilter({T(1)}, CoeffGetter() / std::pow(timestep, Order)) + {} + void setTimestep(T timestep) { setCoeffs({T(1)}, CoeffGetter() / std::pow(timestep, Order)); } + T timestep() const noexcept { return std::pow(bCoeff()(0) / CoeffGetter()(0), T(1) / Order); } +}; + +template typename CoeffGetter> +class CentralDifferentiator : public GenericFilter { +public: + CentralDifferentiator() + : GenericFilter({T(1)}, CoeffGetter(), Type::Centered) + {} + CentralDifferentiator(T timestep) + : GenericFilter({T(1)}, CoeffGetter() / std::pow(timestep, Order)) + {} + void setTimestep(T timestep) { setCoeffs({T(1)}, CoeffGetter() / std::pow(timestep, Order)); } + T timestep() const noexcept { return std::pow(bCoeff()(0) / CoeffGetter()(0), T(1) / Order); } +}; + +} // namespace details + +// Central differentiators +template using CentralDiff = CentralDifferentiator; +template using LowNoiseLanczosDiff = CentralDifferentiator; +template using SuperLowNoiseLanczosDiff = CentralDifferentiator; +template using CenteredNoiseRobust2Diff = CentralDifferentiator; +template using CenteredNoiseRobust4Diff = CentralDifferentiator; + +// Forward differentiators +template using ForwardNoiseRobustDiff = ForwardDifferentiator; +template using ForwardHybridNoiseRobustDiff = ForwardDifferentiator; + +// TODO: time-variable differentiators + +// Second-order central differentiators +template using CenteredSecondOrderDiff = CentralDifferentiator; + +// Second-order central differentiators +template using ForwardSecondOrderDiff = ForwardDifferentiator; + + +} // namespace difi \ No newline at end of file diff --git a/include/math.h b/include/math.h new file mode 100644 index 0000000..362ba08 --- /dev/null +++ b/include/math.h @@ -0,0 +1,62 @@ +// 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 + +namespace difi +{ + +// https://stackoverflow.com/questions/44718971/calculate-binomial-coffeficient-very-reliably +template +constexpr T Binomial(int n, int k) +{ + if (k > n) + return T(0); + else if (k == 0 || k == n) + return T(1); + else if (k == 1 || k == n-1) + return T(n); + else if ( 2 * k < n) + return Binomial(n-1, k - 1) * n / k; + else + return Binomial(n-1, k) * n / (n - k); +} + +template +constexpr T pow(T n, T k) +{ + static_assert(std::is_integral_v); + if (n == 0) + return (k == 0) ? 1 : 0; + else if (k == 0 || k == 1) + return T(1); + else + return n * pow(n, k - 1); +} + +} // namespace difi diff --git a/include/typedefs.h b/include/typedefs.h index 38c01e0..d67f479 100644 --- a/include/typedefs.h +++ b/include/typedefs.h @@ -37,6 +37,9 @@ constexpr T pi = T(3.14159265358979323846264338327950288419716939937510582097494 template using vectX_t = Eigen::Matrix; /*!< Eigen column-vector */ +template +using vectN_t = Eigen::Matrix; /*!< Fixed Eigen column-vector */ + template using vectXc_t = vectX_t>; /*!< Eigen complex column-vector */