[wpilib] Add function to adjust LQR controller gain for pure time delay (#2878)

There were three options for where to put this function:

1. A free function in LinearQuadraticRegulator.h. Returning a K matrix
   means the user can't use the LinearQuadraticRegulator in a loop
   anymore.
2. A default argument added to ctors in LinearQuadraticRegulator for a
   time delay (default of 0). This has the smallest API footprint from
   the user perspective, but it bloats the already substantial
   constructor overload set of LinearQuadraticRegulator.
3. A member function in LinearQuadraticRegulator that modifies the
   internal K. This would still have to take in a LinearSystem or (A, B)
   pair because the ctor doesn't store it. Storing it internally feels
   like paying for what we don't use most of the time.

I went with option 3.

I verified the tests's expected values in Python with
scipy.linalg.fractional_matrix_power().

Closes #2877.

wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java

@@ -69,11 +69,21 @@ public final class WPIMathJNI {
    * Computes the matrix exp.
    *
    * @param src  Array of elements of the matrix to be exponentiated.
-   * @param rows how many rows there are.
+   * @param rows How many rows there are.
    * @param dst  Array where the result will be stored.
    */
   public static native void exp(double[] src, int rows, double[] dst);
 
+  /**
+   * Computes the matrix pow.
+   *
+   * @param src      Array of elements of the matrix to be raised to a power.
+   * @param rows     How many rows there are.
+   * @param exponent The exponent.
+   * @param dst      Array where the result will be stored.
+   */
+  public static native void pow(double[] src, int rows, double exponent, double[] dst);
+
   /**
    * Returns true if (A, B) is a stabilizable pair.
    *

wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java

@@ -211,4 +211,30 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num,
     m_r = nextR;
     return calculate(x);
   }
+
+  /**
+   * Adjusts LQR controller gain to compensate for a pure time delay in the
+   * input.
+   *
+   * <p>Linear-Quadratic regulator controller gains tend to be aggressive. If
+   * sensor measurements are time-delayed too long, the LQR may be unstable.
+   * However, if we know the amount of delay, we can compute the control based
+   * on where the system will be after the time delay.
+   *
+   * <p>See https://file.tavsys.net/control/controls-engineering-in-frc.pdf
+   * appendix C.4 for a derivation.
+   *
+   * @param plant             The plant being controlled.
+   * @param dtSeconds         Discretization timestep in seconds.
+   * @param inputDelaySeconds Input time delay in seconds.
+   */
+  public void latencyCompensate(
+      LinearSystem<States, Inputs, Outputs> plant, double dtSeconds,
+      double inputDelaySeconds) {
+    var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dtSeconds);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    m_K = m_K.times((discA.minus(discB.times(m_K))).pow(inputDelaySeconds / dtSeconds));
+  }
 }

wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java

@@ -368,6 +368,25 @@ public class Matrix<R extends Num, C extends Num> {
     return toReturn;
   }
 
+  /**
+   * Computes the matrix power using Eigen's solver.
+   * This method only works for square matrices, and will
+   * otherwise throw an {@link MatrixDimensionException}.
+   *
+   * @param exponent The exponent.
+   * @return The exponential of A.
+   */
+  public final Matrix<R, C> pow(double exponent) {
+    if (this.getNumRows() != this.getNumCols()) {
+      throw new MatrixDimensionException("Non-square matrices cannot be raised to a power! "
+            + "This matrix is " + this.getNumRows() + " x " + this.getNumCols());
+    }
+    Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
+    WPIMathJNI.pow(this.m_storage.getDDRM().getData(), this.getNumRows(), exponent,
+          toReturn.m_storage.getDDRM().getData());
+    return toReturn;
+  }
+
   /**
    * Returns the determinant of this matrix.
    *