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] Use SDA algorithm instead of SSCA for DARE solver (#5526)

Browse Source 
Both seem to work, but the SDA algorithm is specifically recommended for
solving DAREs as opposed to P-DAREs.

The QR decomposition was replaced with a partial pivoting LU
decomposition at the recommendation of section 2.4 of the paper.

More tests and a separate JNI function for each DARE solver variant were
added.
This commit is contained in:
Tyler Veness
2023-08-12 19:45:45 -07:00
committed by GitHub
parent a4b7fde767
commit 394cfeadbd
7 changed files with 549 additions and 155 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
wpimath/src/main/java/edu/wpi/first/math/DARE.java
View File

@@ -19,10 +19,14 @@ public final class DARE {
* @param Q State cost matrix.
* @param R Input cost matrix.
* @return Solution of DARE.
* @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
* @throws IllegalArgumentException if R isn't symmetric positive definite.
* @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
* @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
*/
public static SimpleMatrix dare(SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) {
var S = new SimpleMatrix(A.getNumRows(), A.getNumCols());
WPIMathJNI.dare(
WPIMathJNI.dareABQR(
A.getDDRM().getData(),
B.getDDRM().getData(),
Q.getDDRM().getData(),
@@ -43,6 +47,10 @@ public final class DARE {
* @param Q State cost matrix.
* @param R Input cost matrix.
* @return Solution of DARE.
* @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
* @throws IllegalArgumentException if R isn't symmetric positive definite.
* @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
* @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
*/
public static <States extends Num, Inputs extends Num> Matrix<States, States> dare(
Matrix<States, States> A,
@@ -61,21 +69,20 @@ public final class DARE {
* @param R Input cost matrix.
* @param N State-input cross-term cost matrix.
* @return Solution of DARE.
* @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
* @throws IllegalArgumentException if R isn't symmetric positive definite.
* @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
* @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
*/
public static SimpleMatrix dare(
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.getNumRows(), A.getNumCols());
WPIMathJNI.dare(
scrA.getDDRM().getData(),
WPIMathJNI.dareABQRN(
A.getDDRM().getData(),
B.getDDRM().getData(),
scrQ.getDDRM().getData(),
Q.getDDRM().getData(),
R.getDDRM().getData(),
N.getDDRM().getData(),
A.getNumCols(),
B.getNumCols(),
S.getDDRM().getData());
@@ -93,6 +100,10 @@ public final class DARE {
* @param R Input cost matrix.
* @param N State-input cross-term cost matrix.
* @return Solution of DARE.
* @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
* @throws IllegalArgumentException if R isn't symmetric positive definite.
* @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
* @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
*/
public static <States extends Num, Inputs extends Num> Matrix<States, States> dare(
Matrix<States, States> A,
@@ -100,22 +111,7 @@ public final class DARE {
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R,
Matrix<States, Inputs> N) {
// This is a change of variables to make the DARE that includes Q, R, and N
// cost matrices fit the form of the DARE that includes only Q and R cost
// matrices.
//
// This is equivalent to solving the original DARE:
//
// A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
//
// where A₂ and Q₂ are a change of variables:
//
// A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
return new Matrix<>(
dare(
A.minus(B.times(R.solve(N.transpose()))).getStorage(),
B.getStorage(),
Q.minus(N.times(R.solve(N.transpose()))).getStorage(),
R.getStorage()));
dare(A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage(), N.getStorage()));
}
}