[wpimath] Fix potential divide-by-zero in RKDP (#5242)

If f(x, u) has no dynamics, the truncation error can be zero.
This commit is contained in:
Tyler Veness
2023-03-26 17:00:09 -07:00
committed by GitHub
parent 9227b2166e
commit 63512bbbb8
6 changed files with 36 additions and 4 deletions

View File

@@ -30,6 +30,20 @@ class NumericalIntegrationTest {
assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
}
@Test
void testZeroRKDP() {
var y1 =
NumericalIntegration.rkdp(
(x, u) -> {
return VecBuilder.fill(0);
},
VecBuilder.fill(0),
VecBuilder.fill(0),
0.1);
assertEquals(0.0, y1.get(0, 0), 1e-3);
}
@Test
void testExponentialRKDP() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);