[wpilib] Use RKDP in DifferentialDrivetrainSim (wpilibsuite#5931)

Starlight220 · Dec 1, 2023 · 51f1989 · 51f1989
1 parent 2b19a64
commit 51f1989
Show file tree

Hide file tree

Showing 4 changed files with 6 additions and 7 deletions.
diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
@@ -58,7 +58,7 @@ void DifferentialDrivetrainSim::SetGearing(double newGearing) {
 }
 
 void DifferentialDrivetrainSim::Update(units::second_t dt) {
-  m_x = RK4([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt);
+  m_x = RKDP([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/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::RK4(
+    groundTruthX = frc::RKDP(
         [&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 RK4 anyway
+  // ODE solution using RKDP 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,8 +148,7 @@ public void setInputs(double leftVoltageVolts, double rightVoltageVolts) {
    * @param dtSeconds the time difference
    */
   public void update(double dtSeconds) {
-    // Update state estimate with RK4
-    m_x = NumericalIntegration.rk4(this::getDynamics, m_x, m_u, dtSeconds);
+    m_x = NumericalIntegration.rkdp(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/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.rk4(sim::getDynamics, groundTruthX, voltages, 0.020);
+      groundTruthX = NumericalIntegration.rkdp(sim::getDynamics, groundTruthX, voltages, 0.020);
     }
 
     // 2 inch tolerance is OK since our ground truth is an approximation of the
-    // ODE solution using RK4 anyway
+    // ODE solution using RKDP anyway
     assertEquals(
         groundTruthX.get(DifferentialDrivetrainSim.State.kX.value, 0),
         sim.getState(DifferentialDrivetrainSim.State.kX),