[wpimath] Make LTV controller constructors use faster DARE solver (#5543)

Made JNI modifications to expose the faster function, made the API use
the typesafe Matrix API, and synchronized the documentation with C++.

Sped up C++ LTV diff drive test from 20 ms to 15 ms.
Sped up C++ LTV unicycle test from 15 ms to 10 ms.

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

@@ -7,9 +7,11 @@
 #include <cmath>
 #include <stdexcept>
 
+#include "Eigen/Cholesky"
+#include "frc/DARE.h"
 #include "frc/MathUtil.h"
 #include "frc/StateSpaceUtil.h"
-#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/system/Discretization.h"
 
 using namespace frc;
 
@@ -64,6 +66,7 @@ LTVDifferentialDriveController::LTVDifferentialDriveController(
   // Ax = -Bu
   // x = -A⁻¹Bu
   units::meters_per_second_t maxV{
+      // NOLINTNEXTLINE(clang-analyzer-unix.Malloc)
       -plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)};
 
   if (maxV <= 0_mps) {
@@ -75,16 +78,27 @@ LTVDifferentialDriveController::LTVDifferentialDriveController(
         "Max velocity of plant with 12 V input must be less than 15 m/s.");
   }
 
+  auto R_llt = R.llt();
+
   for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) {
     // The DARE is ill-conditioned if the velocity is close to zero, so don't
     // let the system stop.
     if (units::math::abs(velocity) < 1e-4_mps) {
-      m_table.insert(velocity, Matrixd<2, 5>::Zero());
+      A(State::kY, State::kHeading) = 1e-4;
     } else {
       A(State::kY, State::kHeading) = velocity.value();
-      m_table.insert(velocity,
-                     frc::LinearQuadraticRegulator<5, 2>{A, B, Q, R, dt}.K());
     }
+
+    Matrixd<5, 5> discA;
+    Matrixd<5, 2> discB;
+    DiscretizeAB(A, B, dt, &discA, &discB);
+
+    Matrixd<5, 5> S = detail::DARE<5, 2>(discA, discB, Q, R_llt);
+
+    // K = (BᵀSB + R)⁻¹BᵀSA
+    m_table.insert(velocity, (discB.transpose() * S * discB + R)
+                                 .llt()
+                                 .solve(discB.transpose() * S * discA));
   }
 }