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

[wpimath] Add RKF45 integration (#3047)

Browse Source 
This is more stable than Runge-Kutta for systems with large elements in their A or B matrices.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2021-01-06 21:40:25 -08:00
committed by GitHub
parent 278e0f126e
commit 85a0bd43c2
25 changed files with 560 additions and 210 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -9,7 +9,7 @@
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "frc/system/Discretization.h"
#include "frc/system/RungeKutta.h"
#include "frc/system/NumericalIntegration.h"
// Check that for a simple second-order system that we can easily analyze
// analytically,

27
wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp → wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
View File

@@ -6,14 +6,14 @@
#include <cmath>
#include "frc/system/RungeKutta.h"
#include "frc/system/NumericalIntegration.h"
// Tests that integrating dx/dt = e^x works.
TEST(RungeKuttaTest, Exponential) {
TEST(NumericalIntegrationTest, Exponential) {
Eigen::Matrix<double, 1, 1> y0;
y0(0) = 0.0;
Eigen::Matrix<double, 1, 1> y1 = frc::RungeKutta(
Eigen::Matrix<double, 1, 1> y1 = frc::RK4(
[](Eigen::Matrix<double, 1, 1> x) {
Eigen::Matrix<double, 1, 1> y;
y(0) = std::exp(x(0));
@@ -24,11 +24,11 @@ TEST(RungeKuttaTest, Exponential) {
}
// Tests that integrating dx/dt = e^x works when we provide a U.
TEST(RungeKuttaTest, ExponentialWithU) {
TEST(NumericalIntegrationTest, ExponentialWithU) {
Eigen::Matrix<double, 1, 1> y0;
y0(0) = 0.0;
Eigen::Matrix<double, 1, 1> y1 = frc::RungeKutta(
Eigen::Matrix<double, 1, 1> y1 = frc::RK4(
[](Eigen::Matrix<double, 1, 1> x, Eigen::Matrix<double, 1, 1> u) {
Eigen::Matrix<double, 1, 1> y;
y(0) = std::exp(u(0) * x(0));
@@ -38,6 +38,21 @@ TEST(RungeKuttaTest, 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) {
Eigen::Matrix<double, 1, 1> y0;
y0(0) = 0.0;
Eigen::Matrix<double, 1, 1> y1 = frc::RKF45(
[](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>()
@@ -54,7 +69,7 @@ Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
// The true (analytical) solution is:
//
// x(t) = 12 * e^t / ((e^t + 1)^2)
TEST(RungeKuttaTest, RungeKuttaTimeVarying) {
TEST(NumericalIntegrationTest, RungeKuttaTimeVarying) {
Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
Eigen::Matrix<double, 1, 1> y1 = frc::RungeKuttaTimeVarying(