[wpimath] Fix LQR matrix constructor overload for Q, R, and N (#3884)

It was using the continuous B matrix to compute the feedback gain
instead of the discrete B matrix.

Tests were added for the matrix constructor overloads.

wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h

@@ -135,7 +135,7 @@ class LinearQuadraticRegulatorImpl {
         drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
 
     // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
-    m_K = (B.transpose() * S * B + R)
+    m_K = (discB.transpose() * S * discB + R)
               .llt()
               .solve(discB.transpose() * S * discA + N.transpose());