[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.

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 <int Derivative, int Samples, typename F, typename DfDx>
+void AssertResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
+                   double max) {
+  auto filter =
+      frc::LinearFilter<double>::BackwardFiniteDifference<Derivative, Samples>(
+          h);
+
+  for (int i = min / h.to<double>(); i < max / h.to<double>(); ++i) {
+    // Let filter initialize
+    if (i < static_cast<int>(min / h.to<double>()) + Samples) {
+      filter.Calculate(f(i * h.to<double>()));
+      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<double>()),
+                filter.Calculate(f(i * h.to<double>())),
+                10.0 * std::pow(h.to<double>(), 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);
+}