Implemented 5th order gaussian quadrature.
Change-Id: Id7023b4f175c60b6930b5cb8aa829636604b2d25
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index a25a2e8..3cc8dd3 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -202,6 +202,24 @@
],
)
+cc_library(
+ name = "fixed_quadrature",
+ hdrs = [
+ "fixed_quadrature.h",
+ ],
+)
+
+cc_test(
+ name = "fixed_quadrature_test",
+ srcs = [
+ "fixed_quadrature_test.cc",
+ ],
+ deps = [
+ ":fixed_quadrature",
+ "//aos/testing:googletest",
+ ],
+)
+
queue_library(
name = "profiled_subsystem_queue",
srcs = [
diff --git a/frc971/control_loops/fixed_quadrature.h b/frc971/control_loops/fixed_quadrature.h
new file mode 100644
index 0000000..572c235
--- /dev/null
+++ b/frc971/control_loops/fixed_quadrature.h
@@ -0,0 +1,37 @@
+#ifndef FRC971_CONTROL_LOOPS_FIXED_QUADRATURE_H_
+#define FRC971_CONTROL_LOOPS_FIXED_QUADRATURE_H_
+
+#include <array>
+
+namespace frc971 {
+namespace control_loops {
+
+// 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) {
+ // 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}};
+
+ double answer = 0.0;
+ 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
+
+#endif // FRC971_CONTROL_LOOPS_FIXED_QUADRATURE_H_
diff --git a/frc971/control_loops/fixed_quadrature_test.cc b/frc971/control_loops/fixed_quadrature_test.cc
new file mode 100644
index 0000000..f842519
--- /dev/null
+++ b/frc971/control_loops/fixed_quadrature_test.cc
@@ -0,0 +1,21 @@
+#include "frc971/control_loops/fixed_quadrature.h"
+
+#include <cmath>
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Tests that integrating y = cos(x) works.
+TEST(GaussianQuadratureTest, Cos) {
+ double y1 =
+ GaussianQuadrature5([](double x) { return ::std::cos(x); }, 0.0, 0.5);
+
+ EXPECT_NEAR(y1, ::std::sin(0.5), 1e-15);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971