Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Use RKDP in DifferentialDrivetrainSim (#5931)

Browse Source
This commit is contained in:
Tyler Veness
2023-11-17 08:41:18 -08:00
committed by GitHub
parent ead9ae5a69
commit b1fad062f7
4 changed files with 6 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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);
}

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

@@ -55,7 +55,7 @@ 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);
},
@@ -63,7 +63,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
}
// 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);

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

@@ -148,8 +148,7 @@ public class DifferentialDrivetrainSim {
* @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));

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

@@ -76,11 +76,11 @@ class DifferentialDrivetrainSimTest {
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),