mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpimath] Make LTV controller constructors use faster DARE solver (#5543)
Made JNI modifications to expose the faster function, made the API use the typesafe Matrix API, and synchronized the documentation with C++. Sped up C++ LTV diff drive test from 20 ms to 15 ms. Sped up C++ LTV unicycle test from 15 ms to 10 ms.
This commit is contained in:
Tyler Veness
@@ -12,33 +12,122 @@ public final class DARE {
|
||||
}
|
||||
|
||||
/**
|
||||
* Solves the discrete algebraic Riccati equation.
|
||||
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
|
||||
*
|
||||
* <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
|
||||
*
|
||||
* <p>This internal function skips expensive precondition checks for increased performance. The
|
||||
* solver may hang if any of the following occur:
|
||||
*
|
||||
* <ul>
|
||||
* <li>Q isn't symmetric positive semidefinite
|
||||
* <li>R isn't symmetric positive definite
|
||||
* <li>The (A, B) pair isn't stabilizable
|
||||
* <li>The (A, C) pair where Q = CᵀC isn't detectable
|
||||
* </ul>
|
||||
*
|
||||
* <p>Only use this function if you're sure the preconditions are met.
|
||||
*
|
||||
* @param <States> Number of states.
|
||||
* @param <Inputs> Number of inputs.
|
||||
* @param A System matrix.
|
||||
* @param B Input matrix.
|
||||
* @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.dareABQR(
|
||||
A.getDDRM().getData(),
|
||||
B.getDDRM().getData(),
|
||||
Q.getDDRM().getData(),
|
||||
R.getDDRM().getData(),
|
||||
public static <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail(
|
||||
Matrix<States, States> A,
|
||||
Matrix<States, Inputs> B,
|
||||
Matrix<States, States> Q,
|
||||
Matrix<Inputs, Inputs> R) {
|
||||
var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
|
||||
WPIMathJNI.dareDetailABQR(
|
||||
A.getStorage().getDDRM().getData(),
|
||||
B.getStorage().getDDRM().getData(),
|
||||
Q.getStorage().getDDRM().getData(),
|
||||
R.getStorage().getDDRM().getData(),
|
||||
A.getNumCols(),
|
||||
B.getNumCols(),
|
||||
S.getDDRM().getData());
|
||||
S.getStorage().getDDRM().getData());
|
||||
return S;
|
||||
}
|
||||
|
||||
/**
|
||||
* Solves the discrete algebraic Riccati equation.
|
||||
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
|
||||
*
|
||||
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
|
||||
*
|
||||
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
|
||||
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
* J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
|
||||
* control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
|
||||
*
|
||||
* <pre>
|
||||
* ∞
|
||||
* J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This can be refactored as:
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
* J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This internal function skips expensive precondition checks for increased performance. The
|
||||
* solver may hang if any of the following occur:
|
||||
*
|
||||
* <ul>
|
||||
* <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
|
||||
* <li>R isn't symmetric positive definite
|
||||
* <li>The (A, B) pair isn't stabilizable
|
||||
* <li>The (A, C) pair where Q = CᵀC isn't detectable
|
||||
* </ul>
|
||||
*
|
||||
* <p>Only use this function if you're sure the preconditions are met.
|
||||
*
|
||||
* @param <States> Number of states.
|
||||
* @param <Inputs> Number of inputs.
|
||||
* @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.
|
||||
*/
|
||||
public static <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail(
|
||||
Matrix<States, States> A,
|
||||
Matrix<States, Inputs> B,
|
||||
Matrix<States, States> Q,
|
||||
Matrix<Inputs, Inputs> R,
|
||||
Matrix<States, Inputs> N) {
|
||||
var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
|
||||
WPIMathJNI.dareDetailABQRN(
|
||||
A.getStorage().getDDRM().getData(),
|
||||
B.getStorage().getDDRM().getData(),
|
||||
Q.getStorage().getDDRM().getData(),
|
||||
R.getStorage().getDDRM().getData(),
|
||||
N.getStorage().getDDRM().getData(),
|
||||
A.getNumCols(),
|
||||
B.getNumCols(),
|
||||
S.getStorage().getDDRM().getData());
|
||||
return S;
|
||||
}
|
||||
|
||||
/**
|
||||
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
|
||||
*
|
||||
* <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
|
||||
*
|
||||
* @param <States> Number of states.
|
||||
* @param <Inputs> Number of inputs.
|
||||
@@ -57,40 +146,48 @@ public final class DARE {
|
||||
Matrix<States, Inputs> B,
|
||||
Matrix<States, States> Q,
|
||||
Matrix<Inputs, Inputs> R) {
|
||||
return new Matrix<>(dare(A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Solves the discrete algebraic 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.
|
||||
* @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) {
|
||||
var S = new SimpleMatrix(A.getNumRows(), A.getNumCols());
|
||||
WPIMathJNI.dareABQRN(
|
||||
A.getDDRM().getData(),
|
||||
B.getDDRM().getData(),
|
||||
Q.getDDRM().getData(),
|
||||
R.getDDRM().getData(),
|
||||
N.getDDRM().getData(),
|
||||
var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
|
||||
WPIMathJNI.dareABQR(
|
||||
A.getStorage().getDDRM().getData(),
|
||||
B.getStorage().getDDRM().getData(),
|
||||
Q.getStorage().getDDRM().getData(),
|
||||
R.getStorage().getDDRM().getData(),
|
||||
A.getNumCols(),
|
||||
B.getNumCols(),
|
||||
S.getDDRM().getData());
|
||||
S.getStorage().getDDRM().getData());
|
||||
return S;
|
||||
}
|
||||
|
||||
/**
|
||||
* Solves the discrete algebraic Riccati equation.
|
||||
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
|
||||
*
|
||||
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
|
||||
*
|
||||
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
|
||||
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
* J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
|
||||
* control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
|
||||
*
|
||||
* <pre>
|
||||
* ∞
|
||||
* J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This can be refactored as:
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
* J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* @param <States> Number of states.
|
||||
* @param <Inputs> Number of inputs.
|
||||
@@ -111,7 +208,16 @@ public final class DARE {
|
||||
Matrix<States, States> Q,
|
||||
Matrix<Inputs, Inputs> R,
|
||||
Matrix<States, Inputs> N) {
|
||||
return new Matrix<>(
|
||||
dare(A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage(), N.getStorage()));
|
||||
var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
|
||||
WPIMathJNI.dareABQRN(
|
||||
A.getStorage().getDDRM().getData(),
|
||||
B.getStorage().getDDRM().getData(),
|
||||
Q.getStorage().getDDRM().getData(),
|
||||
R.getStorage().getDDRM().getData(),
|
||||
N.getStorage().getDDRM().getData(),
|
||||
A.getNumCols(),
|
||||
B.getNumCols(),
|
||||
S.getStorage().getDDRM().getData());
|
||||
return S;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user