Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Allow multiple vision measurements from same timestamp (#4917)

Browse Source 
Co-authored-by: Jordan McMichael <jlmcmchl@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2023-01-12 02:04:30 -05:00
committed by GitHub
parent befd12911c
commit f7f19207e0
13 changed files with 409 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
View File

@@ -227,4 +227,55 @@ class DifferentialDrivePoseEstimatorTest {
assertEquals(0.0, maxError, 0.2, "Incorrect max error");
}
}
@Test
void testSimultaneousVisionMeasurements() {
// This tests for multiple vision measurements appled at the same time. The expected behavior
// is that all measurements affect the estimated pose. The alternative result is that only one
// vision measurement affects the outcome. If that were the case, after 1000 measurements, the
// estimated pose would converge to that measurement.
var kinematics = new DifferentialDriveKinematics(1);
var estimator =
new DifferentialDrivePoseEstimator(
kinematics,
new Rotation2d(),
0,
0,
new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
VecBuilder.fill(0.02, 0.02, 0.01),
VecBuilder.fill(0.1, 0.1, 0.1));
estimator.updateWithTime(0, new Rotation2d(), 0, 0);
var visionMeasurements =
new Pose2d[] {
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
new Pose2d(2, 4, Rotation2d.fromRadians(180)),
};
for (int i = 0; i < 1000; i++) {
for (var measurement : visionMeasurements) {
estimator.addVisionMeasurement(measurement, 0);
}
}
for (var measurement : visionMeasurements) {
var errorLog =
"Estimator converged to one vision measurement: "
+ estimator.getEstimatedPosition().toString()
+ " -> "
+ measurement.toString();
var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
var dtheta =
Math.abs(
measurement.getRotation().getDegrees()
- estimator.getEstimatedPosition().getRotation().getDegrees());
assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
}
}
}

55
wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
View File

@@ -236,4 +236,59 @@ class MecanumDrivePoseEstimatorTest {
assertEquals(0.0, maxError, 0.2, "Incorrect max error");
}
}
@Test
void testSimultaneousVisionMeasurements() {
// This tests for multiple vision measurements appled at the same time. The expected behavior
// is that all measurements affect the estimated pose. The alternative result is that only one
// vision measurement affects the outcome. If that were the case, after 1000 measurements, the
// estimated pose would converge to that measurement.
var kinematics =
new MecanumDriveKinematics(
new Translation2d(1, 1), new Translation2d(1, -1),
new Translation2d(-1, -1), new Translation2d(-1, 1));
var wheelPositions = new MecanumDriveWheelPositions();
var estimator =
new MecanumDrivePoseEstimator(
kinematics,
new Rotation2d(),
wheelPositions,
new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.45, 0.45, 0.1));
estimator.updateWithTime(0, new Rotation2d(), wheelPositions);
var visionMeasurements =
new Pose2d[] {
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
new Pose2d(2, 4, Rotation2d.fromRadians(180)),
};
for (int i = 0; i < 1000; i++) {
for (var measurement : visionMeasurements) {
estimator.addVisionMeasurement(measurement, 0);
}
}
for (var measurement : visionMeasurements) {
var errorLog =
"Estimator converged to one vision measurement: "
+ estimator.getEstimatedPosition().toString()
+ " -> "
+ measurement.toString();
var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
var dtheta =
Math.abs(
measurement.getRotation().getDegrees()
- estimator.getEstimatedPosition().getRotation().getDegrees());
assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
}
}
}

60
wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
View File

@@ -263,4 +263,64 @@ class SwerveDrivePoseEstimatorTest {
assertEquals(0.0, maxError, 0.2, "Incorrect max error");
}
}
@Test
void testSimultaneousVisionMeasurements() {
// This tests for multiple vision measurements appled at the same time. The expected behavior
// is that all measurements affect the estimated pose. The alternative result is that only one
// vision measurement affects the outcome. If that were the case, after 1000 measurements, the
// estimated pose would converge to that measurement.
var kinematics =
new SwerveDriveKinematics(
new Translation2d(1, 1),
new Translation2d(1, -1),
new Translation2d(-1, -1),
new Translation2d(-1, 1));
var fl = new SwerveModulePosition();
var fr = new SwerveModulePosition();
var bl = new SwerveModulePosition();
var br = new SwerveModulePosition();
var estimator =
new SwerveDrivePoseEstimator(
kinematics,
new Rotation2d(),
new SwerveModulePosition[] {fl, fr, bl, br},
new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.9, 0.9, 0.9));
estimator.updateWithTime(0, new Rotation2d(), new SwerveModulePosition[] {fl, fr, bl, br});
var visionMeasurements =
new Pose2d[] {
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
new Pose2d(2, 4, Rotation2d.fromRadians(180)),
};
for (int i = 0; i < 1000; i++) {
for (var measurement : visionMeasurements) {
estimator.addVisionMeasurement(measurement, 0);
}
}
for (var measurement : visionMeasurements) {
var errorLog =
"Estimator converged to one vision measurement: "
+ estimator.getEstimatedPosition().toString()
+ " -> "
+ measurement.toString();
var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
var dtheta =
Math.abs(
measurement.getRotation().getDegrees()
- estimator.getEstimatedPosition().getRotation().getDegrees());
assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
}
}
}