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

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;