Implemented Tsitouras 5th order numerical integrator

wpilibsuite · Nov 12, 2024 · e498fc7 · e498fc7
1 parent 6adad7b
commit e498fc7
Show file tree

Hide file tree

Showing 16 changed files with 622 additions and 22 deletions.
diff --git a/sysid/src/main/native/cpp/analysis/ArmSim.cpp b/sysid/src/main/native/cpp/analysis/ArmSim.cpp
@@ -42,7 +42,7 @@ void ArmSim::Update(units::volt_t voltage, units::second_t dt) {
   // small for ill-conditioned data (e.g., high velocities with sharp spikes in
   // acceleration).
   Eigen::Vector<double, 1> u{voltage.value()};
-  m_x = frc::RKDP(f, m_x, u, dt, 0.25);
+  m_x = frc::Tsit5(f, m_x, u, dt, 0.25);
 }
 
 double ArmSim::GetPosition() const {

diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
@@ -59,7 +59,8 @@ void DifferentialDrivetrainSim::SetGearing(double newGearing) {
 }
 
 void DifferentialDrivetrainSim::Update(units::second_t dt) {
-  m_x = RKDP([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt);
+  m_x =
+      Tsit5([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt);
   m_y = m_x + frc::MakeWhiteNoiseVector<7>(m_measurementStdDevs);
 }
 

diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
@@ -104,7 +104,7 @@ void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
 
 Vectord<2> ElevatorSim::UpdateX(const Vectord<2>& currentXhat,
                                 const Vectord<1>& u, units::second_t dt) {
-  auto updatedXhat = RKDP(
+  auto updatedXhat = Tsit5(
       [&](const Vectord<2>& x, const Vectord<1>& u_) -> Vectord<2> {
         Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u;
 

diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
@@ -112,7 +112,7 @@ Vectord<2> SingleJointedArmSim::UpdateX(const Vectord<2>& currentXhat,
   //   f(x, u) = Ax + Bu + [0  α]ᵀ
   //   f(x, u) = Ax + Bu + [0  3/2⋅g⋅cos(θ)/L]ᵀ
 
-  Vectord<2> updatedXhat = RKDP(
+  Vectord<2> updatedXhat = Tsit5(
       [&](const auto& x, const auto& u) -> Vectord<2> {
         Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u;
 

diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
@@ -55,15 +55,15 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
     sim.Update(20_ms);
 
     // Update ground truth.
-    groundTruthX = frc::RKDP(
+    groundTruthX = frc::Tsit5(
         [&sim](const auto& x, const auto& u) -> frc::Vectord<7> {
           return sim.Dynamics(x, u);
         },
         groundTruthX, voltages, 20_ms);
   }
 
   // 2 inch tolerance is OK since our ground truth is an approximation of the
-  // ODE solution using RKDP anyway
+  // ODE solution using Tsit5 anyway
   EXPECT_NEAR(groundTruthX(0, 0), sim.GetPose().X().value(), 0.05);
   EXPECT_NEAR(groundTruthX(1, 0), sim.GetPose().Y().value(), 0.05);
   EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().value(), 0.01);

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
@@ -148,7 +148,7 @@ public void setInputs(double leftVoltageVolts, double rightVoltageVolts) {
    * @param dtSeconds the time difference
    */
   public void update(double dtSeconds) {
-    m_x = NumericalIntegration.rkdp(this::getDynamics, m_x, m_u, dtSeconds);
+    m_x = NumericalIntegration.tsit5(this::getDynamics, m_x, m_u, dtSeconds);
     m_y = m_x;
     if (m_measurementStdDevs != null) {
       m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
@@ -235,7 +235,7 @@ public void setInputVoltage(double volts) {
   protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
     // Calculate updated x-hat from Runge-Kutta.
     var updatedXhat =
-        NumericalIntegration.rkdp(
+        NumericalIntegration.tsit5(
             (x, _u) -> {
               Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
               if (m_simulateGravity) {

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
@@ -244,7 +244,7 @@ protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, d
     //   f(x, u) = Ax + Bu + [0  3/2⋅g⋅cos(θ)/L]ᵀ
 
     Matrix<N2, N1> updatedXhat =
-        NumericalIntegration.rkdp(
+        NumericalIntegration.tsit5(
             (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) {

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
@@ -76,11 +76,11 @@ void testConvergence() {
       sim.update(0.020);
 
       // Update our ground truth
-      groundTruthX = NumericalIntegration.rkdp(sim::getDynamics, groundTruthX, voltages, 0.020);
+      groundTruthX = NumericalIntegration.tsit5(sim::getDynamics, groundTruthX, voltages, 0.020);
     }
 
     // 2 inch tolerance is OK since our ground truth is an approximation of the
-    // ODE solution using RKDP anyway
+    // ODE solution using tsit5 anyway
     assertEquals(
         groundTruthX.get(DifferentialDrivetrainSim.State.kX.value, 0),
         sim.getState(DifferentialDrivetrainSim.State.kX),