mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-24 01:31:46 +00:00
[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:
@@ -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.
|
||||
*
|
||||
|
||||
Reference in New Issue
Block a user