mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-22 01:11:42 +00:00
[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>
This commit is contained in:
Declan Freeman-Gleason
committed by
GitHub
GitHub
parent
3413bfc06a
commit
bc8f338771
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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"
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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"
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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"
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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(
|
||||
|
||||
Reference in New Issue
Block a user