From 3b5d0d141a31ff31147e63307c237ba25dbe3d28 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 28 Aug 2021 20:50:18 -0700 Subject: [PATCH] [wpimath] Add LinearFilter::BackwardFiniteDifference() (#3528) This is an alternative to #2344 that handles arbitrary order derivatives of arbitrary precision. The downside is that since it's part of LinearFilter, it can't utilize the units type system in the same way to make Calculate()'s input type different from its output type. --- .../wpi/first/math/filter/LinearFilter.java | 96 ++++++++++++++- .../native/include/frc/filter/LinearFilter.h | 93 ++++++++++++++- .../first/math/filter/LinearFilterTest.java | 109 ++++++++++++++++++ .../cpp/filter/LinearFilterOutputTest.cpp | 95 +++++++++++++++ 4 files changed, 387 insertions(+), 6 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java index d85d663891..e91ab504a4 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; import edu.wpi.first.util.CircularBuffer; import java.util.Arrays; +import org.ejml.simple.SimpleMatrix; /** * This class implements a linear, digital filter. All types of FIR and IIR filters are supported. @@ -57,8 +58,8 @@ public class LinearFilter { /** * Create a linear FIR or IIR filter. * - * @param ffGains The "feed forward" or FIR gains. - * @param fbGains The "feed back" or IIR gains. + * @param ffGains The "feedforward" or FIR gains. + * @param fbGains The "feedback" or IIR gains. */ public LinearFilter(double[] ffGains, double[] fbGains) { m_inputs = new CircularBuffer(ffGains.length); @@ -136,6 +137,84 @@ public class LinearFilter { return new LinearFilter(ffGains, fbGains); } + /** + * Creates a backward finite difference filter that computes the nth derivative of the input given + * the specified number of samples. + * + *

For example, a first derivative filter that uses two samples and a sample period of 20 ms + * would be + * + * 


+   * LinearFilter.backwardFiniteDifference(1, 2, 0.02);
+   *
+ * + * @param derivative The order of the derivative to compute. + * @param samples The number of samples to use to compute the given derivative. This must be one + * more than the order of derivative or higher. + * @param period The period in seconds between samples taken by the user. + */ + @SuppressWarnings("LocalVariableName") + public static LinearFilter backwardFiniteDifference(int derivative, int samples, double period) { + // See + // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points + // + //

For a given list of stencil points s of length n and the order of + // derivative d < n, the finite difference coefficients can be obtained by + // solving the following linear system for the vector a. + // + // 

+    // [s₁⁰   ⋯  sₙ⁰ ][a₁]      [ δ₀,d ]
+    // [ ⋮    ⋱  ⋮   ][⋮ ] = d! [  ⋮   ]
+    // [s₁ⁿ⁻¹ ⋯ sₙⁿ⁻¹][aₙ]      [δₙ₋₁,d]
+    //
+ // + //

where δᵢ,ⱼ are the Kronecker delta. For backward finite difference, + // the stencil points are the range [-n + 1, 0]. The FIR gains are the + // elements of the vector a in reverse order divided by hᵈ. + // + //

The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ). + + if (derivative < 1) { + throw new IllegalArgumentException( + "Order of derivative must be greater than or equal to one."); + } + + if (samples <= 0) { + throw new IllegalArgumentException("Number of samples must be greater than zero."); + } + + if (derivative >= samples) { + throw new IllegalArgumentException( + "Order of derivative must be less than number of samples."); + } + + var S = new SimpleMatrix(samples, samples); + for (int row = 0; row < samples; ++row) { + for (int col = 0; col < samples; ++col) { + double s = 1 - samples + col; + S.set(row, col, Math.pow(s, row)); + } + } + + // Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta + var d = new SimpleMatrix(samples, 1); + for (int i = 0; i < samples; ++i) { + d.set(i, 0, (i == derivative) ? factorial(derivative) : 0.0); + } + + var a = S.solve(d).divide(Math.pow(period, derivative)); + + // Reverse gains list + double[] ffGains = new double[samples]; + for (int i = 0; i < samples; ++i) { + ffGains[i] = a.get(samples - i - 1, 0); + } + + double[] fbGains = new double[0]; + + return new LinearFilter(ffGains, fbGains); + } + /** Reset the filter state. */ public void reset() { m_inputs.clear(); @@ -171,4 +250,17 @@ public class LinearFilter { return retVal; } + + /** + * Factorial of n. + * + * @param n Argument of which to take factorial. + */ + private static int factorial(int n) { + if (n < 2) { + return 1; + } else { + return n * factorial(n - 1); + } + } } diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h index 925c67b8aa..266d149b9a 100644 --- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h +++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h @@ -13,6 +13,8 @@ #include #include +#include "Eigen/Core" +#include "Eigen/QR" #include "units/time.h" #include "wpimath/MathShared.h" @@ -74,8 +76,8 @@ class LinearFilter { /** * Create a linear FIR or IIR filter. * - * @param ffGains The "feed forward" or FIR gains. - * @param fbGains The "feed back" or IIR gains. + * @param ffGains The "feedforward" or FIR gains. + * @param fbGains The "feedback" or IIR gains. */ LinearFilter(wpi::span ffGains, wpi::span fbGains) : m_inputs(ffGains.size()), @@ -98,8 +100,8 @@ class LinearFilter { /** * Create a linear FIR or IIR filter. * - * @param ffGains The "feed forward" or FIR gains. - * @param fbGains The "feed back" or IIR gains. + * @param ffGains The "feedforward" or FIR gains. + * @param fbGains The "feedback" or IIR gains. */ LinearFilter(std::initializer_list ffGains, std::initializer_list fbGains) @@ -164,6 +166,76 @@ class LinearFilter { return LinearFilter(gains, {}); } + /** + * Creates a backward finite difference filter that computes the nth + * derivative of the input given the specified number of samples. + * + * For example, a first derivative filter that uses two samples and a sample + * period of 20 ms would be + * + * 


+   * LinearFilter::BackwardFiniteDifference<1, 2>(20_ms);
+   *
+ * + * @tparam Derivative The order of the derivative to compute. + * @tparam Samples The number of samples to use to compute the given + * derivative. This must be one more than the order of + * derivative or higher. + * @param period The period in seconds between samples taken by the user. + */ + template + static auto BackwardFiniteDifference(units::second_t period) { + // See + // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points + // + // For a given list of stencil points s of length n and the order of + // derivative d < n, the finite difference coefficients can be obtained by + // solving the following linear system for the vector a. + // + // @verbatim + // [s₁⁰ ⋯ sₙ⁰ ][a₁] [ δ₀,d ] + // [ ⋮ ⋱ ⋮ ][⋮ ] = d! [ ⋮ ] + // [s₁ⁿ⁻¹ ⋯ sₙⁿ⁻¹][aₙ] [δₙ₋₁,d] + // @endverbatim + // + // where δᵢ,ⱼ are the Kronecker delta. For backward finite difference, the + // stencil points are the range [-n + 1, 0]. The FIR gains are the elements + // of the vector a in reverse order divided by hᵈ. + // + // The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ). + + static_assert(Derivative >= 1, + "Order of derivative must be greater than or equal to one."); + static_assert(Samples > 0, "Number of samples must be greater than zero."); + static_assert(Derivative < Samples, + "Order of derivative must be less than number of samples."); + + Eigen::Matrix S; + for (int row = 0; row < Samples; ++row) { + for (int col = 0; col < Samples; ++col) { + double s = 1 - Samples + col; + S(row, col) = std::pow(s, row); + } + } + + // Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta + Eigen::Matrix d; + for (int i = 0; i < Samples; ++i) { + d(i) = (i == Derivative) ? Factorial(Derivative) : 0.0; + } + + Eigen::Matrix a = + S.householderQr().solve(d) / std::pow(period.to(), Derivative); + + // Reverse gains list + std::vector gains; + for (int i = Samples - 1; i >= 0; --i) { + gains.push_back(a(i)); + } + + return LinearFilter(gains, {}); + } + /** * Reset the filter state. */ @@ -208,6 +280,19 @@ class LinearFilter { wpi::circular_buffer m_outputs; std::vector m_inputGains; std::vector m_outputGains; + + /** + * Factorial of n. + * + * @param n Argument of which to take factorial. + */ + static int Factorial(int n) { + if (n < 2) { + return 1; + } else { + return n * Factorial(n - 1); + } + } }; } // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java index 41be6f62c4..805129f95b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java @@ -108,4 +108,113 @@ class LinearFilterTest { (DoubleFunction) LinearFilterTest::getPulseData, 0.0)); } + + /** Test backward finite difference. */ + @Test + void backwardFiniteDifferenceTest() { + double h = 0.005; + + assertResults( + 1, + 2, + // f(x) = x² + (double x) -> x * x, + // df/dx = 2x + (double x) -> 2.0 * x, + h, + -20.0, + 20.0); + + assertResults( + 1, + 2, + // f(x) = sin(x) + (double x) -> Math.sin(x), + // df/dx = cos(x) + (double x) -> Math.cos(x), + h, + -20.0, + 20.0); + + assertResults( + 1, + 2, + // f(x) = ln(x) + (double x) -> Math.log(x), + // df/dx = 1 / x + (double x) -> 1.0 / x, + h, + 1.0, + 20.0); + + assertResults( + 2, + 4, + // f(x) = x² + (double x) -> x * x, + // d²f/dx² = 2 + (double x) -> 2.0, + h, + -20.0, + 20.0); + + assertResults( + 2, + 4, + // f(x) = sin(x) + (double x) -> Math.sin(x), + // d²f/dx² = -sin(x) + (double x) -> -Math.sin(x), + h, + -20.0, + 20.0); + + assertResults( + 2, + 4, + // f(x) = ln(x) + (double x) -> Math.log(x), + // d²f/dx² = -1 / x² + (double x) -> -1.0 / (x * x), + h, + 1.0, + 20.0); + } + + /** + * Helper for checking results of backward finite difference. + * + * @param derivative The order of the derivative. + * @param samples The number of sample points. + * @param f Function of which to take derivative. + * @param dfdx Derivative of f. + * @param h Sample period in seconds. + * @param min Minimum of f's domain to test. + * @param max Maximum of f's domain to test. + */ + void assertResults( + int derivative, + int samples, + DoubleFunction f, + DoubleFunction dfdx, + double h, + double min, + double max) { + var filter = LinearFilter.backwardFiniteDifference(derivative, samples, h); + + for (int i = (int) (min / h); i < (int) (max / h); ++i) { + // Let filter initialize + if (i < (int) (min / h) + samples) { + filter.calculate(f.apply(i * h)); + continue; + } + + // The order of accuracy is O(h^(N - d)) where N is number of stencil + // points and d is order of derivative + assertEquals( + dfdx.apply(i * h), + filter.calculate(f.apply(i * h)), + 10.0 * Math.pow(h, samples - derivative)); + } + } } diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp index 6b944121c7..ae9d943d5d 100644 --- a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp +++ b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp @@ -118,3 +118,98 @@ TEST_P(LinearFilterOutputTest, Output) { INSTANTIATE_TEST_SUITE_P(Test, LinearFilterOutputTest, testing::Values(kTestSinglePoleIIR, kTestHighPass, kTestMovAvg, kTestPulse)); + +template +void AssertResults(F&& f, DfDx&& dfdx, units::second_t h, double min, + double max) { + auto filter = + frc::LinearFilter::BackwardFiniteDifference( + h); + + for (int i = min / h.to(); i < max / h.to(); ++i) { + // Let filter initialize + if (i < static_cast(min / h.to()) + Samples) { + filter.Calculate(f(i * h.to())); + continue; + } + + // The order of accuracy is O(h^(N - d)) where N is number of stencil + // points and d is order of derivative + EXPECT_NEAR(dfdx(i * h.to()), + filter.Calculate(f(i * h.to())), + 10.0 * std::pow(h.to(), Samples - Derivative)); + } +} + +/** + * Test backward finite difference. + */ +TEST(LinearFilterOutputTest, BackwardFiniteDifference) { + constexpr auto h = 5_ms; + + AssertResults<1, 2>( + [](double x) { + // f(x) = x² + return x * x; + }, + [](double x) { + // df/dx = 2x + return 2.0 * x; + }, + h, -20.0, 20.0); + + AssertResults<1, 2>( + [](double x) { + // f(x) = std::sin(x) + return std::sin(x); + }, + [](double x) { + // df/dx = std::cos(x) + return std::cos(x); + }, + h, -20.0, 20.0); + + AssertResults<1, 2>( + [](double x) { + // f(x) = ln(x) + return std::log(x); + }, + [](double x) { + // df/dx = 1 / x + return 1.0 / x; + }, + h, 1.0, 20.0); + + AssertResults<2, 4>( + [](double x) { + // f(x) = x^2 + return x * x; + }, + [](double x) { + // d²f/dx² = 2 + return 2.0; + }, + h, -20.0, 20.0); + + AssertResults<2, 4>( + [](double x) { + // f(x) = std::sin(x) + return std::sin(x); + }, + [](double x) { + // d²f/dx² = -std::sin(x) + return -std::sin(x); + }, + h, -20.0, 20.0); + + AssertResults<2, 4>( + [](double x) { + // f(x) = ln(x) + return std::log(x); + }, + [](double x) { + // d²f/dx² = -1 / x² + return -1.0 / (x * x); + }, + h, 1.0, 20.0); +}