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