Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +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
wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java
View File

@@ -10,8 +10,8 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
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.NumericalIntegration;
import edu.wpi.first.wpilibj.system.NumericalJacobian;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
@@ -177,7 +177,8 @@ public class ExtendedKalmanFilterTest {
observer.predict(u, dtSeconds);
groundTruthX =
RungeKutta.rungeKutta(ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dtSeconds);
NumericalIntegration.rk4(
ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dtSeconds);
r = nextR;

4
wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java
View File

@@ -12,8 +12,8 @@ import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.math.Discretization;
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
import edu.wpi.first.wpilibj.system.NumericalIntegration;
import edu.wpi.first.wpilibj.system.NumericalJacobian;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
@@ -217,7 +217,7 @@ public class UnscentedKalmanFilterTest {
r = nextR;
observer.predict(u, dtSeconds);
trueXhat =
RungeKutta.rungeKutta(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds);
NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds);
}
var localY = getLocalMeasurementModel(trueXhat, u);

25
wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java → wpimath/src/test/java/edu/wpi/first/wpilibj/system/NumericalIntegrationTest.java
View File

@@ -12,7 +12,7 @@ import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import org.junit.jupiter.api.Test;
public class RungeKuttaTest {
public class NumericalIntegrationTest {
@Test
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public void testExponential() {
@@ -21,7 +21,7 @@ public class RungeKuttaTest {
//noinspection SuspiciousNameCombination
var y1 =
RungeKutta.rungeKutta(
NumericalIntegration.rk4(
(Matrix<N1, N1> x) -> {
var y = new Matrix<>(Nat.N1(), Nat.N1());
y.set(0, 0, Math.exp(x.get(0, 0)));
@@ -32,4 +32,25 @@ public class RungeKuttaTest {
assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
}
@Test
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public void testExponentialAdaptive() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
//noinspection SuspiciousNameCombination
var y1 =
NumericalIntegration.rkf45(
(x, u) -> {
var y = new Matrix<>(Nat.N1(), Nat.N1());
y.set(0, 0, Math.exp(x.get(0, 0)));
return y;
},
y0,
VecBuilder.fill(0),
0.1);
assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
}
}