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] Remove discretizeAQTaylor() (#5562)

Browse Source 
It gives incorrect results. Any replacement should just be an
implementation detail of discretizeAQ().

Closes #5339.
This commit is contained in:
Tyler Veness
2023-08-23 10:47:32 -07:00
committed by GitHub
parent 7c20fa1b18
commit 8f3d6a1d4b
4 changed files with 0 additions and 364 deletions
Download Patch File Download Diff File Expand all files Collapse all files

92
wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
View File

@@ -122,98 +122,6 @@ class DiscretizationTest {
+ discQIntegrated);
}
// Test that the Taylor series discretization produces nearly identical results.
@Test
void testDiscretizeSlowModelAQTaylor() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
final var dt = 1.0;
// Continuous Q should be positive semidefinite
final var esCont = contQ.getStorage().eig();
for (int i = 0; i < contQ.getNumRows(); ++i) {
assertTrue(esCont.getEigenvalue(i).real >= 0);
}
// T
// Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
final var discQIntegrated =
RungeKuttaTimeVarying.rungeKuttaTimeVarying(
(Double t, Matrix<N2, N2> x) ->
contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
0.0,
new Matrix<>(Nat.N2(), Nat.N2()),
dt);
var discA = Discretization.discretizeA(contA, dt);
var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
var discATaylor = discAQPair.getFirst();
var discQTaylor = discAQPair.getSecond();
assertTrue(
discQIntegrated.minus(discQTaylor).normF() < 1e-10,
"Expected these to be nearly equal:\ndiscQTaylor:\n"
+ discQTaylor
+ "\ndiscQIntegrated:\n"
+ discQIntegrated);
assertTrue(discA.minus(discATaylor).normF() < 1e-10);
// Discrete Q should be positive semidefinite
final var esDisc = discQTaylor.getStorage().eig();
for (int i = 0; i < discQTaylor.getNumRows(); ++i) {
assertTrue(esDisc.getEigenvalue(i).real >= 0);
}
}
// Test that the Taylor series discretization produces nearly identical results.
@Test
void testDiscretizeFastModelAQTaylor() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1500);
final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1);
final var dt = 0.005;
// Continuous Q should be positive semidefinite
final var esCont = contQ.getStorage().eig();
for (int i = 0; i < contQ.getNumRows(); ++i) {
assertTrue(esCont.getEigenvalue(i).real >= 0);
}
// T
// Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
final var discQIntegrated =
RungeKuttaTimeVarying.rungeKuttaTimeVarying(
(Double t, Matrix<N2, N2> x) ->
contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
0.0,
new Matrix<>(Nat.N2(), Nat.N2()),
dt);
var discA = Discretization.discretizeA(contA, dt);
var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
var discATaylor = discAQPair.getFirst();
var discQTaylor = discAQPair.getSecond();
assertTrue(
discQIntegrated.minus(discQTaylor).normF() < 1e-3,
"Expected these to be nearly equal:\ndiscQTaylor:\n"
+ discQTaylor
+ "\ndiscQIntegrated:\n"
+ discQIntegrated);
assertTrue(discA.minus(discATaylor).normF() < 1e-10);
// Discrete Q should be positive semidefinite
final var esDisc = discQTaylor.getStorage().eig();
for (int i = 0; i < discQTaylor.getNumRows(); ++i) {
assertTrue(esDisc.getEigenvalue(i).real >= 0);
}
}
// Test that DiscretizeR() works
@Test
void testDiscretizeR() {