[wpimath] Add LTV controller derivations and make enums private (#4380)

The LTV differential drive controller derivation wasn't included inline
because it's too long.

wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java

@@ -38,7 +38,7 @@ public class LTVDifferentialDriveController {
   private Matrix<N5, N1> m_tolerance = new Matrix<>(Nat.N5(), Nat.N1());
 
   /** States of the drivetrain system. */
-  enum State {
+  private enum State {
     kX(0),
     kY(1),
     kHeading(2),
@@ -73,6 +73,8 @@ public class LTVDifferentialDriveController {
       double dt) {
     m_trackwidth = trackwidth;
 
+    // Control law derivation is in section 8.7 of
+    // https://file.tavsys.net/control/controls-engineering-in-frc.pdf
     var A =
         new MatBuilder<>(Nat.N5(), Nat.N5())
             .fill(

wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java

@@ -36,7 +36,7 @@ public class LTVUnicycleController {
   private boolean m_enabled = true;
 
   /** States of the drivetrain system. */
-  enum State {
+  private enum State {
     kX(0),
     kY(1),
     kHeading(2);
@@ -50,20 +50,6 @@ public class LTVUnicycleController {
     }
   }
 
-  /** Inputs of the drivetrain system. */
-  enum Input {
-    kLeftVelocity(3),
-    kRightVelocity(4);
-
-    @SuppressWarnings("MemberName")
-    public final int value;
-
-    @SuppressWarnings("ParameterName")
-    Input(int i) {
-      this.value = i;
-    }
-  }
-
   /**
    * Constructs a linear time-varying unicycle controller with default maximum desired error
    * tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s,
@@ -111,6 +97,39 @@ public class LTVUnicycleController {
   @SuppressWarnings("LocalVariableName")
   public LTVUnicycleController(
       Vector<N3> qelems, Vector<N2> relems, double dt, double maxVelocity) {
+    // The change in global pose for a unicycle is defined by the following
+    // three equations.
+    //
+    // ẋ = v cosθ
+    // ẏ = v sinθ
+    // θ̇ = ω
+    //
+    // Here's the model as a vector function where x = [x  y  θ]ᵀ and
+    // u = [v  ω]ᵀ.
+    //
+    //           [v cosθ]
+    // f(x, u) = [v sinθ]
+    //           [  ω   ]
+    //
+    // To create an LQR, we need to linearize this.
+    //
+    //               [0  0  −v sinθ]                  [cosθ  0]
+    // ∂f(x, u)/∂x = [0  0   v cosθ]    ∂f(x, u)/∂u = [sinθ  0]
+    //               [0  0     0   ]                  [ 0    1]
+    //
+    // We're going to make a cross-track error controller, so we'll apply a
+    // clockwise rotation matrix to the global tracking error to transform it
+    // into the robot's coordinate frame. Since the cross-track error is always
+    // measured from the robot's coordinate frame, the model used to compute the
+    // LQR should be linearized around θ = 0 at all times.
+    //
+    //     [0  0  −v sin0]        [cos0  0]
+    // A = [0  0   v cos0]    B = [sin0  0]
+    //     [0  0     0   ]        [ 0    1]
+    //
+    //     [0  0  0]              [1  0]
+    // A = [0  0  v]          B = [0  0]
+    //     [0  0  0]              [0  1]
     var A = new Matrix<>(Nat.N3(), Nat.N3());
     var B = new MatBuilder<>(Nat.N3(), Nat.N2()).fill(1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
     var Q = StateSpaceUtil.makeCostMatrix(qelems);