Refactor discretization of Q to c2d and add c2d tests.
This gives us the important functionality of kalmd in C++.
Change-Id: I2898018ce23ef5d24dfaa04cfb6bbbf09a9b9270
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 6d895de..d46bca6 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -265,6 +265,19 @@
],
)
+cc_test(
+ name = "c2d_test",
+ srcs = [
+ "c2d_test.cc",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":c2d",
+ ":runge_kutta",
+ "//aos/testing:googletest",
+ ],
+)
+
cc_library(
name = "c2d",
hdrs = [
diff --git a/frc971/control_loops/c2d.h b/frc971/control_loops/c2d.h
index 453a6a5..1bf99a2 100644
--- a/frc971/control_loops/c2d.h
+++ b/frc971/control_loops/c2d.h
@@ -4,6 +4,8 @@
#include <chrono>
#include <Eigen/Dense>
+// We need to include MatrixFunctions for the matrix exponential.
+#include "unsupported/Eigen/MatrixFunctions"
namespace frc971 {
namespace controls {
@@ -33,6 +35,39 @@
*B = M_state.template block<num_states, num_inputs>(0, num_states);
}
+template <typename Scalar, int num_states>
+void DiscretizeQ(
+ const Eigen::Matrix<Scalar, num_states, num_states> &Q_continuous,
+ const Eigen::Matrix<Scalar, num_states, num_states> &A_continuous,
+ ::std::chrono::nanoseconds dt,
+ Eigen::Matrix<Scalar, num_states, num_states> *Q_d) {
+ Eigen::Matrix<Scalar, num_states, num_states> Qtemp =
+ (Q_continuous + Q_continuous.transpose()) / static_cast<Scalar>(2.0);
+ Eigen::Matrix<Scalar, 2 * num_states, 2 * num_states> M_gain;
+ M_gain.setZero();
+ // Set up the matrix M = [[-A, Q], [0, A.T]]
+ M_gain.template block<num_states, num_states>(0, 0) = -A_continuous;
+ M_gain.template block<num_states, num_states>(0, num_states) = Qtemp;
+ M_gain.template block<num_states, num_states>(num_states, num_states) =
+ A_continuous.transpose();
+
+ Eigen::Matrix<Scalar, 2 * num_states, 2 *num_states> phi =
+ (M_gain *
+ ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
+ .count()).exp();
+
+ // Phi12 = phi[0:num_states, num_states:2*num_states]
+ // Phi22 = phi[num_states:2*num_states,
+ // num_states:2*num_states]
+ Eigen::Matrix<Scalar, num_states, num_states> phi12 =
+ phi.block(0, num_states, num_states, num_states);
+ Eigen::Matrix<Scalar, num_states, num_states> phi22 =
+ phi.block(num_states, num_states, num_states, num_states);
+
+ *Q_d = phi22.transpose() * phi12;
+ *Q_d = (*Q_d + Q_d->transpose()) / static_cast<Scalar>(2.0);
+}
+
} // namespace controls
} // namespace frc971
diff --git a/frc971/control_loops/c2d_test.cc b/frc971/control_loops/c2d_test.cc
new file mode 100644
index 0000000..dcdb048
--- /dev/null
+++ b/frc971/control_loops/c2d_test.cc
@@ -0,0 +1,74 @@
+#include "frc971/control_loops/c2d.h"
+
+#include <functional>
+
+#include "frc971/control_loops/runge_kutta.h"
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace controls {
+namespace testing {
+
+class C2DTest : public ::testing::Test {
+ public:
+ C2DTest() {
+ // Create a trivial second-order system.
+ A_continuous << 0, 1, 0, 0;
+ B_continuous << 0, 1;
+ Q_continuous << 1, 0, 0, 1;
+ }
+
+ protected:
+ Eigen::Matrix<double, 2, 2> A_continuous;
+ Eigen::Matrix<double, 2, 1> B_continuous;
+ Eigen::Matrix<double, 2, 2> Q_continuous;
+};
+
+// Check that for a simple second-order system that we can easily analyze
+// analytically, C2D creates valid A/B matrices.
+TEST_F(C2DTest, DiscretizeAB) {
+ Eigen::Matrix<double, 2, 1> X0;
+ X0 << 1, 1;
+ Eigen::Matrix<double, 1, 1> U;
+ U << 1;
+ Eigen::Matrix<double, 2, 2> A_d;
+ Eigen::Matrix<double, 2, 1> B_d;
+
+ C2D(A_continuous, B_continuous, ::std::chrono::seconds(1), &A_d, &B_d);
+ Eigen::Matrix<double, 2, 1> X1_discrete = A_d * X0 + B_d * U;
+ // We now have pos = vel = accel = 1, which should give us:
+ Eigen::Matrix<double, 2, 1> X1_truth;
+ X1_truth(1, 0) = X0(1, 0) + 1.0 * U(0, 0);
+ X1_truth(0, 0) = X0(0, 0) + 1.0 * X0(1, 0) + 0.5 * U(0, 0);
+ EXPECT_EQ(X1_truth, X1_discrete);
+}
+
+// Test that the discrete approximation of Q is roughly equal to
+// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
+TEST_F(C2DTest, DiscretizeQ) {
+ Eigen::Matrix<double, 2, 2> Q_d;
+ const auto dt = ::std::chrono::seconds(1);
+ DiscretizeQ(Q_continuous, A_continuous, dt, &Q_d);
+ // TODO(james): Using Runge Kutta for this is a bit silly as f is just a
+ // function of t, not Q, but I don't want to rewrite any of our math
+ // utilities.
+ // Note that we are being very explicit about the types of everything in this
+ // integration because otherwise it doesn't compile very well.
+ Eigen::Matrix<double, 2, 2> Q_d_integrated = control_loops::RungeKutta<
+ ::std::function<Eigen::Matrix<double, 2, 2>(
+ const double, const Eigen::Matrix<double, 2, 2> &)>,
+ Eigen::Matrix<double, 2, 2>>(
+ [this](const double t, const Eigen::Matrix<double, 2, 2> &) {
+ return Eigen::Matrix<double, 2, 2>(
+ (A_continuous * t).exp() * Q_continuous *
+ (A_continuous.transpose() * t).exp());
+ },
+ Eigen::Matrix<double, 2, 2>::Zero(), 0, 1.0);
+ EXPECT_LT((Q_d_integrated - Q_d).norm(), 1e-10)
+ << "Expected these to be nearly equal:\nQ_d:\n" << Q_d
+ << "\nQ_d_integrated:\n" << Q_d_integrated;
+}
+
+} // namespace testing
+} // namespace controls
+} // namespace frc971
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 0af0144..ad7551f 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -336,43 +336,15 @@
void UpdateQR(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
number_of_outputs> *plant,
::std::chrono::nanoseconds dt) {
- // Now, compute the discrete time Q and R coefficients.
- Eigen::Matrix<Scalar, number_of_states, number_of_states> Qtemp =
- (coefficients().Q_continuous +
- coefficients().Q_continuous.transpose()) /
- static_cast<Scalar>(2.0);
+ ::frc971::controls::DiscretizeQ(coefficients().Q_continuous,
+ plant->coefficients().A_continuous, dt,
+ &Q_);
+
Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> Rtemp =
(coefficients().R_continuous +
coefficients().R_continuous.transpose()) /
static_cast<Scalar>(2.0);
- Eigen::Matrix<Scalar, 2 * number_of_states, 2 * number_of_states> M_gain;
- M_gain.setZero();
- // Set up the matrix M = [[-A, Q], [0, A.T]]
- M_gain.template block<number_of_states, number_of_states>(0, 0) =
- -plant->coefficients().A_continuous;
- M_gain.template block<number_of_states, number_of_states>(
- 0, number_of_states) = Qtemp;
- M_gain.template block<number_of_states, number_of_states>(
- number_of_states, number_of_states) =
- plant->coefficients().A_continuous.transpose();
-
- Eigen::Matrix<Scalar, 2 * number_of_states, 2 *number_of_states> phi =
- (M_gain *
- ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
- .count())
- .exp();
-
- // Phi12 = phi[0:number_of_states, number_of_states:2*number_of_states]
- // Phi22 = phi[number_of_states:2*number_of_states,
- // number_of_states:2*number_of_states]
- Eigen::Matrix<Scalar, number_of_states, number_of_states> phi12 =
- phi.block(0, number_of_states, number_of_states, number_of_states);
- Eigen::Matrix<Scalar, number_of_states, number_of_states> phi22 = phi.block(
- number_of_states, number_of_states, number_of_states, number_of_states);
-
- Q_ = phi22.transpose() * phi12;
- Q_ = (Q_ + Q_.transpose()) / static_cast<Scalar>(2.0);
R_ = Rtemp /
::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
.count();