Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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);
}