[wpimath] Add core State-space classes (#2614)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>

wpimath/src/main/native/cpp/StateSpaceUtil.cpp

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/StateSpaceUtil.h"
+
+namespace frc {
+
+template <>
+bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
+                          const Eigen::Matrix<double, 1, 1>& B) {
+  return detail::IsStabilizableImpl<1, 1>(A, B);
+}
+
+template <>
+bool IsStabilizable<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
+                          const Eigen::Matrix<double, 2, 1>& B) {
+  return detail::IsStabilizableImpl<2, 1>(A, B);
+}
+
+Eigen::Matrix<double, 3, 1> PoseToVector(const Pose2d& pose) {
+  return frc::MakeMatrix<3, 1>(pose.X().to<double>(), pose.Y().to<double>(),
+                               pose.Rotation().Radians().to<double>());
+}
+
+}  // namespace frc

wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/LinearQuadraticRegulator.h"
+
+namespace frc {
+
+LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
+    const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
+    const std::array<double, 1>& Qelems, const std::array<double, 1>& Relems,
+    units::second_t dt)
+    : LinearQuadraticRegulator(A, B, Qelems, 1.0, Relems, dt) {}
+
+LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
+    const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
+    const std::array<double, 1>& Qelems, const double rho,
+    const std::array<double, 1>& Relems, units::second_t dt)
+    : detail::LinearQuadraticRegulatorImpl<1, 1>{A,   B,      Qelems,
+                                                 rho, Relems, dt} {}
+
+LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
+    const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
+    const Eigen::Matrix<double, 1, 1>& Q, const Eigen::Matrix<double, 1, 1>& R,
+    units::second_t dt)
+    : detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, dt) {}
+
+LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+    const std::array<double, 2>& Qelems, const std::array<double, 1>& Relems,
+    units::second_t dt)
+    : LinearQuadraticRegulator(A, B, Qelems, 1.0, Relems, dt) {}
+
+LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+    const std::array<double, 2>& Qelems, const double rho,
+    const std::array<double, 1>& Relems, units::second_t dt)
+    : detail::LinearQuadraticRegulatorImpl<2, 1>{A,   B,      Qelems,
+                                                 rho, Relems, dt} {}
+
+LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
+    units::second_t dt)
+    : detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, dt) {}
+
+}  // namespace frc

wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/estimator/KalmanFilter.h"
+
+namespace frc {
+
+KalmanFilter<1, 1, 1>::KalmanFilter(
+    LinearSystem<1, 1, 1>& plant, const std::array<double, 1>& stateStdDevs,
+    const std::array<double, 1>& measurementStdDevs, units::second_t dt)
+    : detail::KalmanFilterImpl<1, 1, 1>{plant, stateStdDevs, measurementStdDevs,
+                                        dt} {}
+
+KalmanFilter<2, 1, 1>::KalmanFilter(
+    LinearSystem<2, 1, 1>& plant, const std::array<double, 2>& stateStdDevs,
+    const std::array<double, 1>& measurementStdDevs, units::second_t dt)
+    : detail::KalmanFilterImpl<2, 1, 1>{plant, stateStdDevs, measurementStdDevs,
+                                        dt} {}
+
+}  // namespace frc

wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp

@@ -10,12 +10,39 @@
 #include <wpi/jni_util.h>
 
 #include "Eigen/Core"
+#include "Eigen/Eigenvalues"
+#include "Eigen/QR"
 #include "drake/math/discrete_algebraic_riccati_equation.h"
 #include "edu_wpi_first_math_WPIMathJNI.h"
 #include "unsupported/Eigen/MatrixFunctions"
 
 using namespace wpi::java;
 
+bool check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
+                        const Eigen::Ref<const Eigen::MatrixXd>& B) {
+  // This function checks if (A,B) is a stabilizable pair.
+  // (A,B) is stabilizable if and only if the uncontrollable eigenvalues of
+  // A, if any, have absolute values less than one, where an eigenvalue is
+  // uncontrollable if Rank[lambda * I - A, B] < n.
+  int n = B.rows(), m = B.cols();
+  Eigen::EigenSolver<Eigen::MatrixXd> es(A);
+  for (int i = 0; i < n; i++) {
+    if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
+            es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
+        1)
+      continue;
+
+    Eigen::MatrixXcd E(n, n + m);
+    E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B;
+    Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr(E);
+    if (qr.rank() != n) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
 extern "C" {
 
 /*
@@ -57,4 +84,54 @@ Java_edu_wpi_first_math_WPIMathJNI_discreteAlgebraicRiccatiEquation
   env->SetDoubleArrayRegion(S, 0, states * states, result.data());
 }
 
+/*
+ * Class:     edu_wpi_first_wpiutil_math_WPIMathJNI
+ * Method:    exp
+ * Signature: ([DI[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_wpiutil_math_WPIMathJNI_exp
+  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst)
+{
+  jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr);
+
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      Amat{arrayBody, rows, rows};
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Aexp =
+      Amat.exp();
+
+  env->ReleaseDoubleArrayElements(src, arrayBody, 0);
+  env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data());
+}
+
+/*
+ * Class:     edu_wpi_first_wpiutil_math_WPIMathJNI
+ * Method:    isStabilizable
+ * Signature: (II[D[D)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_wpiutil_math_WPIMathJNI_isStabilizable
+  (JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc,
+   jdoubleArray bSrc)
+{
+  jdouble* nativeA = env->GetDoubleArrayElements(aSrc, nullptr);
+  jdouble* nativeB = env->GetDoubleArrayElements(bSrc, nullptr);
+
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      A{nativeA, states, states};
+
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      B{nativeB, states, inputs};
+
+  bool isStabilizable = check_stabilizable(A, B);
+
+  env->ReleaseDoubleArrayElements(aSrc, nativeA, 0);
+  env->ReleaseDoubleArrayElements(bSrc, nativeB, 0);
+
+  return isStabilizable;
+}
+
 }  // extern "C"