Add a matrix version of GaussianQuadrature5
Change-Id: I205c37c825ce0abdf31bc2821e82e0d89505a056
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
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