[wpimath] Add math docs to plant inversion feedforward internals (NFC) (#5618)

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

@@ -181,6 +181,9 @@ public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs e
   public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
     var rDot = (nextR.minus(r)).div(m_dt);
 
+    // ṙ = f(r) + Bu
+    // Bu = ṙ − f(r)
+    // u = B⁺(ṙ − f(r))
     m_uff = m_B.solve(rDot.minus(m_f.apply(r, new Matrix<>(m_inputs, Nat.N1()))));
 
     m_r = nextR;

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

@@ -139,6 +139,9 @@ public class LinearPlantInversionFeedforward<
    * @return The calculated feedforward.
    */
   public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
+    // rₖ₊₁ = Arₖ + Buₖ
+    // Buₖ = rₖ₊₁ − Arₖ
+    // uₖ = B⁺(rₖ₊₁ − Arₖ)
     m_uff = new Matrix<>(m_B.solve(nextR.minus(m_A.times(r))));
 
     m_r = nextR;