mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[wpimath] Rewrite DARE solver (#5328)
I timed the DARE unit tests, and the new solver is 0 to 100% faster in all cases (that is, it's at least as fast as Drake's and up to 2x faster in some cases). The new solver is also much simpler, takes less time to compile, and drops the libwpimath.so size from 325 MB to 301 MB. I think most of the compilation time is coming from the eigenvalue decompositions used to enforce argument preconditions.
This commit is contained in:
Tyler Veness
185
wpimath/src/test/java/edu/wpi/first/math/DARETest.java
Normal file
185
wpimath/src/test/java/edu/wpi/first/math/DARETest.java
Normal file
@@ -0,0 +1,185 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import org.ejml.simple.SimpleMatrix;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class DARETest {
|
||||
public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) {
|
||||
for (int i = 0; i < A.numRows(); i++) {
|
||||
for (int j = 0; j < A.numCols(); j++) {
|
||||
assertEquals(A.get(i, j), B.get(i, j), 1e-4);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void assertDARESolution(
|
||||
SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R, SimpleMatrix X) {
|
||||
// Check that X is the solution to the DARE
|
||||
// Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
|
||||
var Y =
|
||||
(A.transpose().mult(X).mult(A))
|
||||
.minus(X)
|
||||
.minus(
|
||||
(A.transpose().mult(X).mult(B))
|
||||
.mult((B.transpose().mult(X).mult(B).plus(R)).invert())
|
||||
.mult(B.transpose().mult(X).mult(A)))
|
||||
.plus(Q);
|
||||
assertMatrixEqual(new SimpleMatrix(Y.numRows(), Y.numCols()), Y);
|
||||
}
|
||||
|
||||
void assertDARESolution(
|
||||
SimpleMatrix A,
|
||||
SimpleMatrix B,
|
||||
SimpleMatrix Q,
|
||||
SimpleMatrix R,
|
||||
SimpleMatrix N,
|
||||
SimpleMatrix X) {
|
||||
// Check that X is the solution to the DARE
|
||||
// Y = AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
|
||||
var Y =
|
||||
(A.transpose().mult(X).mult(A))
|
||||
.minus(X)
|
||||
.minus(
|
||||
(A.transpose().mult(X).mult(B).plus(N))
|
||||
.mult((B.transpose().mult(X).mult(B).plus(R)).invert())
|
||||
.mult(B.transpose().mult(X).mult(A).plus(N.transpose())))
|
||||
.plus(Q);
|
||||
assertMatrixEqual(new SimpleMatrix(Y.numRows(), Y.numCols()), Y);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testNonInvertibleA_ABQR() {
|
||||
// Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
|
||||
// Riccati Equation"
|
||||
|
||||
var A =
|
||||
new SimpleMatrix(
|
||||
4, 4, true, new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
|
||||
var B = new SimpleMatrix(4, 1, true, new double[] {0, 0, 0, 1});
|
||||
var Q =
|
||||
new SimpleMatrix(4, 4, true, new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
|
||||
var R = new SimpleMatrix(1, 1, true, new double[] {0.25});
|
||||
|
||||
var X = DARE.dare(A, B, Q, R);
|
||||
assertMatrixEqual(X, X.transpose());
|
||||
assertDARESolution(A, B, Q, R, X);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testNonInvertibleA_ABQRN() {
|
||||
// Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
|
||||
// Riccati Equation"
|
||||
|
||||
var A =
|
||||
new SimpleMatrix(
|
||||
4, 4, true, new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
|
||||
var B = new SimpleMatrix(4, 1, true, new double[] {0, 0, 0, 1});
|
||||
var Q =
|
||||
new SimpleMatrix(4, 4, true, new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
|
||||
var R = new SimpleMatrix(1, 1, true, new double[] {0.25});
|
||||
|
||||
var Aref =
|
||||
new SimpleMatrix(
|
||||
4, 4, true, new double[] {0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
|
||||
Q = A.minus(Aref).transpose().mult(Q).mult(A.minus(Aref));
|
||||
R = B.transpose().mult(Q).mult(B).plus(R);
|
||||
var N = A.minus(Aref).transpose().mult(Q).mult(B);
|
||||
|
||||
var X = DARE.dare(A, B, Q, R, N);
|
||||
assertMatrixEqual(X, X.transpose());
|
||||
assertDARESolution(A, B, Q, R, N, X);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInvertibleA_ABQR() {
|
||||
var A = new SimpleMatrix(2, 2, true, new double[] {1, 1, 0, 1});
|
||||
var B = new SimpleMatrix(2, 1, true, new double[] {0, 1});
|
||||
var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 0});
|
||||
var R = new SimpleMatrix(1, 1, true, new double[] {0.3});
|
||||
|
||||
var X = DARE.dare(A, B, Q, R);
|
||||
assertMatrixEqual(X, X.transpose());
|
||||
assertDARESolution(A, B, Q, R, X);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInvertibleA_ABQRN() {
|
||||
var A = new SimpleMatrix(2, 2, true, new double[] {1, 1, 0, 1});
|
||||
var B = new SimpleMatrix(2, 1, true, new double[] {0, 1});
|
||||
var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 0});
|
||||
var R = new SimpleMatrix(1, 1, true, new double[] {0.3});
|
||||
|
||||
var Aref = new SimpleMatrix(2, 2, true, new double[] {0.5, 1, 0, 1});
|
||||
Q = A.minus(Aref).transpose().mult(Q).mult(A.minus(Aref));
|
||||
R = B.transpose().mult(Q).mult(B).plus(R);
|
||||
var N = A.minus(Aref).transpose().mult(Q).mult(B);
|
||||
|
||||
var X = DARE.dare(A, B, Q, R, N);
|
||||
assertMatrixEqual(X, X.transpose());
|
||||
assertDARESolution(A, B, Q, R, N, X);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testFirstGeneralizedEigenvalueOfSTIsStable_ABQR() {
|
||||
// The first generalized eigenvalue of (S, T) is stable
|
||||
|
||||
var A = new SimpleMatrix(2, 2, true, new double[] {0, 1, 0, 0});
|
||||
var B = new SimpleMatrix(2, 1, true, new double[] {0, 1});
|
||||
var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 1});
|
||||
var R = new SimpleMatrix(1, 1, true, new double[] {1});
|
||||
|
||||
var X = DARE.dare(A, B, Q, R);
|
||||
assertMatrixEqual(X, X.transpose());
|
||||
assertDARESolution(A, B, Q, R, X);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testFirstGeneralizedEigenvalueOfSTIsStable_ABQRN() {
|
||||
// The first generalized eigenvalue of (S, T) is stable
|
||||
|
||||
var A = new SimpleMatrix(2, 2, true, new double[] {0, 1, 0, 0});
|
||||
var B = new SimpleMatrix(2, 1, true, new double[] {0, 1});
|
||||
var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 1});
|
||||
var R = new SimpleMatrix(1, 1, true, new double[] {1});
|
||||
|
||||
var Aref = new SimpleMatrix(2, 2, true, new double[] {0, 0.5, 0, 0});
|
||||
Q = A.minus(Aref).transpose().mult(Q).mult(A.minus(Aref));
|
||||
R = B.transpose().mult(Q).mult(B).plus(R);
|
||||
var N = A.minus(Aref).transpose().mult(Q).mult(B);
|
||||
|
||||
var X = DARE.dare(A, B, Q, R, N);
|
||||
assertMatrixEqual(X, X.transpose());
|
||||
assertDARESolution(A, B, Q, R, N, X);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testIdentitySystem_ABQR() {
|
||||
var A = SimpleMatrix.identity(2);
|
||||
var B = SimpleMatrix.identity(2);
|
||||
var Q = SimpleMatrix.identity(2);
|
||||
var R = SimpleMatrix.identity(2);
|
||||
|
||||
var X = DARE.dare(A, B, Q, R);
|
||||
assertMatrixEqual(X, X.transpose());
|
||||
assertDARESolution(A, B, Q, R, X);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testIdentitySystem_ABQRN() {
|
||||
var A = SimpleMatrix.identity(2);
|
||||
var B = SimpleMatrix.identity(2);
|
||||
var Q = SimpleMatrix.identity(2);
|
||||
var R = SimpleMatrix.identity(2);
|
||||
var N = SimpleMatrix.identity(2);
|
||||
|
||||
var X = DARE.dare(A, B, Q, R, N);
|
||||
assertMatrixEqual(X, X.transpose());
|
||||
assertDARESolution(A, B, Q, R, N, X);
|
||||
}
|
||||
}
|
||||
@@ -1,73 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertTrue;
|
||||
|
||||
import org.ejml.simple.SimpleMatrix;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class DrakeTest {
|
||||
public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) {
|
||||
for (int i = 0; i < A.numRows(); i++) {
|
||||
for (int j = 0; j < A.numCols(); j++) {
|
||||
assertEquals(A.get(i, j), B.get(i, j), 1e-4);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private boolean solveDAREandVerify(
|
||||
SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) {
|
||||
var X = Drake.discreteAlgebraicRiccatiEquation(A, B, Q, R);
|
||||
|
||||
// expect that x is the same as it's transpose
|
||||
assertEquals(X.numRows(), X.numCols());
|
||||
assertMatrixEqual(X, X.transpose());
|
||||
|
||||
// Verify that this is a solution to the DARE.
|
||||
SimpleMatrix Y =
|
||||
A.transpose()
|
||||
.mult(X)
|
||||
.mult(A)
|
||||
.minus(X)
|
||||
.minus(
|
||||
A.transpose()
|
||||
.mult(X)
|
||||
.mult(B)
|
||||
.mult(((B.transpose().mult(X).mult(B)).plus(R)).invert())
|
||||
.mult(B.transpose())
|
||||
.mult(X)
|
||||
.mult(A))
|
||||
.plus(Q);
|
||||
assertMatrixEqual(Y, new SimpleMatrix(Y.numRows(), Y.numCols()));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@Test
|
||||
void testDiscreteAlgebraicRicattiEquation() {
|
||||
int n1 = 4;
|
||||
int m1 = 1;
|
||||
|
||||
// we know from Scipy that this should be [[0.05048525 0.10097051 0.20194102 0.40388203]]
|
||||
SimpleMatrix A1 =
|
||||
new SimpleMatrix(
|
||||
n1, n1, true, new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0})
|
||||
.transpose();
|
||||
SimpleMatrix B1 = new SimpleMatrix(n1, m1, true, new double[] {0, 0, 0, 1});
|
||||
SimpleMatrix Q1 =
|
||||
new SimpleMatrix(
|
||||
n1, n1, true, new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
|
||||
SimpleMatrix R1 = new SimpleMatrix(m1, m1, true, new double[] {0.25});
|
||||
assertTrue(solveDAREandVerify(A1, B1, Q1, R1));
|
||||
|
||||
SimpleMatrix A2 = new SimpleMatrix(2, 2, true, new double[] {1, 1, 0, 1});
|
||||
SimpleMatrix B2 = new SimpleMatrix(2, 1, true, new double[] {0, 1});
|
||||
SimpleMatrix Q2 = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 0});
|
||||
SimpleMatrix R2 = new SimpleMatrix(1, 1, true, new double[] {0.3});
|
||||
assertTrue(solveDAREandVerify(A2, B2, Q2, R2));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user