Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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

116
wpimath/src/test/java/edu/wpi/first/math/DARETest.java
View File

@@ -5,6 +5,7 @@
package edu.wpi.first.math;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertThrows;
import edu.wpi.first.wpilibj.UtilityClassTest;
import org.ejml.simple.SimpleMatrix;
@@ -187,4 +188,119 @@ class DARETest extends UtilityClassTest<DARE> {
assertMatrixEqual(X, X.transpose());
assertDARESolution(A, B, Q, R, N, X);
}
@Test
void testMoreInputsThanStates_ABQR() {
var A = SimpleMatrix.identity(2);
var B = new SimpleMatrix(2, 3, true, new double[] {1, 0, 0, 0, 0.5, 0.3});
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.identity(3);
var X = DARE.dare(A, B, Q, R);
assertMatrixEqual(X, X.transpose());
assertDARESolution(A, B, Q, R, X);
}
@Test
void testMoreInputsThanStates_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = new SimpleMatrix(2, 3, true, new double[] {1, 0, 0, 0, 0.5, 0.3});
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.identity(3);
var N = new SimpleMatrix(2, 3, true, new double[] {1, 0, 0, 0, 1, 0});
var X = DARE.dare(A, B, Q, R, N);
assertMatrixEqual(X, X.transpose());
assertDARESolution(A, B, Q, R, N, X);
}
@Test
void testQNotSymmetricPositiveSemidefinite_ABQR() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = SimpleMatrix.diag(-1.0, -1.0);
var R = SimpleMatrix.identity(2);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
}
@Test
void testQNotSymmetricPositiveSemidefinite_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.diag(-1.0, -1.0);
var N = SimpleMatrix.diag(2.0, 2.0);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
}
@Test
void testRNotSymmetricPositiveDefinite_ABQR() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = SimpleMatrix.identity(2);
var R1 = new SimpleMatrix(2, 2);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R1));
var R2 = SimpleMatrix.diag(-1.0, -1.0);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R2));
}
@Test
void testRNotSymmetricPositiveDefinite_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = SimpleMatrix.identity(2);
var N = SimpleMatrix.identity(2);
var R1 = new SimpleMatrix(2, 2);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R1, N));
var R2 = SimpleMatrix.diag(-1.0, -1.0);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R2, N));
}
@Test
void testABNotStabilizable_ABQR() {
var A = SimpleMatrix.identity(2);
var B = new SimpleMatrix(2, 2);
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.identity(2);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
}
@Test
void testABNotStabilizable_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = new SimpleMatrix(2, 2);
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.identity(2);
var N = SimpleMatrix.identity(2);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
}
@Test
void testACNotDetectable_ABQR() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = new SimpleMatrix(2, 2);
var R = SimpleMatrix.identity(2);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
}
@Test
void testACNotDetectable_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = new SimpleMatrix(2, 2);
var R = SimpleMatrix.identity(2);
var N = new SimpleMatrix(2, 2);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
}
}