mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpimath] Use SDA algorithm instead of SSCA for DARE solver (#5526)
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
@@ -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()));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -53,10 +53,40 @@ public final class WPIMathJNI {
|
||||
* @param states Number of states in A matrix.
|
||||
* @param inputs Number of inputs in B matrix.
|
||||
* @param S Array storage for DARE solution.
|
||||
* @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 native void dare(
|
||||
public static native void dareABQR(
|
||||
double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
|
||||
|
||||
/**
|
||||
* Solves the discrete alegebraic Riccati equation.
|
||||
*
|
||||
* @param A Array containing elements of A in row-major order.
|
||||
* @param B Array containing elements of B in row-major order.
|
||||
* @param Q Array containing elements of Q in row-major order.
|
||||
* @param R Array containing elements of R in row-major order.
|
||||
* @param N Array containing elements of N in row-major order.
|
||||
* @param states Number of states in A matrix.
|
||||
* @param inputs Number of inputs in B matrix.
|
||||
* @param S Array storage for DARE solution.
|
||||
* @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 native void dareABQRN(
|
||||
double[] A,
|
||||
double[] B,
|
||||
double[] Q,
|
||||
double[] R,
|
||||
double[] N,
|
||||
int states,
|
||||
int inputs,
|
||||
double[] S);
|
||||
|
||||
/**
|
||||
* Computes the matrix exp.
|
||||
*
|
||||
|
||||
Reference in New Issue
Block a user