Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11: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() {

96
wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
View File

@@ -114,102 +114,6 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
<< discQIntegrated;
}
// Test that the Taylor series discretization produces nearly identical results.
TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}};
constexpr auto dt = 1_s;
frc::Matrixd<2, 2> discQTaylor;
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discATaylor;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont{contQ,
Eigen::EigenvaluesOnly};
for (int i = 0; i < contQ.rows(); ++i) {
EXPECT_GE(esCont.eigenvalues()[i], 0);
}
// T
// Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<frc::Matrixd<2, 2>(units::second_t,
const frc::Matrixd<2, 2>&)>,
frc::Matrixd<2, 2>>(
[&](units::second_t t, const frc::Matrixd<2, 2>&) {
return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, frc::Matrixd<2, 2>::Zero(), dt);
frc::DiscretizeA<2>(contA, dt, &discA);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-10)
<< "Expected these to be nearly equal:\ndiscQTaylor:\n"
<< discQTaylor << "\ndiscQIntegrated:\n"
<< discQIntegrated;
EXPECT_LT((discA - discATaylor).norm(), 1e-10);
// Discrete Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc{discQTaylor,
Eigen::EigenvaluesOnly};
for (int i = 0; i < discQTaylor.rows(); ++i) {
EXPECT_GE(esDisc.eigenvalues()[i], 0);
}
}
// Test that the Taylor series discretization produces nearly identical results.
TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
frc::Matrixd<2, 2> contA{{0, 1}, {0, -1500}};
frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}};
constexpr auto dt = 5_ms;
frc::Matrixd<2, 2> discQTaylor;
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discATaylor;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont{contQ,
Eigen::EigenvaluesOnly};
for (int i = 0; i < contQ.rows(); ++i) {
EXPECT_GE(esCont.eigenvalues()[i], 0);
}
// T
// Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<frc::Matrixd<2, 2>(units::second_t,
const frc::Matrixd<2, 2>&)>,
frc::Matrixd<2, 2>>(
[&](units::second_t t, const frc::Matrixd<2, 2>&) {
return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, frc::Matrixd<2, 2>::Zero(), dt);
frc::DiscretizeA<2>(contA, dt, &discA);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-3)
<< "Expected these to be nearly equal:\ndiscQTaylor:\n"
<< discQTaylor << "\ndiscQIntegrated:\n"
<< discQIntegrated;
EXPECT_LT((discA - discATaylor).norm(), 1e-10);
// Discrete Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc{discQTaylor,
Eigen::EigenvaluesOnly};
for (int i = 0; i < discQTaylor.rows(); ++i) {
EXPECT_GE(esDisc.eigenvalues()[i], 0);
}
}
// Test that DiscretizeR() works
TEST(DiscretizationTest, DiscretizeR) {
frc::Matrixd<2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};