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

[wpimath] Add time-varying RKDP (#7362)

Browse Source 
This makes the ground truth for the Taylor series AQ discretization more
accurate.
This commit is contained in:
Tyler Veness
2024-11-07 23:46:52 -08:00
committed by GitHub
parent 01f85abcfe
commit 661bae568f
10 changed files with 369 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -10,7 +10,6 @@
#include "frc/EigenCore.h"
#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,
@@ -62,15 +61,15 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
// 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::Matrixd<2, 2> discQIntegrated =
frc::RKDP<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::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discQ;
@@ -94,15 +93,15 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
// 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::Matrixd<2, 2> discQIntegrated =
frc::RKDP<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::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discQ;

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

@@ -9,7 +9,7 @@
#include "frc/EigenCore.h"
#include "frc/system/NumericalIntegration.h"
// Tests that integrating dx/dt = e^x works.
// Test that integrating dx/dt = eˣ works
TEST(NumericalIntegrationTest, Exponential) {
frc::Vectord<1> y0{0.0};
@@ -19,7 +19,7 @@ TEST(NumericalIntegrationTest, Exponential) {
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 that integrating dx/dt = eˣ works when we provide a u
TEST(NumericalIntegrationTest, ExponentialWithU) {
frc::Vectord<1> y0{0.0};
@@ -31,6 +31,27 @@ TEST(NumericalIntegrationTest, ExponentialWithU) {
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
// Tests RK4 with a time varying solution. From
// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
//
// dx/dt = x (2 / (eᵗ + 1) - 1)
//
// The true (analytical) solution is:
//
// x(t) = 12eᵗ/(eᵗ + 1)²
TEST(NumericalIntegrationTest, RK4TimeVarying) {
frc::Vectord<1> y0{12.0 * std::exp(5.0) / std::pow(std::exp(5.0) + 1.0, 2.0)};
frc::Vectord<1> y1 = frc::RK4(
[](units::second_t t, const frc::Vectord<1>& x) {
return frc::Vectord<1>{x(0) *
(2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
},
5_s, y0, 1_s);
EXPECT_NEAR(y1(0), 12.0 * std::exp(6.0) / std::pow(std::exp(6.0) + 1.0, 2.0),
1e-3);
}
// Tests that integrating dx/dt = 0 works with RKDP
TEST(NumericalIntegrationTest, ZeroRKDP) {
frc::Vectord<1> y1 = frc::RKDP(
@@ -41,7 +62,7 @@ TEST(NumericalIntegrationTest, ZeroRKDP) {
EXPECT_NEAR(y1(0), 0.0, 1e-3);
}
// Tests that integrating dx/dt = e^x works with RKDP
// Tests that integrating dx/dt = eˣ works with RKDP
TEST(NumericalIntegrationTest, ExponentialRKDP) {
frc::Vectord<1> y0{0.0};
@@ -52,3 +73,24 @@ TEST(NumericalIntegrationTest, ExponentialRKDP) {
y0, frc::Vectord<1>{0.0}, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
// Tests RKDP with a time varying solution. From
// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
//
// dx/dt = x(2/(eᵗ + 1) - 1)
//
// The true (analytical) solution is:
//
// x(t) = 12eᵗ/(eᵗ + 1)²
TEST(NumericalIntegrationTest, RKDPTimeVarying) {
frc::Vectord<1> y0{12.0 * std::exp(5.0) / std::pow(std::exp(5.0) + 1.0, 2.0)};
frc::Vectord<1> y1 = frc::RKDP(
[](units::second_t t, const frc::Vectord<1>& x) {
return frc::Vectord<1>{x(0) *
(2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
},
5_s, y0, 1_s, 1e-12);
EXPECT_NEAR(y1(0), 12.0 * std::exp(6.0) / std::pow(std::exp(6.0) + 1.0, 2.0),
1e-3);
}

35
wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
View File

@@ -1,35 +0,0 @@
// 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 <cmath>
#include <gtest/gtest.h>
#include "frc/EigenCore.h"
#include "frc/system/RungeKuttaTimeVarying.h"
namespace {
frc::Vectord<1> RungeKuttaTimeVaryingSolution(double t) {
return frc::Vectord<1>{12.0 * std::exp(t) / std::pow(std::exp(t) + 1.0, 2.0)};
}
} // namespace
// Tests RK4 with a time varying solution. From
// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
// x' = x (2/(eᵗ + 1) - 1)
//
// The true (analytical) solution is:
//
// x(t) = 12eᵗ/((eᵗ + 1)²)
TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) {
frc::Vectord<1> y0 = RungeKuttaTimeVaryingSolution(5.0);
frc::Vectord<1> y1 = frc::RungeKuttaTimeVarying(
[](units::second_t t, const frc::Vectord<1>& x) {
return frc::Vectord<1>{x(0) *
(2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
},
5_s, y0, 1_s);
EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);
}