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

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

@@ -166,6 +166,9 @@ class ControlAffinePlantInversionFeedforward {
   InputVector Calculate(const StateVector& r, const StateVector& nextR) {
     StateVector rDot = (nextR - r) / m_dt.value();
 
+    // ṙ = f(r) + Bu
+    // Bu = ṙ − f(r)
+    // u = B⁺(ṙ − f(r))
     m_uff = m_B.householderQr().solve(rDot - m_f(r, InputVector::Zero()));
 
     m_r = nextR;

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

@@ -138,6 +138,9 @@ class LinearPlantInversionFeedforward {
    * @return The calculated feedforward.
    */
   InputVector Calculate(const StateVector& r, const StateVector& nextR) {
+    // rₖ₊₁ = Arₖ + Buₖ
+    // Buₖ = rₖ₊₁ − Arₖ
+    // uₖ = B⁺(rₖ₊₁ − Arₖ)
     m_uff = m_B.householderQr().solve(nextR - (m_A * r));
     m_r = nextR;
     return m_uff;