[wpimath] Use SDA algorithm instead of SSCA for DARE solver (#5526)

Both seem to work, but the SDA algorithm is specifically recommended for
solving DAREs as opposed to P-DAREs.

The QR decomposition was replaced with a partial pivoting LU
decomposition at the recommendation of section 2.4 of the paper.

More tests and a separate JNI function for each DARE solver variant were
added.

wpimath/src/main/native/include/frc/DARE.h

@@ -11,7 +11,6 @@
 namespace frc {
 
 /**
- *
  * Computes the unique stabilizing solution X to the discrete-time algebraic
  * Riccati equation:
  *
@@ -21,8 +20,6 @@ namespace frc {
  * @param B The input matrix.
  * @param Q The state cost matrix.
  * @param R The input cost matrix.
- * @throws std::invalid_argument if number of inputs is greater than number of
- *   states.
  * @throws std::invalid_argument if Q isn't symmetric positive semidefinite.
  * @throws std::invalid_argument if R isn't symmetric positive definite.
  * @throws std::invalid_argument if the (A, B) pair isn't stabilizable.
@@ -73,8 +70,6 @@ J = Σ [uₖ] [0 R][uₖ] ΔT
 @param Q The state cost matrix.
 @param R The input cost matrix.
 @param N The state-input cross cost matrix.
-@throws std::invalid_argument if number of inputs is greater than number of
-  states.
 @throws std::invalid_argument if Q − NR⁻¹Nᵀ isn't symmetric positive
   semidefinite.
 @throws std::invalid_argument if R isn't symmetric positive definite.
@@ -88,4 +83,90 @@ Eigen::MatrixXd DARE(const Eigen::Ref<const Eigen::MatrixXd>& A,
                      const Eigen::Ref<const Eigen::MatrixXd>& R,
                      const Eigen::Ref<const Eigen::MatrixXd>& N);
 
+namespace internal {
+
+/**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic
+ * Riccati equation:
+ *
+ *   AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ *
+ * This internal function skips expensive precondition checks for increased
+ * performance. The solver may hang if any of the following occur:
+ * <ul>
+ *   <li>Q isn't symmetric positive semidefinite</li>
+ *   <li>R isn't symmetric positive definite</li>
+ *   <li>The (A, B) pair isn't stabilizable</li>
+ *   <li>The (A, C) pair where Q = CᵀC isn't detectable</li>
+ * </ul>
+ * Only use this function if you're sure the preconditions are met.
+ *
+ * @param A The system matrix.
+ * @param B The input matrix.
+ * @param Q The state cost matrix.
+ * @param R The input cost matrix.
+ */
+WPILIB_DLLEXPORT
+Eigen::MatrixXd DARE(const Eigen::Ref<const Eigen::MatrixXd>& A,
+                     const Eigen::Ref<const Eigen::MatrixXd>& B,
+                     const Eigen::Ref<const Eigen::MatrixXd>& Q,
+                     const Eigen::Ref<const Eigen::MatrixXd>& R);
+
+/**
+Computes the unique stabilizing solution X to the discrete-time algebraic
+Riccati equation:
+
+  AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+
+This overload of the DARE is useful for finding the control law uₖ that
+minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+
+@verbatim
+    ∞ [xₖ]ᵀ[Q  N][xₖ]
+J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   k=0
+@endverbatim
+
+This is a more general form of the following. The linear-quadratic regulator
+is the feedback control law uₖ that minimizes the following cost function
+subject to xₖ₊₁ = Axₖ + Buₖ:
+
+@verbatim
+    ∞
+J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   k=0
+@endverbatim
+
+This can be refactored as:
+
+@verbatim
+    ∞ [xₖ]ᵀ[Q 0][xₖ]
+J = Σ [uₖ] [0 R][uₖ] ΔT
+   k=0
+@endverbatim
+
+This internal function skips expensive precondition checks for increased
+performance. The solver may hang if any of the following occur:
+<ul>
+  <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite</li>
+  <li>R isn't symmetric positive definite</li>
+  <li>The (A, B) pair isn't stabilizable</li>
+  <li>The (A, C) pair where Q = CᵀC isn't detectable</li>
+</ul>
+Only use this function if you're sure the preconditions are met.
+
+@param A The system matrix.
+@param B The input matrix.
+@param Q The state cost matrix.
+@param R The input cost matrix.
+@param N The state-input cross cost matrix.
+*/
+WPILIB_DLLEXPORT
+Eigen::MatrixXd DARE(const Eigen::Ref<const Eigen::MatrixXd>& A,
+                     const Eigen::Ref<const Eigen::MatrixXd>& B,
+                     const Eigen::Ref<const Eigen::MatrixXd>& Q,
+                     const Eigen::Ref<const Eigen::MatrixXd>& R,
+                     const Eigen::Ref<const Eigen::MatrixXd>& N);
+
+}  // namespace internal
 }  // namespace frc