Add a matrix version of GaussianQuadrature5
Change-Id: I205c37c825ce0abdf31bc2821e82e0d89505a056
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index ee53c4a..a62b45f 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -354,6 +354,9 @@
"fixed_quadrature.h",
],
target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "@org_tuxfamily_eigen//:eigen",
+ ],
)
cc_test(
diff --git a/frc971/control_loops/fixed_quadrature.h b/frc971/control_loops/fixed_quadrature.h
index 572c235..22d4984 100644
--- a/frc971/control_loops/fixed_quadrature.h
+++ b/frc971/control_loops/fixed_quadrature.h
@@ -1,6 +1,7 @@
#ifndef FRC971_CONTROL_LOOPS_FIXED_QUADRATURE_H_
#define FRC971_CONTROL_LOOPS_FIXED_QUADRATURE_H_
+#include <Eigen/Dense>
#include <array>
namespace frc971 {
@@ -9,8 +10,8 @@
// Implements Gaussian Quadrature integration (5th order). fn is the function to
// integrate. It must take 1 argument of type T. The integration is between a
// and b.
-template <typename F, typename T>
-T GaussianQuadrature5(const F &fn, T a, T b) {
+template <typename T, typename F>
+double GaussianQuadrature5(const F &fn, T a, T b) {
// Pulled from Python.
// numpy.set_printoptions(precision=20)
// scipy.special.p_roots(5)
@@ -31,6 +32,30 @@
return answer;
}
+template <size_t N, typename F>
+Eigen::Matrix<double, N, 1> MatrixGaussianQuadrature5(const F &fn, double a,
+ double b) {
+ // Pulled from Python.
+ // numpy.set_printoptions(precision=20)
+ // scipy.special.p_roots(5)
+ const ::std::array<double, 5> x{{
+ -9.06179845938663630633e-01, -5.38469310105682885670e-01,
+ 3.24607628916367383789e-17, 5.38469310105683218737e-01,
+ 9.06179845938663408589e-01}};
+
+ const ::std::array<double, 5> w{{
+ 0.23692688505618844652, 0.4786286704993669705, 0.56888888888888811124,
+ 0.47862867049936674846, 0.23692688505618875183}};
+
+ Eigen::Matrix<double, N, 1> answer;
+ answer.setZero();
+ for (int i = 0; i < 5; ++i) {
+ const double y = (b - a) * (x[i] + 1) / 2.0 + a;
+ answer += (b - a) / 2.0 * w[i] * fn(y);
+ }
+ return answer;
+}
+
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/fixed_quadrature_test.cc b/frc971/control_loops/fixed_quadrature_test.cc
index f842519..0615031 100644
--- a/frc971/control_loops/fixed_quadrature_test.cc
+++ b/frc971/control_loops/fixed_quadrature_test.cc
@@ -16,6 +16,18 @@
EXPECT_NEAR(y1, ::std::sin(0.5), 1e-15);
}
+// Tests that integrating y = [cos(x), sin(x)] works.
+TEST(GaussianQuadratureTest, MatrixCos) {
+ Eigen::Matrix<double, 2, 1> y1 = MatrixGaussianQuadrature5<2>(
+ [](double x) {
+ return Eigen::Matrix<double, 2, 1>(std::cos(x), std::sin(x));
+ },
+ 0.0, 0.5);
+
+ EXPECT_TRUE(y1.isApprox(Eigen::Matrix<double, 2, 1>(
+ ::std::sin(0.5), -std::cos(0.5) + std::cos(0))));
+}
+
} // namespace testing
} // namespace control_loops
} // namespace frc971