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

[wpimath] Implement Dormand-Prince integration method (#3476)

Browse Source 
Also refactored RKF45 implementation to match the new style, which is
easier to read.

The tests were switched from RKF45 to RKDP since it's more accurate.
This commit is contained in:
Tyler Veness
2021-07-11 10:42:33 -04:00
committed by GitHub
parent 9c2723391b
commit 1daadb812f
9 changed files with 392 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
View File

@@ -34,7 +34,7 @@ public class NumericalIntegrationTest {
@Test
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public void testExponentialAdaptive() {
public void testExponentialRKF45() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
//noinspection SuspiciousNameCombination
@@ -51,4 +51,24 @@ public class NumericalIntegrationTest {
assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
}
@Test
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public void testExponentialRKDP() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
//noinspection SuspiciousNameCombination
var y1 =
NumericalIntegration.rkdp(
(x, u) -> {
var y = new Matrix<>(Nat.N1(), Nat.N1());
y.set(0, 0, Math.exp(x.get(0, 0)));
return y;
},
y0,
VecBuilder.fill(0),
0.1);
assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
}
}

19
wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
View File

@@ -38,8 +38,8 @@ TEST(NumericalIntegrationTest, ExponentialWithU) {
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
// Tests that integrating dx/dt = e^x works when we provide a U.
TEST(NumericalIntegrationTest, ExponentialWithUAdaptive) {
// Tests that integrating dx/dt = e^x works with RKF45.
TEST(NumericalIntegrationTest, ExponentialRKF45) {
Eigen::Matrix<double, 1, 1> y0;
y0(0) = 0.0;
@@ -53,6 +53,21 @@ TEST(NumericalIntegrationTest, ExponentialWithUAdaptive) {
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
// Tests that integrating dx/dt = e^x works with RKDP
TEST(NumericalIntegrationTest, ExponentialRKDP) {
Eigen::Matrix<double, 1, 1> y0;
y0(0) = 0.0;
Eigen::Matrix<double, 1, 1> y1 = frc::RKDP(
[](Eigen::Matrix<double, 1, 1> x, Eigen::Matrix<double, 1, 1> u) {
Eigen::Matrix<double, 1, 1> y;
y(0) = std::exp(x(0));
return y;
},
y0, (Eigen::Matrix<double, 1, 1>() << 0.0).finished(), 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
namespace {
Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
return (Eigen::Matrix<double, 1, 1>()