mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-05 03:21: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(
|
||||
|
||||
@@ -0,0 +1,46 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "frc/estimator/AngleStatistics.h"
|
||||
|
||||
TEST(AngleStatisticsTest, TestMean) {
|
||||
Eigen::Matrix<double, 3, 3> sigmas;
|
||||
sigmas << 1, 1.2, 0, 359 * wpi::math::pi / 180, 3 * wpi::math::pi / 180, 0, 1,
|
||||
2, 0;
|
||||
// Weights need to produce the mean of the sigmas
|
||||
Eigen::Vector3d weights{};
|
||||
weights.fill(1.0 / sigmas.cols());
|
||||
|
||||
EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
|
||||
.isApprox(frc::AngleMean<3, 1>(sigmas, weights, 1), 1e-3));
|
||||
}
|
||||
|
||||
TEST(AngleStatisticsTest, TestResidual) {
|
||||
Eigen::Vector3d a(1, 1 * wpi::math::pi / 180, 2);
|
||||
Eigen::Vector3d b(1, 359 * wpi::math::pi / 180, 1);
|
||||
|
||||
EXPECT_TRUE(frc::AngleResidual<3>(a, b, 1).isApprox(
|
||||
Eigen::Vector3d(0, 2 * wpi::math::pi / 180, 1)));
|
||||
}
|
||||
|
||||
TEST(AngleStatisticsTest, TestAdd) {
|
||||
Eigen::Vector3d a(1, 1 * wpi::math::pi / 180, 2);
|
||||
Eigen::Vector3d b(1, 359 * wpi::math::pi / 180, 1);
|
||||
|
||||
EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d(2, 0, 3)));
|
||||
}
|
||||
|
||||
TEST(AngleStatisticsTest, TestNormalize) {
|
||||
EXPECT_NEAR(frc::NormalizeAngle(-2000 * wpi::math::pi / 180),
|
||||
160 * wpi::math::pi / 180, 1e-6);
|
||||
EXPECT_NEAR(frc::NormalizeAngle(358 * wpi::math::pi / 180),
|
||||
-2 * wpi::math::pi / 180, 1e-6);
|
||||
EXPECT_NEAR(frc::NormalizeAngle(360 * wpi::math::pi / 180), 0, 1e-6);
|
||||
}
|
||||
@@ -0,0 +1,102 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <limits>
|
||||
#include <random>
|
||||
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/estimator/DifferentialDrivePoseEstimator.h"
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
#include "frc/geometry/Rotation2d.h"
|
||||
#include "frc/kinematics/DifferentialDriveKinematics.h"
|
||||
#include "frc/kinematics/DifferentialDriveOdometry.h"
|
||||
#include "frc/trajectory/TrajectoryGenerator.h"
|
||||
#include "gtest/gtest.h"
|
||||
#include "units/angle.h"
|
||||
#include "units/length.h"
|
||||
#include "units/time.h"
|
||||
|
||||
TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) {
|
||||
frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d(),
|
||||
frc::Pose2d(),
|
||||
{0.02, 0.02, 0.01, 0.02, 0.02},
|
||||
{0.01, 0.01, 0.001},
|
||||
{0.1, 0.1, 0.01}};
|
||||
|
||||
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
|
||||
frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
|
||||
frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
|
||||
frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
|
||||
frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
|
||||
frc::TrajectoryConfig(10_mps, 5.0_mps_sq));
|
||||
|
||||
frc::DifferentialDriveKinematics kinematics{1.0_m};
|
||||
frc::DifferentialDriveOdometry odometry{frc::Rotation2d()};
|
||||
|
||||
std::default_random_engine generator;
|
||||
std::normal_distribution<double> distribution(0.0, 1.0);
|
||||
|
||||
units::second_t dt = 0.02_s;
|
||||
units::second_t t = 0.0_s;
|
||||
|
||||
units::meter_t leftDistance = 0_m;
|
||||
units::meter_t rightDistance = 0_m;
|
||||
|
||||
units::second_t kVisionUpdateRate = 0.1_s;
|
||||
frc::Pose2d lastVisionPose;
|
||||
units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
|
||||
|
||||
double maxError = -std::numeric_limits<double>::max();
|
||||
double errorSum = 0;
|
||||
|
||||
while (t <= trajectory.TotalTime()) {
|
||||
auto groundTruthState = trajectory.Sample(t);
|
||||
auto input = kinematics.ToWheelSpeeds(
|
||||
{groundTruthState.velocity, 0_mps,
|
||||
groundTruthState.velocity * groundTruthState.curvature});
|
||||
|
||||
if (lastVisionUpdateTime + kVisionUpdateRate < t) {
|
||||
if (lastVisionPose != frc::Pose2d()) {
|
||||
estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
|
||||
}
|
||||
lastVisionPose =
|
||||
groundTruthState.pose +
|
||||
frc::Transform2d(
|
||||
frc::Translation2d(distribution(generator) * 0.1 * 1_m,
|
||||
distribution(generator) * 0.1 * 1_m),
|
||||
frc::Rotation2d(distribution(generator) * 0.01 * 1_rad));
|
||||
|
||||
lastVisionUpdateTime = t;
|
||||
}
|
||||
|
||||
leftDistance += input.left * distribution(generator) * 0.01 * dt;
|
||||
rightDistance += input.right * distribution(generator) * 0.01 * dt;
|
||||
|
||||
auto xhat = estimator.UpdateWithTime(
|
||||
t,
|
||||
groundTruthState.pose.Rotation() +
|
||||
frc::Rotation2d(units::radian_t(distribution(generator) * 0.001)),
|
||||
input, leftDistance, rightDistance);
|
||||
|
||||
double error = groundTruthState.pose.Translation()
|
||||
.Distance(xhat.Translation())
|
||||
.to<double>();
|
||||
|
||||
if (error > maxError) {
|
||||
maxError = error;
|
||||
}
|
||||
errorSum += error;
|
||||
|
||||
t += dt;
|
||||
}
|
||||
|
||||
EXPECT_NEAR(
|
||||
0.0, errorSum / (trajectory.TotalTime().to<double>() / dt.to<double>()),
|
||||
0.2);
|
||||
EXPECT_NEAR(0.0, maxError, 0.4);
|
||||
}
|
||||
@@ -0,0 +1,93 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <limits>
|
||||
#include <random>
|
||||
|
||||
#include "frc/estimator/MecanumDrivePoseEstimator.h"
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
#include "frc/kinematics/MecanumDriveKinematics.h"
|
||||
#include "frc/kinematics/MecanumDriveOdometry.h"
|
||||
#include "frc/trajectory/TrajectoryGenerator.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
TEST(MecanumDrivePoseEstimatorTest, TestAccuracy) {
|
||||
frc::MecanumDriveKinematics kinematics{
|
||||
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
|
||||
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
|
||||
|
||||
frc::MecanumDrivePoseEstimator estimator{
|
||||
frc::Rotation2d(), frc::Pose2d(), kinematics,
|
||||
{0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}};
|
||||
|
||||
frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d()};
|
||||
|
||||
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
|
||||
frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
|
||||
frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
|
||||
frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
|
||||
frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
|
||||
frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
|
||||
|
||||
std::default_random_engine generator;
|
||||
std::normal_distribution<double> distribution(0.0, 1.0);
|
||||
|
||||
units::second_t dt = 0.02_s;
|
||||
units::second_t t = 0_s;
|
||||
|
||||
units::second_t kVisionUpdateRate = 0.1_s;
|
||||
frc::Pose2d lastVisionPose;
|
||||
units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
|
||||
|
||||
std::vector<frc::Pose2d> visionPoses;
|
||||
|
||||
double maxError = -std::numeric_limits<double>::max();
|
||||
double errorSum = 0;
|
||||
|
||||
while (t < trajectory.TotalTime()) {
|
||||
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
|
||||
|
||||
if (lastVisionUpdateTime + kVisionUpdateRate < t) {
|
||||
if (lastVisionPose != frc::Pose2d()) {
|
||||
estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
|
||||
}
|
||||
lastVisionPose =
|
||||
groundTruthState.pose +
|
||||
frc::Transform2d(
|
||||
frc::Translation2d(distribution(generator) * 0.1_m,
|
||||
distribution(generator) * 0.1_m),
|
||||
frc::Rotation2d(distribution(generator) * 0.1 * 1_rad));
|
||||
visionPoses.push_back(lastVisionPose);
|
||||
lastVisionUpdateTime = t;
|
||||
}
|
||||
|
||||
auto wheelSpeeds = kinematics.ToWheelSpeeds(
|
||||
{groundTruthState.velocity, 0_mps,
|
||||
groundTruthState.velocity * groundTruthState.curvature});
|
||||
|
||||
auto xhat = estimator.UpdateWithTime(
|
||||
t,
|
||||
groundTruthState.pose.Rotation() +
|
||||
frc::Rotation2d(distribution(generator) * 0.05_rad),
|
||||
wheelSpeeds);
|
||||
double error = groundTruthState.pose.Translation()
|
||||
.Distance(xhat.Translation())
|
||||
.to<double>();
|
||||
|
||||
if (error > maxError) {
|
||||
maxError = error;
|
||||
}
|
||||
errorSum += error;
|
||||
|
||||
t += dt;
|
||||
}
|
||||
|
||||
EXPECT_LT(errorSum / (trajectory.TotalTime().to<double>() / dt.to<double>()),
|
||||
0.2);
|
||||
EXPECT_LT(maxError, 0.4);
|
||||
}
|
||||
@@ -0,0 +1,93 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <limits>
|
||||
#include <random>
|
||||
|
||||
#include "frc/estimator/SwerveDrivePoseEstimator.h"
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
#include "frc/kinematics/SwerveDriveKinematics.h"
|
||||
#include "frc/kinematics/SwerveDriveOdometry.h"
|
||||
#include "frc/trajectory/TrajectoryGenerator.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
TEST(SwerveDrivePoseEstimatorTest, TestAccuracy) {
|
||||
frc::SwerveDriveKinematics<4> kinematics{
|
||||
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
|
||||
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
|
||||
|
||||
frc::SwerveDrivePoseEstimator<4> estimator{
|
||||
frc::Rotation2d(), frc::Pose2d(), kinematics,
|
||||
{0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}};
|
||||
|
||||
frc::SwerveDriveOdometry<4> odometry{kinematics, frc::Rotation2d()};
|
||||
|
||||
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
|
||||
frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
|
||||
frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
|
||||
frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
|
||||
frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
|
||||
frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
|
||||
|
||||
std::default_random_engine generator;
|
||||
std::normal_distribution<double> distribution(0.0, 1.0);
|
||||
|
||||
units::second_t dt = 0.02_s;
|
||||
units::second_t t = 0_s;
|
||||
|
||||
units::second_t kVisionUpdateRate = 0.1_s;
|
||||
frc::Pose2d lastVisionPose;
|
||||
units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
|
||||
|
||||
std::vector<frc::Pose2d> visionPoses;
|
||||
|
||||
double maxError = -std::numeric_limits<double>::max();
|
||||
double errorSum = 0;
|
||||
|
||||
while (t < trajectory.TotalTime()) {
|
||||
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
|
||||
|
||||
if (lastVisionUpdateTime + kVisionUpdateRate < t) {
|
||||
if (lastVisionPose != frc::Pose2d()) {
|
||||
estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
|
||||
}
|
||||
lastVisionPose =
|
||||
groundTruthState.pose +
|
||||
frc::Transform2d(
|
||||
frc::Translation2d(distribution(generator) * 0.1_m,
|
||||
distribution(generator) * 0.1_m),
|
||||
frc::Rotation2d(distribution(generator) * 0.1 * 1_rad));
|
||||
visionPoses.push_back(lastVisionPose);
|
||||
lastVisionUpdateTime = t;
|
||||
}
|
||||
|
||||
auto moduleStates = kinematics.ToSwerveModuleStates(
|
||||
{groundTruthState.velocity, 0_mps,
|
||||
groundTruthState.velocity * groundTruthState.curvature});
|
||||
|
||||
auto xhat = estimator.UpdateWithTime(
|
||||
t,
|
||||
groundTruthState.pose.Rotation() +
|
||||
frc::Rotation2d(distribution(generator) * 0.05_rad),
|
||||
moduleStates[0], moduleStates[1], moduleStates[2], moduleStates[3]);
|
||||
double error = groundTruthState.pose.Translation()
|
||||
.Distance(xhat.Translation())
|
||||
.to<double>();
|
||||
|
||||
if (error > maxError) {
|
||||
maxError = error;
|
||||
}
|
||||
errorSum += error;
|
||||
|
||||
t += dt;
|
||||
}
|
||||
|
||||
EXPECT_LT(errorSum / (trajectory.TotalTime().to<double>() / dt.to<double>()),
|
||||
0.2);
|
||||
EXPECT_LT(maxError, 0.4);
|
||||
}
|
||||
@@ -13,6 +13,7 @@
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/QR"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/estimator/AngleStatistics.h"
|
||||
#include "frc/estimator/UnscentedKalmanFilter.h"
|
||||
#include "frc/system/NumericalJacobian.h"
|
||||
#include "frc/system/RungeKutta.h"
|
||||
@@ -94,7 +95,9 @@ TEST(UnscentedKalmanFilterTest, Init) {
|
||||
|
||||
auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
|
||||
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
|
||||
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
|
||||
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
|
||||
frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
|
||||
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
|
||||
}
|
||||
|
||||
TEST(UnscentedKalmanFilterTest, Convergence) {
|
||||
@@ -162,7 +165,11 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
|
||||
|
||||
auto globalY = GlobalMeasurementModel(trueXhat, u);
|
||||
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
|
||||
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
|
||||
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
|
||||
frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
|
||||
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)
|
||||
|
||||
);
|
||||
|
||||
auto finalPosition = trajectory.Sample(trajectory.TotalTime());
|
||||
ASSERT_NEAR(finalPosition.pose.Translation().X().template to<double>(),
|
||||
|
||||
Reference in New Issue
Block a user