[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.
This commit is contained in:
Tyler Veness
2020-11-20 15:28:00 -08:00
committed by GitHub
parent 2816b06c05
commit c8ea1b6c38
7 changed files with 154 additions and 2 deletions

View File

@@ -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.
*