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