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] Clean up NumericalIntegration and add Discretization tests (#3489)

Browse Source 
* Rename Butcher tableau sections in NumericalIntegration such that
  top-left is c, top-right is A, and bottom-right is b
* Move edu.wpi.first.math.Discretization to
  edu.wpi.first.math.system.Discretization
* Sort Java Discretization to match C++ function order
* Add tests for Java Discretization
  * Required adding Runge-Kutta time-varying impl to tests
* Move C++ Runge-Kutta time-varying impl to tests only
  * Users don't need it
This commit is contained in:
Tyler Veness
2021-07-25 07:42:59 -07:00
committed by GitHub
parent bfc209b120
commit 50af74c38f
20 changed files with 641 additions and 310 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -10,6 +10,7 @@
#include "Eigen/Eigenvalues"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/RungeKuttaTimeVarying.h"
// Check that for a simple second-order system that we can easily analyze
// analytically,
@@ -26,8 +27,8 @@ TEST(DiscretizationTest, DiscretizeA) {
// We now have pos = vel = 1 and accel = 0, which should give us:
Eigen::Matrix<double, 2, 1> x1Truth;
x1Truth(1) = x0(1);
x1Truth(0) = x0(0) + 1.0 * x0(1);
x1Truth(0) = 1.0 * x0(0) + 1.0 * x0(1);
x1Truth(1) = 0.0 * x0(0) + 1.0 * x0(1);
EXPECT_EQ(x1Truth, x1Discrete);
}
@@ -53,8 +54,8 @@ TEST(DiscretizationTest, DiscretizeAB) {
// We now have pos = vel = accel = 1, which should give us:
Eigen::Matrix<double, 2, 1> x1Truth;
x1Truth(1) = x0(1) + 1.0 * u(0);
x1Truth(0) = x0(0) + 1.0 * x0(1) + 0.5 * u(0);
x1Truth(0) = 1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0);
x1Truth(1) = 0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0);
EXPECT_EQ(x1Truth, x1Discrete);
}
@@ -79,7 +80,7 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
},
Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discQ;
@@ -100,7 +101,7 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
Eigen::Matrix<double, 2, 2> contQ;
contQ << 0.0025, 0, 0, 1;
constexpr auto dt = 5.05_ms;
constexpr auto dt = 5_ms;
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<Eigen::Matrix<double, 2, 2>(
@@ -111,7 +112,7 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
},
Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discQ;
@@ -128,9 +129,6 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
Eigen::Matrix<double, 2, 2> contA;
contA << 0, 1, 0, 0;
Eigen::Matrix<double, 2, 1> contB;
contB << 0, 1;
Eigen::Matrix<double, 2, 2> contQ;
contQ << 1, 0, 0, 1;
@@ -139,12 +137,11 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
Eigen::Matrix<double, 2, 2> discQTaylor;
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discATaylor;
Eigen::Matrix<double, 2, 1> discB;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
for (int i = 0; i < contQ.rows(); i++) {
EXPECT_GT(esCont.eigenvalues()[i], 0);
for (int i = 0; i < contQ.rows(); ++i) {
EXPECT_GE(esCont.eigenvalues()[i], 0);
}
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
@@ -156,9 +153,9 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
},
Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB);
frc::DiscretizeA<2>(contA, dt, &discA);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-10)
@@ -169,8 +166,8 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
// Discrete Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
for (int i = 0; i < discQTaylor.rows(); i++) {
EXPECT_GT(esDisc.eigenvalues()[i], 0);
for (int i = 0; i < discQTaylor.rows(); ++i) {
EXPECT_GE(esDisc.eigenvalues()[i], 0);
}
}
@@ -179,23 +176,19 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
Eigen::Matrix<double, 2, 2> contA;
contA << 0, 1, 0, -1500;
Eigen::Matrix<double, 2, 1> contB;
contB << 0, 1;
Eigen::Matrix<double, 2, 2> contQ;
contQ << 0.0025, 0, 0, 1;
constexpr auto dt = 5.05_ms;
constexpr auto dt = 5_ms;
Eigen::Matrix<double, 2, 2> discQTaylor;
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discATaylor;
Eigen::Matrix<double, 2, 1> discB;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
for (int i = 0; i < contQ.rows(); i++) {
EXPECT_GT(esCont.eigenvalues()[i], 0);
for (int i = 0; i < contQ.rows(); ++i) {
EXPECT_GE(esCont.eigenvalues()[i], 0);
}
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
@@ -207,9 +200,9 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
},
Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB);
frc::DiscretizeA<2>(contA, dt, &discA);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-3)
@@ -220,8 +213,8 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
// Discrete Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
for (int i = 0; i < discQTaylor.rows(); i++) {
EXPECT_GT(esDisc.eigenvalues()[i], 0);
for (int i = 0; i < discQTaylor.rows(); ++i) {
EXPECT_GE(esDisc.eigenvalues()[i], 0);
}
}

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

@@ -67,32 +67,3 @@ TEST(NumericalIntegrationTest, ExponentialRKDP) {
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>()
<< 12.0 * std::exp(t) / (std::pow(std::exp(t) + 1.0, 2.0)))
.finished();
}
} // namespace
// Tests RungeKutta with a time varying solution.
// Now, lets test RK4 with a time varying solution. From
// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
// x' = x (2 / (e^t + 1) - 1)
//
// The true (analytical) solution is:
//
// x(t) = 12 * e^t / ((e^t + 1)^2)
TEST(NumericalIntegrationTest, RungeKuttaTimeVarying) {
Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
Eigen::Matrix<double, 1, 1> y1 = frc::RungeKuttaTimeVarying(
[](units::second_t t, Eigen::Matrix<double, 1, 1> x) {
return (Eigen::Matrix<double, 1, 1>()
<< x(0) * (2.0 / (std::exp(t.to<double>()) + 1.0) - 1.0))
.finished();
},
y0, 5_s, 1_s);
EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);
}

37
wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp Normal file
View File

@@ -0,0 +1,37 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <gtest/gtest.h>
#include <cmath>
#include "frc/system/RungeKuttaTimeVarying.h"
namespace {
Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
return (Eigen::Matrix<double, 1, 1>()
<< 12.0 * std::exp(t) / (std::pow(std::exp(t) + 1.0, 2.0)))
.finished();
}
} // namespace
// Tests RK4 with a time varying solution. From
// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
// x' = x (2 / (e^t + 1) - 1)
//
// The true (analytical) solution is:
//
// x(t) = 12 * e^t / ((e^t + 1)^2)
TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) {
Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
Eigen::Matrix<double, 1, 1> y1 = frc::RungeKuttaTimeVarying(
[](units::second_t t, Eigen::Matrix<double, 1, 1> x) {
return (Eigen::Matrix<double, 1, 1>()
<< x(0) * (2.0 / (std::exp(t.to<double>()) + 1.0) - 1.0))
.finished();
},
5_s, y0, 1_s);
EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);
}