Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add DARE solver for Q, R, and N with LQR ctor overloads

Browse Source 
This is useful for implementing implicit model following.
This commit is contained in:
Tyler Veness
2021-02-15 18:17:55 -08:00
committed by Peter Johnson
parent b2c3b2dd8e
commit edd2f0232c
6 changed files with 216 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
wpimath/src/main/java/edu/wpi/first/math/Drake.java
View File

@@ -53,4 +53,64 @@ public final class Drake {
discreteAlgebraicRiccatiEquation(
A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage()));
}
/**
* Solves the discrete alegebraic Riccati equation.
*
* @param A System matrix.
* @param B Input matrix.
* @param Q State cost matrix.
* @param R Input cost matrix.
* @param N State-input cross-term cost matrix.
* @return Solution of DARE.
*/
@SuppressWarnings({"LocalVariableName", "ParameterName"})
public static SimpleMatrix discreteAlgebraicRiccatiEquation(
SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R, SimpleMatrix N) {
// See
// https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
// for the change of variables used here.
var scrA = A.minus(B.mult(R.solve(N.transpose())));
var scrQ = Q.minus(N.mult(R.solve(N.transpose())));
var S = new SimpleMatrix(A.numRows(), A.numCols());
WPIMathJNI.discreteAlgebraicRiccatiEquation(
scrA.getDDRM().getData(),
B.getDDRM().getData(),
scrQ.getDDRM().getData(),
R.getDDRM().getData(),
A.numCols(),
B.numCols(),
S.getDDRM().getData());
return S;
}
/**
* Solves the discrete alegebraic Riccati equation.
*
* @param A System matrix.
* @param B Input matrix.
* @param Q State cost matrix.
* @param R Input cost matrix.
* @param N State-input cross-term cost matrix.
* @return Solution of DARE.
*/
@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public static <States extends Num, Inputs extends Num>
Matrix<States, States> discreteAlgebraicRiccatiEquation(
Matrix<States, States> A,
Matrix<States, Inputs> B,
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R,
Matrix<States, Inputs> N) {
// See
// https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
// for the change of variables used here.
var scrA = A.minus(B.times(R.solve(N.transpose())));
var scrQ = Q.minus(N.times(R.solve(N.transpose())));
return new Matrix<>(
discreteAlgebraicRiccatiEquation(
scrA.getStorage(), B.getStorage(), scrQ.getStorage(), R.getStorage()));
}
}