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] Fix pose estimator performance (#4111)

Browse Source 
Fixes #4087.
This commit is contained in:
Tyler Veness
2022-04-26 18:43:59 -07:00
committed by GitHub
parent 51bc893bc5
commit d926dd1610
12 changed files with 159 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java
View File

@@ -16,15 +16,15 @@ class TimeInterpolatableBufferTest {
TimeInterpolatableBuffer<Rotation2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
buffer.addSample(0, new Rotation2d());
assertEquals(0, buffer.getSample(0).getRadians(), 0.001);
assertEquals(0, buffer.getSample(0).get().getRadians(), 0.001);
buffer.addSample(1, new Rotation2d(1));
assertEquals(0.5, buffer.getSample(0.5).getRadians(), 0.001);
assertEquals(1.0, buffer.getSample(1.0).getRadians(), 0.001);
assertEquals(0.5, buffer.getSample(0.5).get().getRadians(), 0.001);
assertEquals(1.0, buffer.getSample(1.0).get().getRadians(), 0.001);
buffer.addSample(3, new Rotation2d(2));
assertEquals(1.5, buffer.getSample(2).getRadians(), 0.001);
assertEquals(1.5, buffer.getSample(2).get().getRadians(), 0.001);
buffer.addSample(10.5, new Rotation2d(2));
assertEquals(new Rotation2d(1), buffer.getSample(0));
assertEquals(new Rotation2d(1), buffer.getSample(0).get());
}
@Test
@@ -34,7 +34,7 @@ class TimeInterpolatableBufferTest {
// We expect to be at (1 - 1/Math.sqrt(2), 1/Math.sqrt(2), 45deg) at t=0.5
buffer.addSample(0, new Pose2d(0, 0, Rotation2d.fromDegrees(90)));
buffer.addSample(1, new Pose2d(1, 1, Rotation2d.fromDegrees(0)));
Pose2d sample = buffer.getSample(0.5);
Pose2d sample = buffer.getSample(0.5).get();
assertEquals(1 - 1 / Math.sqrt(2), sample.getTranslation().getX(), 0.01);
assertEquals(1 / Math.sqrt(2), sample.getTranslation().getY(), 0.01);