Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +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

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

@@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpilibj.system.NumericalIntegration;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.util.Units;
@@ -151,7 +151,7 @@ public class DifferentialDrivetrainSim {
public void update(double dtSeconds) {
// Update state estimate with RK4
m_x = RungeKutta.rungeKutta(this::getDynamics, m_x, m_u, dtSeconds);
m_x = NumericalIntegration.rk4(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/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View File

@@ -5,7 +5,7 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpilibj.system.NumericalIntegration;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpiutil.math.Matrix;
@@ -202,7 +202,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
// Calculate updated x-hat from Runge-Kutta.
var updatedXhat =
RungeKutta.rungeKutta(
NumericalIntegration.rkf45(
(x, u_) ->
(m_plant.getA().times(x))
.plus(m_plant.getB().times(u_))

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
View File

@@ -5,7 +5,7 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpilibj.system.NumericalIntegration;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpiutil.math.Matrix;
@@ -263,7 +263,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
// We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
// cos(theta)]]
Matrix<N2, N1> updatedXhat =
RungeKutta.rungeKutta(
NumericalIntegration.rkf45(
(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) {