Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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

5
wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
View File

@@ -9,7 +9,7 @@
#include <utility>
#include "frc/RobotController.h"
#include "frc/system/RungeKutta.h"
#include "frc/system/NumericalIntegration.h"
using namespace frc;
using namespace frc::sim;
@@ -56,8 +56,7 @@ void DifferentialDrivetrainSim::SetGearing(double newGearing) {
}
void DifferentialDrivetrainSim::Update(units::second_t dt) {
m_x = RungeKutta([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x,
m_u, dt);
m_x = RK4([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt);
m_y = m_x + frc::MakeWhiteNoiseVector<7>(m_measurementStdDevs);
}

4
wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
View File

@@ -7,7 +7,7 @@
#include <wpi/MathExtras.h>
#include "frc/StateSpaceUtil.h"
#include "frc/system/RungeKutta.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/plant/LinearSystemId.h"
using namespace frc;
@@ -77,7 +77,7 @@ void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
Eigen::Matrix<double, 2, 1> ElevatorSim::UpdateX(
const Eigen::Matrix<double, 2, 1>& currentXhat,
const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
auto updatedXhat = RungeKutta(
auto updatedXhat = RKF45(
[&](const Eigen::Matrix<double, 2, 1>& x,
const Eigen::Matrix<double, 1, 1>& u_)
-> Eigen::Matrix<double, 2, 1> {

4
wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
View File

@@ -9,7 +9,7 @@
#include <units/voltage.h>
#include <wpi/MathExtras.h>
#include "frc/system/RungeKutta.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/plant/LinearSystemId.h"
using namespace frc;
@@ -82,7 +82,7 @@ Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
// We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
// std::cos(theta)]]
auto updatedXhat = RungeKutta(
Eigen::Matrix<double, 2, 1> updatedXhat = RKF45(
[&](const auto& x, const auto& u) -> Eigen::Matrix<double, 2, 1> {
Eigen::Matrix<double, 2, 1> xdot = m_plant.A() * x + m_plant.B() * u;

4
wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
View File

@@ -10,7 +10,7 @@
#include "frc/controller/RamseteController.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/simulation/DifferentialDrivetrainSim.h"
#include "frc/system/RungeKutta.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/system/plant/LinearSystemId.h"
#include "frc/trajectory/TrajectoryGenerator.h"
@@ -57,7 +57,7 @@ TEST(DifferentialDriveSim, Convergence) {
sim.Update(20_ms);
// Update ground truth.
groundTruthX = frc::RungeKutta(
groundTruthX = frc::RK4(
[&sim](const auto& x, const auto& u) -> Eigen::Matrix<double, 7, 1> {
return sim.Dynamics(x, u);
},

26
wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
View File

@@ -13,6 +13,7 @@
#include "frc/controller/PIDController.h"
#include "frc/simulation/ElevatorSim.h"
#include "frc/simulation/EncoderSim.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/system/plant/LinearSystemId.h"
#include "gtest/gtest.h"
@@ -64,3 +65,28 @@ TEST(ElevatorSim, MinMax) {
EXPECT_TRUE(height < 1.05_m);
}
}
TEST(ElevatorSim, Stability) {
static constexpr double kElevatorGearing = 100.0;
static constexpr units::meter_t kElevatorDrumRadius = 0.5_in;
static constexpr units::kilogram_t kCarriageMass = 4.0_kg;
frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4);
frc::LinearSystem<2, 1, 1> system = frc::LinearSystemId::ElevatorSystem(
m_elevatorGearbox, kCarriageMass, kElevatorDrumRadius, kElevatorGearing);
Eigen::Matrix<double, 2, 1> x0 = frc::MakeMatrix<2, 1>(0.0, 0.0);
Eigen::Matrix<double, 1, 1> u0 = frc::MakeMatrix<1, 1>(12.0);
Eigen::Matrix<double, 2, 1> x1 = frc::MakeMatrix<2, 1>(0.0, 0.0);
for (size_t i = 0; i < 50; i++) {
x1 = frc::RKF45(
[&](Eigen::Matrix<double, 2, 1> x,
Eigen::Matrix<double, 1, 1> u) -> Eigen::Matrix<double, 2, 1> {
return system.A() * x + system.B() * u;
},
x1, u0, 0.020_s);
}
EXPECT_NEAR(x1(0), system.CalculateX(x0, u0, 1_s)(0), 0.1);
}

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
View File

@@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpilibj.system.NumericalIntegration;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.util.Units;
@@ -151,7 +151,7 @@ public class DifferentialDrivetrainSim {
public void update(double dtSeconds) {
// Update state estimate with RK4
m_x = RungeKutta.rungeKutta(this::getDynamics, m_x, m_u, dtSeconds);
m_x = NumericalIntegration.rk4(this::getDynamics, m_x, m_u, dtSeconds);
m_y = m_x;
if (m_measurementStdDevs != null) {
m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View File

@@ -5,7 +5,7 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpilibj.system.NumericalIntegration;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpiutil.math.Matrix;
@@ -202,7 +202,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
// Calculate updated x-hat from Runge-Kutta.
var updatedXhat =
RungeKutta.rungeKutta(
NumericalIntegration.rkf45(
(x, u_) ->
(m_plant.getA().times(x))
.plus(m_plant.getB().times(u_))

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
View File

@@ -5,7 +5,7 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpilibj.system.NumericalIntegration;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpiutil.math.Matrix;
@@ -263,7 +263,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
// We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
// cos(theta)]]
Matrix<N2, N1> updatedXhat =
RungeKutta.rungeKutta(
NumericalIntegration.rkf45(
(Matrix<N2, N1> x, Matrix<N1, N1> u_) -> {
Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(u_));
if (m_simulateGravity) {

4
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
View File

@@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj.controller.RamseteController;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpilibj.system.NumericalIntegration;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
@@ -76,7 +76,7 @@ class DifferentialDrivetrainSimTest {
sim.update(0.020);
// Update our ground truth
groundTruthX = RungeKutta.rungeKutta(sim::getDynamics, groundTruthX, voltages, 0.020);
groundTruthX = NumericalIntegration.rk4(sim::getDynamics, groundTruthX, voltages, 0.020);
}
assertEquals(

17
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java
View File

@@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.util.Units;
import edu.wpi.first.wpiutil.math.VecBuilder;
import org.junit.jupiter.api.Test;
@@ -87,4 +88,20 @@ public class ElevatorSimTest {
assertTrue(height <= 1.05);
}
}
@Test
public void testStability() {
var sim = new ElevatorSim(DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10);
sim.setState(VecBuilder.fill(0, 0));
sim.setInput(12);
for (int i = 0; i < 50; i++) {
sim.update(0.02);
}
assertEquals(
sim.m_plant.calculateX(VecBuilder.fill(0, 0), VecBuilder.fill(12), 0.02 * 50.0).get(0, 0),
sim.getPositionMeters(),
0.1);
}
}

4
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java
View File

@@ -7,8 +7,8 @@ package edu.wpi.first.wpilibj.estimator;
import edu.wpi.first.math.Drake;
import edu.wpi.first.wpilibj.math.Discretization;
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
import edu.wpi.first.wpilibj.system.NumericalIntegration;
import edu.wpi.first.wpilibj.system.NumericalJacobian;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.Num;
@@ -220,7 +220,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
final var discA = discPair.getFirst();
final var discQ = discPair.getSecond();
m_xHat = RungeKutta.rungeKutta(f, m_xHat, u, dtSeconds);
m_xHat = NumericalIntegration.rk4(f, m_xHat, u, dtSeconds);
m_P = discA.times(m_P).times(discA.transpose()).plus(discQ);
m_dtSeconds = dtSeconds;
}

4
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java
View File

@@ -6,8 +6,8 @@ package edu.wpi.first.wpilibj.estimator;
import edu.wpi.first.wpilibj.math.Discretization;
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
import edu.wpi.first.wpilibj.system.NumericalIntegration;
import edu.wpi.first.wpilibj.system.NumericalJacobian;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.Num;
@@ -291,7 +291,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
for (int i = 0; i < m_pts.getNumSigmas(); ++i) {
Matrix<States, N1> x = sigmas.extractColumnVector(i);
m_sigmasF.setColumn(i, RungeKutta.rungeKutta(m_f, x, u, dtSeconds));
m_sigmasF.setColumn(i, NumericalIntegration.rk4(m_f, x, u, dtSeconds));
}
var ret =

290
wpimath/src/main/java/edu/wpi/first/wpilibj/system/NumericalIntegration.java Normal file
View File

@@ -0,0 +1,290 @@
// 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.
package edu.wpi.first.wpilibj.system;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.Pair;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N5;
import edu.wpi.first.wpiutil.math.numbers.N6;
import java.util.function.BiFunction;
import java.util.function.DoubleFunction;
import java.util.function.Function;
public final class NumericalIntegration {
private NumericalIntegration() {
// utility Class
}
/**
* Performs Runge Kutta integration (4th order).
*
* @param f The function to integrate, which takes one argument x.
* @param x The initial value of x.
* @param dtSeconds The time over which to integrate.
* @return the integration of dx/dt = f(x) for dt.
*/
@SuppressWarnings("ParameterName")
public static double rk4(DoubleFunction<Double> f, double x, double dtSeconds) {
final var halfDt = 0.5 * dtSeconds;
final var k1 = f.apply(x);
final var k2 = f.apply(x + k1 * halfDt);
final var k3 = f.apply(x + k2 * halfDt);
final var k4 = f.apply(x + k3 * dtSeconds);
return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
* Performs Runge Kutta integration (4th order).
*
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param dtSeconds The time over which to integrate.
* @return The result of Runge Kutta integration (4th order).
*/
@SuppressWarnings("ParameterName")
public static double rk4(
BiFunction<Double, Double, Double> f, double x, Double u, double dtSeconds) {
final var halfDt = 0.5 * dtSeconds;
final var k1 = f.apply(x, u);
final var k2 = f.apply(x + k1 * halfDt, u);
final var k3 = f.apply(x + k2 * halfDt, u);
final var k4 = f.apply(x + k3 * dtSeconds, u);
return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
*
* @param <States> A Num representing the states of the system to integrate.
* @param <Inputs> A Num representing the inputs of the system to integrate.
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param dtSeconds The time over which to integrate.
* @return the integration of dx/dt = f(x, u) for dt.
*/
@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rk4(
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x,
Matrix<Inputs, N1> u,
double dtSeconds) {
final var halfDt = 0.5 * dtSeconds;
Matrix<States, N1> k1 = f.apply(x, u);
Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)), u);
Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)), u);
Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)), u);
return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
}
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
*
* @param <States> A Num prepresenting the states of the system.
* @param f The function to integrate. It must take one argument x.
* @param x The initial value of x.
* @param dtSeconds The time over which to integrate.
* @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
*/
@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public static <States extends Num> Matrix<States, N1> rk4(
Function<Matrix<States, N1>, Matrix<States, N1>> f, Matrix<States, N1> x, double dtSeconds) {
final var halfDt = 0.5 * dtSeconds;
Matrix<States, N1> k1 = f.apply(x);
Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)));
Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)));
Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)));
return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
}
/**
* Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described in
* https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method. By default, the max
* error is 1e-6.
*
* @param <States> A Num representing the states of the system to integrate.
* @param <Inputs> A Num representing the inputs of the system to integrate.
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param dtSeconds The time over which to integrate.
* @return the integration of dx/dt = f(x, u) for dt.
*/
@SuppressWarnings("MethodTypeParameterName")
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x,
Matrix<Inputs, N1> u,
double dtSeconds) {
return rkf45(f, x, u, dtSeconds, 1e-6);
}
/**
* Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described in
* https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
*
* @param <States> A Num representing the states of the system to integrate.
* @param <Inputs> A Num representing the inputs of the system to integrate.
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param dtSeconds The time over which to integrate.
* @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
* @return the integration of dx/dt = f(x, u) for dt.
*/
@SuppressWarnings("MethodTypeParameterName")
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x,
Matrix<Inputs, N1> u,
double dtSeconds,
double maxError) {
double dtElapsed = 0;
double previousH = dtSeconds;
// Loop until we've gotten to our desired dt
while (dtElapsed < dtSeconds) {
// RKF45 will give us an updated x and a dt back.
// We use the new dt (h) as the initial dt for our next loop
var ret = rkf45Impl(f, x, u, previousH, maxError, dtSeconds - dtElapsed);
dtElapsed += ret.getSecond();
previousH = ret.getSecond();
x = ret.getFirst();
}
return x;
}
static final double[] ch = {47 / 450.0, 0, 12 / 25.0, 32 / 225.0, 1 / 30.0, 6 / 25.0};
static final double[] ct = {-1 / 150.0, 0, 3 / 100.0, -16 / 75.0, -1 / 20.0, 6 / 25.0};
static final Matrix<N6, N5> Bs =
Matrix.mat(Nat.N6(), Nat.N5())
.fill(
0,
0,
0,
0,
0,
2 / 9.0,
0,
0,
0,
0,
1 / 12.0,
1 / 4.0,
0,
0,
0,
69 / 128.0,
-243 / 128.0,
135 / 64.0,
0,
0,
-17 / 12.0,
27 / 4.0,
-27 / 5.0,
16 / 15.0,
0,
65 / 432.0,
-5 / 16.0,
13 / 16.0,
4 / 27.0,
5 / 144.0);
/**
* Implements one loop of RKF45. This takes an initial state, dt guess, and max truncation error,
* and returns a new x and the dt over which that x was updated. This should be called until there
* is no dt remaining.
*
* @param <States> Num representing the states of the system to integrate.
* @param <Inputs> A Num representing the inputs of the system to integrate.
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param initialH The initial dt guess. This is refined to clamp truncation error to the
* specified max.
* @param maxTruncationError The max truncation error acceptable. Usually a small number like
* 1e-6.
* @param dtRemaining How much time is left to integrate over. Used to clamp h.
* @return the integration of dx/dt = f(x, u) for dt.
*/
@SuppressWarnings("MethodTypeParameterName")
private static <States extends Num, Inputs extends Num>
Pair<Matrix<States, N1>, Double> rkf45Impl(
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x,
Matrix<Inputs, N1> u,
double initialH,
double maxTruncationError,
double dtRemaining) {
double truncationErr;
double h = initialH;
Matrix<States, N1> newX;
do {
// only allow us to advance up to the dt remaining
h = Math.min(h, dtRemaining);
// Notice how the derivative in the Wikipedia notation is dy/dx.
// That means their y is our x and their x is our t
Matrix<States, N1> k1 = f.apply(x, u).times(h);
Matrix<States, N1> k2 = f.apply(x.plus(k1.times(Bs.get(1, 0))), u).times(h);
Matrix<States, N1> k3 =
f.apply(x.plus(k1.times(Bs.get(2, 0))).plus(k2.times(Bs.get(2, 1))), u).times(h);
Matrix<States, N1> k4 =
f.apply(
x.plus(k1.times(Bs.get(3, 0)))
.plus(k2.times(Bs.get(3, 1)))
.plus(k3.times(Bs.get(3, 2))),
u)
.times(h);
Matrix<States, N1> k5 =
f.apply(
x.plus(k1.times(Bs.get(4, 0)))
.plus(k2.times(Bs.get(4, 1)))
.plus(k3.times(Bs.get(4, 2)))
.plus(k4.times(Bs.get(4, 3))),
u)
.times(h);
Matrix<States, N1> k6 =
f.apply(
x.plus(k1.times(Bs.get(5, 0)))
.plus(k2.times(Bs.get(5, 1)))
.plus(k3.times(Bs.get(5, 2)))
.plus(k4.times(Bs.get(5, 3)))
.plus(k5.times(Bs.get(5, 4))),
u)
.times(h);
newX =
x.plus(k1.times(ch[0]))
.plus(k2.times(ch[1]))
.plus(k3.times(ch[2]))
.plus(k4.times(ch[3]))
.plus(k5.times(ch[4]))
.plus(k6.times(ch[5]));
truncationErr =
k1.times(ct[0])
.plus(k2.times(ct[1]))
.plus(k3.times(ct[2]))
.plus(k4.times(ct[3]))
.plus(k5.times(ct[4]))
.plus(k6.times(ct[5]))
.normF();
h = 0.9 * h * Math.pow(maxTruncationError / truncationErr, 1 / 5.0);
} while (truncationErr > maxTruncationError);
// Return the new x, and the timestep
return Pair.of(newX, h);
}
}

103
wpimath/src/main/java/edu/wpi/first/wpilibj/system/RungeKutta.java
View File

@@ -1,103 +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.
package edu.wpi.first.wpilibj.system;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.numbers.N1;
import java.util.function.BiFunction;
import java.util.function.DoubleFunction;
import java.util.function.Function;
public final class RungeKutta {
private RungeKutta() {
// utility Class
}
/**
* Performs Runge Kutta integration (4th order).
*
* @param f The function to integrate, which takes one argument x.
* @param x The initial value of x.
* @param dtSeconds The time over which to integrate.
* @return the integration of dx/dt = f(x) for dt.
*/
@SuppressWarnings("ParameterName")
public static double rungeKutta(DoubleFunction<Double> f, double x, double dtSeconds) {
final var halfDt = 0.5 * dtSeconds;
final var k1 = f.apply(x);
final var k2 = f.apply(x + k1 * halfDt);
final var k3 = f.apply(x + k2 * halfDt);
final var k4 = f.apply(x + k3 * dtSeconds);
return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
* Performs Runge Kutta integration (4th order).
*
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param dtSeconds The time over which to integrate.
* @return The result of Runge Kutta integration (4th order).
*/
@SuppressWarnings("ParameterName")
public static double rungeKutta(
BiFunction<Double, Double, Double> f, double x, Double u, double dtSeconds) {
final var halfDt = 0.5 * dtSeconds;
final var k1 = f.apply(x, u);
final var k2 = f.apply(x + k1 * halfDt, u);
final var k3 = f.apply(x + k2 * halfDt, u);
final var k4 = f.apply(x + k3 * dtSeconds, u);
return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
*
* @param <States> A Num representing the states of the system to integrate.
* @param <Inputs> A Num representing the inputs of the system to integrate.
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param dtSeconds The time over which to integrate.
* @return the integration of dx/dt = f(x, u) for dt.
*/
@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rungeKutta(
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x,
Matrix<Inputs, N1> u,
double dtSeconds) {
final var halfDt = 0.5 * dtSeconds;
Matrix<States, N1> k1 = f.apply(x, u);
Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)), u);
Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)), u);
Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)), u);
return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
}
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
*
* @param <States> A Num prepresenting the states of the system.
* @param f The function to integrate. It must take one argument x.
* @param x The initial value of x.
* @param dtSeconds The time over which to integrate.
* @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
*/
@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public static <States extends Num> Matrix<States, N1> rungeKutta(
Function<Matrix<States, N1>, Matrix<States, N1>> f, Matrix<States, N1> x, double dtSeconds) {
final var halfDt = 0.5 * dtSeconds;
Matrix<States, N1> k1 = f.apply(x);
Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)));
Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)));
Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)));
return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
}
}

4
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
View File

@@ -12,8 +12,8 @@
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
#include "frc/system/RungeKutta.h"
#include "units/time.h"
namespace frc {
@@ -149,7 +149,7 @@ class ExtendedKalmanFilter {
Eigen::Matrix<double, States, States> discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
m_xHat = RungeKutta(m_f, m_xHat, u, dt);
m_xHat = RK4(m_f, m_xHat, u, dt);
m_P = discA * m_P * discA.transpose() + discQ;
}

4
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
View File

@@ -13,8 +13,8 @@
#include "frc/estimator/MerweScaledSigmaPoints.h"
#include "frc/estimator/UnscentedTransform.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
#include "frc/system/RungeKutta.h"
#include "units/time.h"
namespace frc {
@@ -216,7 +216,7 @@ class UnscentedKalmanFilter {
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
Eigen::Matrix<double, States, 1> x =
sigmas.template block<States, 1>(0, i);
m_sigmasF.template block<States, 1>(0, i) = RungeKutta(m_f, x, u, dt);
m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
}
auto ret = UnscentedTransform<States, States>(

150
wpimath/src/main/native/include/frc/system/NumericalIntegration.h Normal file
View File

@@ -0,0 +1,150 @@
// 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.
#pragma once
#include <frc/StateSpaceUtil.h>
#include <algorithm>
#include "Eigen/Core"
#include "units/time.h"
namespace frc {
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
*
* @param f The function to integrate. It must take one argument x.
* @param x The initial value of x.
* @param dt The time over which to integrate.
*/
template <typename F, typename T>
T RK4(F&& f, T x, units::second_t dt) {
const auto halfDt = 0.5 * dt;
T k1 = f(x);
T k2 = f(x + k1 * halfDt.to<double>());
T k3 = f(x + k2 * halfDt.to<double>());
T k4 = f(x + k3 * dt.to<double>());
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
*
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param dt The time over which to integrate.
*/
template <typename F, typename T, typename U>
T RK4(F&& f, T x, U u, units::second_t dt) {
const auto halfDt = 0.5 * dt;
T k1 = f(x, u);
T k2 = f(x + k1 * halfDt.to<double>(), u);
T k3 = f(x + k2 * halfDt.to<double>(), u);
T k4 = f(x + k3 * dt.to<double>(), u);
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, x) for dt.
*
* @param f The function to integrate. It must take two arguments x and t.
* @param x The initial value of x.
* @param t The initial value of t.
* @param dt The time over which to integrate.
*/
template <typename F, typename T>
T RungeKuttaTimeVarying(F&& f, T x, units::second_t t, units::second_t dt) {
const auto halfDt = 0.5 * dt;
T k1 = f(t, x);
T k2 = f(t + halfDt, x + k1 / 2.0);
T k3 = f(t + halfDt, x + k2 / 2.0);
T k4 = f(t + dt, x + k3);
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
template <typename F, typename T, typename U>
T RKF45Impl(F&& f, T x, U u, double& h, double maxTruncationError,
double dtRemaining) {
double truncationErr;
T newX;
static double ch[] = {47 / 450.0, 0, 12 / 25.0,
32 / 225.0, 1 / 30.0, 6 / 25.0};
static double ct[] = {-1 / 150.0, 0, 3 / 100.0,
-16 / 75.0, -1 / 20.0, 6 / 25.0};
static Eigen::Matrix<double, 6, 5> Bs = frc::MakeMatrix<6, 5>(
0.0, 0.0, 0.0, 0.0, 0.0, 2 / 9.0, 0.0, 0.0, 0.0, 0.0, 1 / 12.0, 1 / 4.0,
0.0, 0.0, 0.0, 69 / 128.0, -243 / 128.0, 135 / 64.0, 0.0, 0.0, -17 / 12.0,
27 / 4.0, -27 / 5.0, 16 / 15.0, 0.0, 65 / 432.0, -5 / 16.0, 13 / 16.0,
4 / 27.0, 5 / 144.0);
do {
// only allow us to advance up to the dt remaining
h = std::min(h, dtRemaining);
// Notice how the derivative in the Wikipedia notation is dy/dx.
// That means their y is our x and their x is our t
T k1 = f(x, u) * h;
T k2 = f(x + (k1 * Bs(1, 0)), u) * h;
T k3 = f(x + (k1 * (Bs(2, 0))) + (k2 * (Bs(2, 1))), u) * h;
T k4 =
f(x + (k1 * (Bs(3, 0))) + (k2 * (Bs(3, 1))) + (k3 * (Bs(3, 2))), u) * h;
T k5 = f(x + (k1 * (Bs(4, 0))) + (k2 * (Bs(4, 1))) + (k3 * (Bs(4, 2))) +
(k4 * (Bs(4, 3))),
u) *
h;
T k6 = f(x + (k1 * (Bs(5, 0))) + (k2 * (Bs(5, 1))) + (k3 * (Bs(5, 2))) +
(k4 * (Bs(5, 3))) + (k5 * (Bs(5, 4))),
u) *
h;
newX = x + (k1 * (ch[0])) + (k2 * (ch[1])) + (k3 * (ch[2])) +
(k4 * (ch[3])) + (k5 * (ch[4])) + (k6 * (ch[5]));
truncationErr = (k1 * (ct[0]) + (k2 * (ct[1])) + (k3 * (ct[2])) +
(k4 * (ct[3])) + (k5 * (ct[4])) + (k6 * (ct[5])))
.norm();
h = 0.9 * h * std::pow(maxTruncationError / truncationErr, 1 / 5.);
} while (truncationErr > maxTruncationError);
// Return the new x. Dt is changed by reference
return newX;
}
/**
* Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described
* in https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
*
* @param f The function to integrate. It must take two arguments x and
* u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param dt The time over which to integrate.
* @param maxError The maximum acceptable truncation error. Usually a small
* number like 1e-6.
*/
template <typename F, typename T, typename U>
T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
double dtElapsed = 0;
double dtSeconds = dt.to<double>();
double previousH = dt.to<double>();
// Loop until we've gotten to our desired dt
while (dtElapsed < dtSeconds) {
// RKF45 will give us an updated x and a dt back.
// We use the new dt (h) as the initial dt for our next loop
// previousH is changed by-reference
T ret = RKF45Impl(f, x, u, previousH, maxError, dtSeconds - dtElapsed);
dtElapsed += previousH;
x = ret;
}
return x;
}
} // namespace frc

66
wpimath/src/main/native/include/frc/system/RungeKutta.h
View File

@@ -1,66 +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.
#pragma once
#include "Eigen/Core"
#include "units/time.h"
namespace frc {
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
*
* @param f The function to integrate. It must take one argument x.
* @param x The initial value of x.
* @param dt The time over which to integrate.
*/
template <typename F, typename T>
T RungeKutta(F&& f, T x, units::second_t dt) {
const auto halfDt = 0.5 * dt;
T k1 = f(x);
T k2 = f(x + k1 * halfDt.to<double>());
T k3 = f(x + k2 * halfDt.to<double>());
T k4 = f(x + k3 * dt.to<double>());
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
*
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param dt The time over which to integrate.
*/
template <typename F, typename T, typename U>
T RungeKutta(F&& f, T x, U u, units::second_t dt) {
const auto halfDt = 0.5 * dt;
T k1 = f(x, u);
T k2 = f(x + k1 * halfDt.to<double>(), u);
T k3 = f(x + k2 * halfDt.to<double>(), u);
T k4 = f(x + k3 * dt.to<double>(), u);
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, x) for dt.
*
* @param f The function to integrate. It must take two arguments x and t.
* @param x The initial value of x.
* @param t The initial value of t.
* @param dt The time over which to integrate.
*/
template <typename F, typename T>
T RungeKuttaTimeVarying(F&& f, T x, units::second_t t, units::second_t dt) {
const auto halfDt = 0.5 * dt;
T k1 = f(t, x);
T k2 = f(t + halfDt, x + k1 / 2.0);
T k3 = f(t + halfDt, x + k2 / 2.0);
T k4 = f(t + dt, x + k3);
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
} // namespace frc

5
wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java
View File

@@ -10,8 +10,8 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
import edu.wpi.first.wpilibj.system.NumericalIntegration;
import edu.wpi.first.wpilibj.system.NumericalJacobian;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
@@ -177,7 +177,8 @@ public class ExtendedKalmanFilterTest {
observer.predict(u, dtSeconds);
groundTruthX =
RungeKutta.rungeKutta(ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dtSeconds);
NumericalIntegration.rk4(
ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dtSeconds);
r = nextR;

4
wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java
View File

@@ -12,8 +12,8 @@ import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.math.Discretization;
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
import edu.wpi.first.wpilibj.system.NumericalIntegration;
import edu.wpi.first.wpilibj.system.NumericalJacobian;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
@@ -217,7 +217,7 @@ public class UnscentedKalmanFilterTest {
r = nextR;
observer.predict(u, dtSeconds);
trueXhat =
RungeKutta.rungeKutta(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds);
NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds);
}
var localY = getLocalMeasurementModel(trueXhat, u);

25
wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java → wpimath/src/test/java/edu/wpi/first/wpilibj/system/NumericalIntegrationTest.java
View File

@@ -12,7 +12,7 @@ import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import org.junit.jupiter.api.Test;
public class RungeKuttaTest {
public class NumericalIntegrationTest {
@Test
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public void testExponential() {
@@ -21,7 +21,7 @@ public class RungeKuttaTest {
//noinspection SuspiciousNameCombination
var y1 =
RungeKutta.rungeKutta(
NumericalIntegration.rk4(
(Matrix<N1, N1> x) -> {
var y = new Matrix<>(Nat.N1(), Nat.N1());
y.set(0, 0, Math.exp(x.get(0, 0)));
@@ -32,4 +32,25 @@ public class RungeKuttaTest {
assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
}
@Test
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public void testExponentialAdaptive() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
//noinspection SuspiciousNameCombination
var y1 =
NumericalIntegration.rkf45(
(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);
}
}

2
wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
View File

@@ -8,7 +8,7 @@
#include "Eigen/Core"
#include "frc/StateSpaceUtil.h"
#include "frc/system/RungeKutta.h"
#include "frc/system/NumericalIntegration.h"
TEST(StateSpaceUtilTest, MakeMatrix) {
// Column vector

4
wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
View File

@@ -12,8 +12,8 @@
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
#include "frc/system/RungeKutta.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "units/moment_of_inertia.h"
@@ -154,7 +154,7 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
observer.Predict(u, dt);
r = nextR;
trueXhat = frc::RungeKutta(Dynamics, trueXhat, u, dt);
trueXhat = frc::RK4(Dynamics, trueXhat, u, dt);
}
auto localY = LocalMeasurementModel(trueXhat, u);

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(