[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;