Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix DARE Q decomposition (#5611)

Browse Source
This commit is contained in:
Tyler Veness
2023-09-05 10:48:43 -07:00
committed by GitHub
parent 9b3f7fb548
commit 1a6df6fec6
5 changed files with 49 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,6 +4,7 @@
package edu.wpi.first.math;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertThrows;
@@ -312,4 +313,23 @@ class DARETest extends UtilityClassTest<DARE> {
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
}
@Test
void testQDecomposition() {
// Ensures the decomposition of Q into CᵀC is correct
var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1.0, 0.0, 0.0, 0.0});
var B = Matrix.eye(Nat.N2());
var R = Matrix.eye(Nat.N2());
// (A, C₁) should be detectable pair
var C_1 = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.0, 0.0, 1.0, 0.0});
var Q_1 = C_1.transpose().times(C_1);
assertDoesNotThrow(() -> DARE.dare(A, B, Q_1, R));
// (A, C₂) shouldn't be detectable pair
var C_2 = C_1.transpose();
var Q_2 = C_2.transpose().times(C_2);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q_2, R));
}
}

4
wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
View File

@@ -148,10 +148,10 @@ class LinearQuadraticRegulatorTest {
assertEquals(0.51182128351092726, K.get(0, 1), 1e-10);
// QRN overload
var Aref = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / (Ka * 2.0));
var Aref = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / (Ka * 5.0));
var Kimf = getImplicitModelFollowingK(A, B, Q, R, Aref, 0.005);
assertEquals(0.0, Kimf.get(0, 0), 1e-10);
assertEquals(-5.367540084534802e-05, Kimf.get(0, 1), 1e-10);
assertEquals(-6.9190500116751458e-05, Kimf.get(0, 1), 1e-10);
}
@Test