[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/LTVUnicycleController.cpp

@@ -6,8 +6,10 @@
 
 #include <stdexcept>
 
+#include "Eigen/Cholesky"
+#include "frc/DARE.h"
 #include "frc/StateSpaceUtil.h"
-#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/system/Discretization.h"
 #include "units/math.h"
 
 using namespace frc;
@@ -83,17 +85,28 @@ LTVUnicycleController::LTVUnicycleController(
   Matrixd<3, 3> Q = frc::MakeCostMatrix(Qelems);
   Matrixd<2, 2> R = frc::MakeCostMatrix(Relems);
 
+  auto R_llt = R.llt();
+
   for (auto velocity = -maxVelocity; velocity < maxVelocity;
        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, 3>::Zero());
+      A(State::kY, State::kHeading) = 1e-4;
     } else {
       A(State::kY, State::kHeading) = velocity.value();
-      m_table.insert(velocity,
-                     frc::LinearQuadraticRegulator<3, 2>{A, B, Q, R, dt}.K());
     }
+
+    Matrixd<3, 3> discA;
+    Matrixd<3, 2> discB;
+    DiscretizeAB(A, B, dt, &discA, &discB);
+
+    Matrixd<3, 3> S = detail::DARE<3, 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));
   }
 }