[wpilib] Add pose estimators (#2867)

Pose and state estimators can filter latency-compensated global measurements and fuse them with state-space drivetrain model information to estimate robot position. They are drop-in replacements for the existing odometry classes. Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com> Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com> Co-authored-by: Claudius Tewari <cttewari@gmail.com> Co-authored-by: Matt <matthew.morley.ca@gmail.com>
2026-06-22 01:11:42 +00:00 · 2020-11-28 17:35:35 -05:00
parent 3413bfc06a
commit bc8f338771
58 changed files with 4958 additions and 39 deletions
--- a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/AngleStatisticsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/AngleStatisticsTest.java
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+public class AngleStatisticsTest {
+  @Test
+  public void testMean() {
+    // 3 states, 2 sigmas
+    var sigmas = Matrix.mat(Nat.N3(), Nat.N2()).fill(
+            1, 1.2,
+            Math.toRadians(359), Math.toRadians(3),
+            1, 2
+    );
+    // Weights need to produce the mean of the sigmas
+    var weights = new Matrix<>(Nat.N2(), Nat.N1());
+    weights.fill(1.0 / sigmas.getNumCols());
+
+    assertTrue(AngleStatistics.angleMean(sigmas, weights, 1).isEqual(
+            VecBuilder.fill(1.1, Math.toRadians(1), 1.5), 1e-6));
+  }
+
+  @Test
+  public void testResidual() {
+    var first = VecBuilder.fill(1, Math.toRadians(1), 2);
+    var second = VecBuilder.fill(1, Math.toRadians(359), 1);
+    assertTrue(AngleStatistics.angleResidual(first, second, 1).isEqual(
+            VecBuilder.fill(0, Math.toRadians(2), 1), 1e-6));
+  }
+
+  @Test
+  public void testAdd() {
+    var first = VecBuilder.fill(1, Math.toRadians(1), 2);
+    var second = VecBuilder.fill(1, Math.toRadians(359), 1);
+    assertTrue(AngleStatistics.angleAdd(first, second, 1).isEqual(VecBuilder.fill(2, 0, 3), 1e-6));
+  }
+
+  @Test
+  public void testNormalize() {
+    assertEquals(AngleStatistics.normalizeAngle(Math.toRadians(-2000)), Math.toRadians(160), 1e-6);
+    assertEquals(AngleStatistics.normalizeAngle(Math.toRadians(358)), Math.toRadians(-2), 1e-6);
+    assertEquals(AngleStatistics.normalizeAngle(Math.toRadians(360)), 0, 1e-6);
+  }
+}
--- a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java
@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import java.util.List;
+import java.util.Random;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpiutil.math.MatBuilder;
+import edu.wpi.first.wpiutil.math.Nat;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class DifferentialDrivePoseEstimatorTest {
+  @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength",
+        "PMD.AvoidInstantiatingObjectsInLoops"})
+  @Test
+  public void testAccuracy() {
+    var estimator = new DifferentialDrivePoseEstimator(new Rotation2d(), new Pose2d(),
+            new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02),
+            new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.01, 0.01, 0.001),
+            new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01));
+
+    var traj = TrajectoryGenerator.generateTrajectory(
+            List.of(
+                    new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                    new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+                    new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                    new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+                    new Pose2d(0, 0, Rotation2d.fromDegrees(45))
+            ),
+            new TrajectoryConfig(10, 5));
+
+
+    var kinematics = new DifferentialDriveKinematics(1);
+    var rand = new Random(4915);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    final double visionUpdateRate = 0.1;
+    Pose2d lastVisionPose = null;
+    double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+
+    double distanceLeft = 0.0;
+    double distanceRight = 0.0;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    Trajectory.State groundtruthState;
+    DifferentialDriveWheelSpeeds input;
+    while (t <= traj.getTotalTimeSeconds()) {
+      groundtruthState = traj.sample(t);
+      input = kinematics.toWheelSpeeds(new ChassisSpeeds(
+              groundtruthState.velocityMetersPerSecond, 0.0,
+              // ds/dt * dtheta/ds = dtheta/dt
+              groundtruthState.velocityMetersPerSecond * groundtruthState.curvatureRadPerMeter
+      ));
+
+      if (lastVisionUpdateTime + visionUpdateRate + rand.nextGaussian() * 0.4 < t) {
+        if (lastVisionPose != null) {
+          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+        }
+        var groundPose = groundtruthState.poseMeters;
+        lastVisionPose = new Pose2d(
+                new Translation2d(
+                        groundPose.getTranslation().getX() + rand.nextGaussian() * 0.1,
+                        groundPose.getTranslation().getY() + rand.nextGaussian() * 0.1
+                ),
+                new Rotation2d(rand.nextGaussian() * 0.01).plus(groundPose.getRotation())
+        );
+        lastVisionUpdateTime = t;
+      }
+
+      input.leftMetersPerSecond += rand.nextGaussian() * 0.01;
+      input.rightMetersPerSecond += rand.nextGaussian() * 0.01;
+
+      distanceLeft += input.leftMetersPerSecond * dt;
+      distanceRight += input.rightMetersPerSecond * dt;
+
+      var rotNoise = new Rotation2d(rand.nextGaussian() * 0.001);
+      var xHat = estimator.updateWithTime(
+              t,
+              groundtruthState.poseMeters.getRotation().plus(rotNoise),
+              input,
+              distanceLeft, distanceRight
+      );
+
+      double error =
+              groundtruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+            0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.035,
+            "Incorrect mean error"
+    );
+    assertEquals(
+            0.0, maxError, 0.055,
+            "Incorrect max error"
+    );
+  }
+}
--- a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimatorTest.java
@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import java.util.List;
+import java.util.Random;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class MecanumDrivePoseEstimatorTest {
+  @Test
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops",
+          "PMD.ExcessiveMethodLength"})
+  public void testAccuracy() {
+    var kinematics = new MecanumDriveKinematics(
+            new Translation2d(1, 1), new Translation2d(1, -1),
+            new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+    var estimator = new MecanumDrivePoseEstimator(
+            new Rotation2d(), new Pose2d(), kinematics,
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.05),
+            VecBuilder.fill(0.1, 0.1, 0.1)
+    );
+
+    var trajectory = TrajectoryGenerator.generateTrajectory(
+            List.of(new Pose2d(),
+                    new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
+                    new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
+                    new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
+                    new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
+                    new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
+            new TrajectoryConfig(0.5, 2)
+    );
+
+    var rand = new Random(5190);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    final double visionUpdateRate = 0.1;
+    Pose2d lastVisionPose = null;
+    double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
+
+      if (lastVisionUpdateTime + visionUpdateRate < t) {
+        if (lastVisionPose != null) {
+          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+        }
+
+        lastVisionPose = new Pose2d(
+                new Translation2d(
+                        groundTruthState.poseMeters.getTranslation().getX()
+                                + rand.nextGaussian() * 0.1,
+                        groundTruthState.poseMeters.getTranslation().getY()
+                                + rand.nextGaussian() * 0.1
+                ),
+                new Rotation2d(
+                        rand.nextGaussian() * 0.1).plus(groundTruthState.poseMeters.getRotation())
+        );
+
+        lastVisionUpdateTime = t;
+      }
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(new ChassisSpeeds(
+              groundTruthState.velocityMetersPerSecond, 0,
+              groundTruthState.velocityMetersPerSecond * groundTruthState.curvatureRadPerMeter));
+
+      wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
+
+      var xHat = estimator.updateWithTime(t,
+              groundTruthState.poseMeters.getRotation()
+                      .plus(new Rotation2d(rand.nextGaussian() * 0.05)), wheelSpeeds);
+
+      double error =
+              groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+            0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25,
+            "Incorrect mean error"
+    );
+    assertEquals(
+            0.0, maxError, 0.42,
+            "Incorrect max error"
+    );
+  }
+}
--- a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java
@@ -0,0 +1,125 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import java.util.List;
+import java.util.Random;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class SwerveDrivePoseEstimatorTest {
+  @Test
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops",
+        "PMD.ExcessiveMethodLength"})
+  public void testAccuracy() {
+    var kinematics = new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1)
+    );
+    var estimator = new SwerveDrivePoseEstimator(
+            new Rotation2d(), new Pose2d(), kinematics,
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.005),
+            VecBuilder.fill(0.1, 0.1, 0.1)
+    );
+
+    var trajectory = TrajectoryGenerator.generateTrajectory(
+            List.of(
+                    new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                    new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+                    new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                    new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+                    new Pose2d(0, 0, Rotation2d.fromDegrees(45))
+            ),
+            new TrajectoryConfig(0.5, 2)
+    );
+
+    var rand = new Random(4915);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    final double visionUpdateRate = 0.1;
+    Pose2d lastVisionPose = null;
+    double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
+
+      if (lastVisionUpdateTime + visionUpdateRate < t) {
+        if (lastVisionPose != null) {
+          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+        }
+
+        lastVisionPose = new Pose2d(
+                new Translation2d(
+                        groundTruthState.poseMeters.getTranslation().getX()
+                                + rand.nextGaussian() * 0.1,
+                        groundTruthState.poseMeters.getTranslation().getY()
+                                + rand.nextGaussian() * 0.1
+                ),
+                new Rotation2d(
+                        rand.nextGaussian() * 0.1).plus(groundTruthState.poseMeters.getRotation())
+        );
+
+        lastVisionUpdateTime = t;
+
+      }
+
+      var moduleStates = kinematics.toSwerveModuleStates(new ChassisSpeeds(
+              groundTruthState.velocityMetersPerSecond,
+              0.0,
+              groundTruthState.velocityMetersPerSecond * groundTruthState.curvatureRadPerMeter)
+      );
+      for (var moduleState : moduleStates) {
+        moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
+        moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1;
+      }
+
+      var xHat = estimator.updateWithTime(
+              t,
+              groundTruthState.poseMeters.getRotation()
+                      .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
+              moduleStates);
+
+      double error =
+              groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+            0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25,
+            "Incorrect mean error"
+    );
+    assertEquals(
+            0.0, maxError, 0.42,
+            "Incorrect max error"
+    );
+  }
+}
--- a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java
@@ -222,7 +222,9 @@ public class UnscentedKalmanFilterTest {
    var R = StateSpaceUtil.makeCostMatrix(
          VecBuilder.fill(0.01, 0.01, 0.0001, 0.0001, 0.5, 0.5));
    observer.correct(Nat.N6(), u, globalY,
-          UnscentedKalmanFilterTest::getGlobalMeasurementModel, R);
+        UnscentedKalmanFilterTest::getGlobalMeasurementModel, R,
+        (sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)),
+        Matrix::minus, Matrix::minus, Matrix::plus);

    final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());

@@ -371,7 +373,7 @@ public class UnscentedKalmanFilterTest {
              16.66666667,
              16.66666667,
              16.66666667
-          )
+          ), (sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)), Matrix::minus
    );

    assertTrue(