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

SCRIPT: wpiformat

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:01:58 -05:00
committed by Peter Johnson
parent ae6bdc9d25
commit 2109161534
749 changed files with 5504 additions and 3936 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
View File

@@ -18,8 +18,9 @@ TEST(AngleStatisticsTest, Mean) {
Eigen::Vector3d weights;
weights.fill(1.0 / sigmas.cols());
EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
.isApprox(wpi::math::AngleMean<3, 3>(sigmas, weights, 1), 1e-3));
EXPECT_TRUE(
Eigen::Vector3d(0.7333333, 0.01163323, 1)
.isApprox(wpi::math::AngleMean<3, 3>(sigmas, weights, 1), 1e-3));
}
TEST(AngleStatisticsTest, Mean_DynamicSize) {
@@ -31,10 +32,11 @@ TEST(AngleStatisticsTest, Mean_DynamicSize) {
Eigen::VectorXd weights{3};
weights.fill(1.0 / sigmas.cols());
EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
.isApprox(wpi::math::AngleMean<Eigen::Dynamic, Eigen::Dynamic>(
sigmas, weights, 1),
1e-3));
EXPECT_TRUE(
Eigen::Vector3d(0.7333333, 0.01163323, 1)
.isApprox(wpi::math::AngleMean<Eigen::Dynamic, Eigen::Dynamic>(
sigmas, weights, 1),
1e-3));
}
TEST(AngleStatisticsTest, Residual) {
@@ -57,7 +59,8 @@ TEST(AngleStatisticsTest, Add) {
Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
EXPECT_TRUE(wpi::math::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
EXPECT_TRUE(
wpi::math::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
}
TEST(AngleStatisticsTest, Add_DynamicSize) {

200
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp
View File

@@ -45,7 +45,8 @@ void testFollowTrajectory(
wpi::units::second_t t = 0_s;
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
std::vector<
std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
@@ -66,9 +67,10 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Transform2d{
wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -92,9 +94,10 @@ void testFollowTrajectory(
auto xhat = estimator.UpdateWithTime(
t,
wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation()},
wpi::math::Rotation3d{
groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation()},
leftDistance, rightDistance);
if (debug) {
@@ -119,7 +122,8 @@ void testFollowTrajectory(
}
if (debug) {
wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::print(
"apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
@@ -127,9 +131,9 @@ void testFollowTrajectory(
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
}
}
@@ -155,49 +159,55 @@ void testFollowTrajectory(
TEST(DifferentialDrivePoseEstimator3dTest, Accuracy) {
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
wpi::math::DifferentialDrivePoseEstimator3d estimator{kinematics,
wpi::math::Rotation3d{},
0_m,
0_m,
wpi::math::Pose3d{},
{0.02, 0.02, 0.02, 0.01},
{0.1, 0.1, 0.1, 0.1}};
wpi::math::DifferentialDrivePoseEstimator3d estimator{
kinematics,
wpi::math::Rotation3d{},
0_m,
0_m,
wpi::math::Pose3d{},
{0.02, 0.02, 0.02, 0.01},
{0.1, 0.1, 0.1, 0.1}};
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
wpi::math::Trajectory trajectory =
wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, true, false);
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
20_ms, 100_ms, 250_ms, true, false);
}
TEST(DifferentialDrivePoseEstimator3dTest, BadInitialPose) {
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
wpi::math::DifferentialDrivePoseEstimator3d estimator{kinematics,
wpi::math::Rotation3d{},
0_m,
0_m,
wpi::math::Pose3d{},
{0.02, 0.02, 0.02, 0.01},
{0.1, 0.1, 0.1, 0.1}};
wpi::math::DifferentialDrivePoseEstimator3d estimator{
kinematics,
wpi::math::Rotation3d{},
0_m,
0_m,
wpi::math::Pose3d{},
{0.02, 0.02, 0.02, 0.01},
{0.1, 0.1, 0.1, 0.1}};
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
wpi::math::Trajectory trajectory =
wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
for (wpi::units::degree_t offset_direction_degs = 0_deg;
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
@@ -206,21 +216,21 @@ TEST(DifferentialDrivePoseEstimator3dTest, BadInitialPose) {
auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
auto initial_pose = trajectory.InitialPose() +
wpi::math::Transform2d{
wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, false, false);
}
}
}
@@ -238,7 +248,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
wpi::math::Rotation3d{},
0_m,
0_m,
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
wpi::math::Pose3d{1_m, 2_m, 0_m,
wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
{0.02, 0.02, 0.02, 0.01},
{0.1, 0.1, 0.1, 0.1}};
@@ -246,11 +257,16 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
wpi::math::Pose3d{0_m, 0_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
wpi::math::Pose3d{0_m, 0_m, 0_m,
wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}},
0_s);
estimator.AddVisionMeasurement(
wpi::math::Pose3d{3_m, 1_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
wpi::math::Pose3d{3_m, 1_m, 0_m,
wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}},
0_s);
estimator.AddVisionMeasurement(
wpi::math::Pose3d{2_m, 4_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
wpi::math::Pose3d{2_m, 4_m, 0_m,
wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
0_s);
}
@@ -289,8 +305,13 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
wpi::math::DifferentialDriveKinematics kinematics{1_m};
wpi::math::DifferentialDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, 0_m, 0_m, wpi::math::Pose3d{},
{0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
kinematics,
wpi::math::Rotation3d{},
0_m,
0_m,
wpi::math::Pose3d{},
{0.1, 0.1, 0.1, 0.1},
{0.45, 0.45, 0.45, 0.45}};
// Add enough measurements to fill up the buffer
for (auto time = 0_s; time < 4_s; time += 20_ms) {
@@ -301,8 +322,9 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
wpi::math::Pose3d{10_m, 10_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
{0.1, 0.1, 0.1, 0.1});
wpi::math::Pose3d{10_m, 10_m, 0_m,
wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}},
1_s, {0.1, 0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
estimator.GetEstimatedPosition().X().value(), 1e-6);
@@ -320,9 +342,13 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
wpi::math::DifferentialDriveKinematics kinematics{1_m};
wpi::math::DifferentialDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, 0_m, 0_m, wpi::math::Pose3d{},
{1.0, 1.0, 1.0, 1.0}, {1.0, 1.0, 1.0, 1.0}};
wpi::math::DifferentialDrivePoseEstimator3d estimator{kinematics,
wpi::math::Rotation3d{},
0_m,
0_m,
wpi::math::Pose3d{},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
// Returns empty when null
EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
@@ -331,35 +357,45 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
// Add a tiny tolerance for the upper bound because of floating point rounding
// error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation3d{},
wpi::units::meter_t{time}, wpi::units::meter_t{time});
estimator.UpdateWithTime(wpi::units::second_t{time},
wpi::math::Rotation3d{}, wpi::units::meter_t{time},
wpi::units::meter_t{time});
}
// Sample at an added time
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(
std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Sampling after the newest value returns the newest value
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
EXPECT_EQ(
std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(
wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
wpi::math::Pose3d{2_m, 0_m, 0_m,
wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(
std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
@@ -367,13 +403,17 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
wpi::math::Pose3d{1_m, 0.2_m, 0_m, wpi::math::Rotation3d{}}, 0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
}
@@ -384,7 +424,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
wpi::math::Rotation3d{},
0_m,
0_m,
wpi::math::Pose3d{-1_m, -1_m, -1_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
wpi::math::Pose3d{-1_m, -1_m, -1_m,
wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
@@ -397,8 +438,9 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset position
estimator.ResetPosition(wpi::math::Rotation3d{}, 1_m, 1_m,
wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
estimator.ResetPosition(
wpi::math::Rotation3d{}, 1_m, 1_m,
wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());

145
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
View File

@@ -46,7 +46,8 @@ void testFollowTrajectory(
wpi::units::second_t t = 0_s;
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
std::vector<
std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
@@ -67,9 +68,10 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Transform2d{
wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -119,7 +121,8 @@ void testFollowTrajectory(
}
if (debug) {
wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::print(
"apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
@@ -127,9 +130,9 @@ void testFollowTrajectory(
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
}
}
@@ -151,41 +154,53 @@ void testFollowTrajectory(
TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
wpi::math::DifferentialDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, 0_m, 0_m, wpi::math::Pose2d{},
{0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};
wpi::math::DifferentialDrivePoseEstimator estimator{kinematics,
wpi::math::Rotation2d{},
0_m,
0_m,
wpi::math::Pose2d{},
{0.02, 0.02, 0.01},
{0.1, 0.1, 0.1}};
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
wpi::math::Trajectory trajectory =
wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, true, false);
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
20_ms, 100_ms, 250_ms, true, false);
}
TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
wpi::math::DifferentialDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, 0_m, 0_m, wpi::math::Pose2d{},
{0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};
wpi::math::DifferentialDrivePoseEstimator estimator{kinematics,
wpi::math::Rotation2d{},
0_m,
0_m,
wpi::math::Pose2d{},
{0.02, 0.02, 0.01},
{0.1, 0.1, 0.1}};
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
wpi::math::Trajectory trajectory =
wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
for (wpi::units::degree_t offset_direction_degs = 0_deg;
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
@@ -194,21 +209,21 @@ TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
auto initial_pose = trajectory.InitialPose() +
wpi::math::Transform2d{
wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, false, false);
}
}
}
@@ -285,7 +300,8 @@ TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m}, wpi::math::Rotation2d{0.1_rad}},
wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m},
wpi::math::Rotation2d{0.1_rad}},
1_s, {0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
@@ -310,16 +326,19 @@ TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) {
// Add a tiny tolerance for the upper bound because of floating point rounding
// error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation2d{},
wpi::units::meter_t{time}, wpi::units::meter_t{time});
estimator.UpdateWithTime(wpi::units::second_t{time},
wpi::math::Rotation2d{}, wpi::units::meter_t{time},
wpi::units::meter_t{time});
}
// Sample at an added time
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
@@ -329,31 +348,37 @@ TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) {
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}},
2.2_s);
estimator.AddVisionMeasurement(
wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}}, 2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
estimator.AddVisionMeasurement(wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}},
0.9_s);
estimator.AddVisionMeasurement(
wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}}, 0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
}
TEST(DifferentialDrivePoseEstimatorTest, TestReset) {

41
wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
View File

@@ -18,7 +18,8 @@
namespace {
wpi::math::Vectord<5> Dynamics(const wpi::math::Vectord<5>& x, const wpi::math::Vectord<2>& u) {
wpi::math::Vectord<5> Dynamics(const wpi::math::Vectord<5>& x,
const wpi::math::Vectord<2>& u) {
auto motors = wpi::math::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
@@ -50,12 +51,14 @@ wpi::math::Vectord<5> Dynamics(const wpi::math::Vectord<5>& x, const wpi::math::
}
wpi::math::Vectord<3> LocalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
const wpi::math::Vectord<5>& x,
[[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<3>{x(2), x(3), x(4)};
}
wpi::math::Vectord<5> GlobalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
const wpi::math::Vectord<5>& x,
[[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
} // namespace
@@ -64,10 +67,10 @@ TEST(ExtendedKalmanFilterTest, Init) {
constexpr wpi::units::second_t dt = 5_ms;
wpi::math::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
dt};
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
dt};
wpi::math::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
@@ -84,22 +87,22 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
wpi::math::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
dt};
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
dt};
auto waypoints =
std::vector<wpi::math::Pose2d>{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto waypoints = std::vector<wpi::math::Pose2d>{
wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
wpi::math::Vectord<5> r = wpi::math::Vectord<5>::Zero();
wpi::math::Vectord<2> u = wpi::math::Vectord<2>::Zero();
auto B = wpi::math::NumericalJacobianU<5, 5, 2>(Dynamics, wpi::math::Vectord<5>::Zero(),
wpi::math::Vectord<2>::Zero());
auto B = wpi::math::NumericalJacobianU<5, 5, 2>(
Dynamics, wpi::math::Vectord<5>::Zero(), wpi::math::Vectord<2>::Zero());
observer.SetXhat(wpi::math::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
@@ -119,10 +122,12 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY = LocalMeasurementModel(nextR, wpi::math::Vectord<2>::Zero());
observer.Correct(u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
observer.Correct(
u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot - Dynamics(r, wpi::math::Vectord<2>::Zero()));
u = B.householderQr().solve(rdot -
Dynamics(r, wpi::math::Vectord<2>::Zero()));
observer.Predict(u, dt);

3
wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
View File

@@ -16,6 +16,7 @@
TEST(KalmanFilterTest, Flywheel) {
auto motor = wpi::math::DCMotor::NEO();
auto flywheel = wpi::math::LinearSystemId::FlywheelSystem(motor, 1_kg_sq_m, 1.0);
auto flywheel =
wpi::math::LinearSystemId::FlywheelSystem(motor, 1_kg_sq_m, 1.0);
wpi::math::KalmanFilter<1, 1, 1> kf{flywheel, {1}, {1}, 5_ms};
}

199
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp
View File

@@ -38,14 +38,16 @@ void testFollowTrajectory(
wpi::units::second_t t = 0_s;
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
std::vector<
std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
if (debug) {
wpi::util::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
wpi::util::print(
"time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
}
while (t < trajectory.TotalTime()) {
@@ -57,9 +59,10 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Transform2d{
wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -85,9 +88,10 @@ void testFollowTrajectory(
auto xhat = estimator.UpdateWithTime(
t,
wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation()},
wpi::math::Rotation3d{
groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation()},
wheelPositions);
if (debug) {
@@ -111,7 +115,8 @@ void testFollowTrajectory(
}
if (debug) {
wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::print(
"apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
@@ -119,9 +124,9 @@ void testFollowTrajectory(
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
}
}
@@ -147,49 +152,55 @@ void testFollowTrajectory(
TEST(MecanumDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, wheelPositions,
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
kinematics, wpi::math::Rotation3d{}, wheelPositions,
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory =
wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, true, false);
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
20_ms, 100_ms, 250_ms, true, false);
}
TEST(MecanumDrivePoseEstimator3dTest, BadInitialPose) {
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, wheelPositions,
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.1}};
kinematics, wpi::math::Rotation3d{}, wheelPositions,
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.1}};
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory =
wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
for (wpi::units::degree_t offset_direction_degs = 0_deg;
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
@@ -198,21 +209,21 @@ TEST(MecanumDrivePoseEstimator3dTest, BadInitialPose) {
auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
auto initial_pose = trajectory.InitialPose() +
wpi::math::Transform2d{
wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, false, false);
}
}
}
@@ -225,7 +236,8 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
// pose would converge to that measurement.
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveWheelPositions wheelPositions;
@@ -233,7 +245,8 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
kinematics,
wpi::math::Rotation3d{},
wheelPositions,
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
wpi::math::Pose3d{1_m, 2_m, 0_m,
wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
{0.1, 0.1, 0.1, 0.1},
{0.45, 0.45, 0.45, 0.1}};
@@ -241,11 +254,16 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
wpi::math::Pose3d{0_m, 0_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
wpi::math::Pose3d{0_m, 0_m, 0_m,
wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}},
0_s);
estimator.AddVisionMeasurement(
wpi::math::Pose3d{3_m, 1_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
wpi::math::Pose3d{3_m, 1_m, 0_m,
wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}},
0_s);
estimator.AddVisionMeasurement(
wpi::math::Pose3d{2_m, 4_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
wpi::math::Pose3d{2_m, 4_m, 0_m,
wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
0_s);
}
@@ -283,11 +301,16 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
kinematics,
wpi::math::Rotation3d{},
wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose3d{},
{0.1, 0.1, 0.1, 0.1},
{0.45, 0.45, 0.45, 0.45}};
// Add enough measurements to fill up the buffer
for (auto time = 0_s; time < 4_s; time += 20_ms) {
@@ -299,8 +322,9 @@ TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
wpi::math::Pose3d{10_m, 10_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
{0.1, 0.1, 0.1, 0.1});
wpi::math::Pose3d{10_m, 10_m, 0_m,
wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}},
1_s, {0.1, 0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
estimator.GetEstimatedPosition().X().value(), 1e-6);
@@ -319,10 +343,15 @@ TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
TEST(MecanumDrivePoseEstimator3dTest, TestSampleAt) {
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose3d{}, {1.0, 1.0, 1.0, 1.0}, {1.0, 1.0, 1.0, 1.0}};
kinematics,
wpi::math::Rotation3d{},
wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose3d{},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
// Returns empty when null
EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
@@ -332,37 +361,46 @@ TEST(MecanumDrivePoseEstimator3dTest, TestSampleAt) {
// error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
wpi::math::MecanumDriveWheelPositions wheelPositions{
wpi::units::meter_t{time}, wpi::units::meter_t{time}, wpi::units::meter_t{time},
wpi::units::meter_t{time}};
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation3d{},
wheelPositions);
wpi::units::meter_t{time}, wpi::units::meter_t{time},
wpi::units::meter_t{time}, wpi::units::meter_t{time}};
estimator.UpdateWithTime(wpi::units::second_t{time},
wpi::math::Rotation3d{}, wheelPositions);
}
// Sample at an added time
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(
std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Sampling after the newest value returns the newest value
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
EXPECT_EQ(
std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(
wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
wpi::math::Pose3d{2_m, 0_m, 0_m,
wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(
std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
@@ -370,25 +408,31 @@ TEST(MecanumDrivePoseEstimator3dTest, TestSampleAt) {
wpi::math::Pose3d{1_m, 0.2_m, 0_m, wpi::math::Rotation3d{}}, 0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
}
TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics,
wpi::math::Rotation3d{},
wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose3d{-1_m, -1_m, -1_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
wpi::math::Pose3d{-1_m, -1_m, -1_m,
wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
@@ -401,8 +445,9 @@ TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset position
estimator.ResetPosition(wpi::math::Rotation3d{}, {1_m, 1_m, 1_m, 1_m},
wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
estimator.ResetPosition(
wpi::math::Rotation3d{}, {1_m, 1_m, 1_m, 1_m},
wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());

199
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
View File

@@ -30,7 +30,8 @@ void testFollowTrajectory(
const bool debug) {
wpi::math::MecanumDriveWheelPositions wheelPositions{};
estimator.ResetPosition(wpi::math::Rotation2d{}, wheelPositions, startingPose);
estimator.ResetPosition(wpi::math::Rotation2d{}, wheelPositions,
startingPose);
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
@@ -38,14 +39,16 @@ void testFollowTrajectory(
wpi::units::second_t t = 0_s;
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
std::vector<
std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
if (debug) {
wpi::util::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
wpi::util::print(
"time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
}
while (t < trajectory.TotalTime()) {
@@ -57,9 +60,10 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Transform2d{
wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -90,11 +94,11 @@ void testFollowTrajectory(
wheelPositions);
if (debug) {
wpi::util::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().Radians().value(),
groundTruthState.pose.X().value(),
groundTruthState.pose.Y().value(),
groundTruthState.pose.Rotation().Radians().value());
wpi::util::print(
"{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().Radians().value(),
groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
groundTruthState.pose.Rotation().Radians().value());
}
double error = groundTruthState.pose.Translation()
@@ -110,7 +114,8 @@ void testFollowTrajectory(
}
if (debug) {
wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::print(
"apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
@@ -118,9 +123,9 @@ void testFollowTrajectory(
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
}
}
@@ -142,49 +147,55 @@ void testFollowTrajectory(
TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDrivePoseEstimator estimator{kinematics, wpi::math::Rotation2d{},
wheelPositions, wpi::math::Pose2d{},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
wpi::math::MecanumDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, wheelPositions,
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory =
wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, true, false);
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
20_ms, 100_ms, 250_ms, true, false);
}
TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDrivePoseEstimator estimator{kinematics, wpi::math::Rotation2d{},
wheelPositions, wpi::math::Pose2d{},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
wpi::math::MecanumDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, wheelPositions,
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory =
wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
for (wpi::units::degree_t offset_direction_degs = 0_deg;
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
@@ -193,21 +204,21 @@ TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
auto initial_pose = trajectory.InitialPose() +
wpi::math::Transform2d{
wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, false, false);
}
}
}
@@ -220,14 +231,18 @@ TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
// pose would converge to that measurement.
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{},
wheelPositions, wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
kinematics,
wpi::math::Rotation2d{},
wheelPositions,
wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
{0.1, 0.1, 0.1},
{0.45, 0.45, 0.1}};
estimator.UpdateWithTime(0_s, wpi::math::Rotation2d{}, wheelPositions);
@@ -271,11 +286,16 @@ TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
kinematics,
wpi::math::Rotation2d{},
wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose2d{},
{0.1, 0.1, 0.1},
{0.45, 0.45, 0.45}};
// Add enough measurements to fill up the buffer
for (auto time = 0_s; time < 4_s; time += 20_ms) {
@@ -287,7 +307,8 @@ TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m}, wpi::math::Rotation2d{0.1_rad}},
wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m},
wpi::math::Rotation2d{0.1_rad}},
1_s, {0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
@@ -302,10 +323,15 @@ TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) {
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose2d{}, {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};
kinematics,
wpi::math::Rotation2d{},
wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose2d{},
{1.0, 1.0, 1.0},
{1.0, 1.0, 1.0}};
// Returns empty when null
EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
@@ -315,18 +341,20 @@ TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) {
// error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
wpi::math::MecanumDriveWheelPositions wheelPositions{
wpi::units::meter_t{time}, wpi::units::meter_t{time}, wpi::units::meter_t{time},
wpi::units::meter_t{time}};
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation2d{},
wheelPositions);
wpi::units::meter_t{time}, wpi::units::meter_t{time},
wpi::units::meter_t{time}, wpi::units::meter_t{time}};
estimator.UpdateWithTime(wpi::units::second_t{time},
wpi::math::Rotation2d{}, wheelPositions);
}
// Sample at an added time
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
@@ -336,37 +364,44 @@ TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) {
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}},
2.2_s);
estimator.AddVisionMeasurement(
wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}}, 2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
estimator.AddVisionMeasurement(wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}},
0.9_s);
estimator.AddVisionMeasurement(
wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}}, 0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
}
TEST(MecanumDrivePoseEstimatorTest, TestReset) {
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDrivePoseEstimator estimator{
kinematics,
wpi::math::Rotation2d{},

10
wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
View File

@@ -9,11 +9,13 @@
TEST(MerweScaledSigmaPointsTest, ZeroMean) {
wpi::math::MerweScaledSigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SquareRootSigmaPoints(
wpi::math::Vectord<2>{0.0, 0.0}, wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
wpi::math::Vectord<2>{0.0, 0.0},
wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
EXPECT_TRUE(
(points - wpi::math::Matrixd<2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
{0.0, 0.0, 0.00173205, 0.0, -0.00173205}})
(points -
wpi::math::Matrixd<2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
{0.0, 0.0, 0.00173205, 0.0, -0.00173205}})
.norm() < 1e-3);
}
@@ -25,6 +27,6 @@ TEST(MerweScaledSigmaPointsTest, NonzeroMean) {
EXPECT_TRUE(
(points - wpi::math::Matrixd<2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
{2.0, 2.0, 2.00548, 2.0, 1.99452}})
{2.0, 2.0, 2.00548, 2.0, 1.99452}})
.norm() < 1e-3);
}

99
wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp
View File

@@ -26,7 +26,7 @@ namespace {
// First test system, differential drive
wpi::math::Vectord<5> DriveDynamics(const wpi::math::Vectord<5>& x,
const wpi::math::Vectord<2>& u) {
const wpi::math::Vectord<2>& u) {
auto motors = wpi::math::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
@@ -58,12 +58,14 @@ wpi::math::Vectord<5> DriveDynamics(const wpi::math::Vectord<5>& x,
}
wpi::math::Vectord<3> DriveLocalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
const wpi::math::Vectord<5>& x,
[[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<3>{x(2), x(3), x(4)};
}
wpi::math::Vectord<5> DriveGlobalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
const wpi::math::Vectord<5>& x,
[[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
@@ -71,15 +73,15 @@ TEST(MerweUKFTest, DriveInit) {
constexpr wpi::units::second_t dt = 5_ms;
wpi::math::MerweUKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
wpi::math::AngleMean<5, 2 * 5 + 1>(2),
wpi::math::AngleMean<3, 2 * 5 + 1>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
wpi::math::AngleMean<5, 2 * 5 + 1>(2),
wpi::math::AngleMean<3, 2 * 5 + 1>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
wpi::math::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
@@ -88,9 +90,10 @@ TEST(MerweUKFTest, DriveInit) {
auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
wpi::math::AngleMean<5, 2 * 5 + 1>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2));
observer.Correct<5>(
u, globalY, DriveGlobalMeasurementModel, R,
wpi::math::AngleMean<5, 2 * 5 + 1>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2));
}
TEST(MerweUKFTest, DriveConvergence) {
@@ -98,19 +101,19 @@ TEST(MerweUKFTest, DriveConvergence) {
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
wpi::math::MerweUKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
wpi::math::AngleMean<5, 2 * 5 + 1>(2),
wpi::math::AngleMean<3, 2 * 5 + 1>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
wpi::math::AngleMean<5, 2 * 5 + 1>(2),
wpi::math::AngleMean<3, 2 * 5 + 1>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
auto waypoints =
std::vector<wpi::math::Pose2d>{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto waypoints = std::vector<wpi::math::Pose2d>{
wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
@@ -118,7 +121,8 @@ TEST(MerweUKFTest, DriveConvergence) {
wpi::math::Vectord<2> u = wpi::math::Vectord<2>::Zero();
auto B = wpi::math::NumericalJacobianU<5, 5, 2>(
DriveDynamics, wpi::math::Vectord<5>::Zero(), wpi::math::Vectord<2>::Zero());
DriveDynamics, wpi::math::Vectord<5>::Zero(),
wpi::math::Vectord<2>::Zero());
observer.SetXhat(wpi::math::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
@@ -139,12 +143,14 @@ TEST(MerweUKFTest, DriveConvergence) {
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY = DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
observer.Correct(u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
auto localY =
DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
observer.Correct(
u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot -
DriveDynamics(r, wpi::math::Vectord<2>::Zero()));
u = B.householderQr().solve(
rdot - DriveDynamics(r, wpi::math::Vectord<2>::Zero()));
observer.Predict(u, dt);
@@ -158,7 +164,8 @@ TEST(MerweUKFTest, DriveConvergence) {
auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
wpi::math::AngleMean<5, 2 * 5 + 1>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleMean<5, 2 * 5 + 1>(2),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2)
);
@@ -176,8 +183,9 @@ TEST(MerweUKFTest, DriveConvergence) {
TEST(MerweUKFTest, LinearUKF) {
constexpr wpi::units::second_t dt = 20_ms;
auto plant = wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
auto plant =
wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
wpi::math::MerweUKF<1, 1, 1> observer{
[&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return plant.A() * x + plant.B() * u;
@@ -209,8 +217,12 @@ TEST(MerweUKFTest, RoundTripP) {
constexpr wpi::units::second_t dt = 5_ms;
wpi::math::MerweUKF<2, 2, 2> observer{
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) {
return x;
},
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) {
return x;
},
{0.0, 0.0},
{0.0, 0.0},
dt};
@@ -223,7 +235,7 @@ TEST(MerweUKFTest, RoundTripP) {
// Second system, single motor feedforward estimator
wpi::math::Vectord<4> MotorDynamics(const wpi::math::Vectord<4>& x,
const wpi::math::Vectord<1>& u) {
const wpi::math::Vectord<1>& u) {
double v = x(1);
double kV = x(2);
double kA = x(3);
@@ -235,7 +247,7 @@ wpi::math::Vectord<4> MotorDynamics(const wpi::math::Vectord<4>& x,
}
wpi::math::Vectord<3> MotorMeasurementModel(const wpi::math::Vectord<4>& x,
const wpi::math::Vectord<1>& u) {
const wpi::math::Vectord<1>& u) {
double p = x(0);
double v = x(1);
double kV = x(2);
@@ -271,9 +283,9 @@ TEST(MerweUKFTest, MotorConvergence) {
states[0] = wpi::math::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
constexpr wpi::math::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
{0.0, -true_kV / true_kA, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0}};
{0.0, -true_kV / true_kA, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0}};
constexpr wpi::math::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
wpi::math::Matrixd<4, 4> discA;
@@ -291,7 +303,8 @@ TEST(MerweUKFTest, MotorConvergence) {
wpi::math::Vectord<4> P0{0.001, 0.001, 10, 10};
wpi::math::MerweUKF<4, 1, 3> observer{
MotorDynamics, MotorMeasurementModel, wpi::util::array{0.1, 1.0, 1e-10, 1e-10},
MotorDynamics, MotorMeasurementModel,
wpi::util::array{0.1, 1.0, 1e-10, 1e-10},
wpi::util::array{pos_stddev, vel_stddev, accel_stddev}, dt};
observer.SetXhat(wpi::math::Vectord<4>{0.0, 0.0, 2.0, 2.0});

13
wpimath/src/test/native/cpp/estimator/S3SigmaPointsTest.cpp
View File

@@ -29,11 +29,13 @@ TEST(S3SigmaPointsTest, Simplex) {
TEST(S3SigmaPointsTest, ZeroMean) {
wpi::math::S3SigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SquareRootSigmaPoints(
wpi::math::Vectord<2>{0.0, 0.0}, wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
wpi::math::Vectord<2>{0.0, 0.0},
wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
EXPECT_TRUE(
(points - wpi::math::Matrixd<2, 4>{{0.0, -0.00122474, 0.00122474, 0.0},
{0.0, -0.00070711, -0.00070711, 0.00141421}})
(points -
wpi::math::Matrixd<2, 4>{{0.0, -0.00122474, 0.00122474, 0.0},
{0.0, -0.00070711, -0.00070711, 0.00141421}})
.norm() < 1e-7);
}
@@ -44,7 +46,8 @@ TEST(S3SigmaPointsTest, NonzeroMean) {
wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
EXPECT_TRUE(
(points - wpi::math::Matrixd<2, 4>{{1.0, 0.99877526, 1.00122474, 1.0},
{2.0, 1.99776393, 1.99776393, 2.00447214}})
(points -
wpi::math::Matrixd<2, 4>{{1.0, 0.99877526, 1.00122474, 1.0},
{2.0, 1.99776393, 1.99776393, 2.00447214}})
.norm() < 1e-7);
}

99
wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp
View File

@@ -26,7 +26,7 @@ namespace {
// First test system, differential drive
wpi::math::Vectord<5> DriveDynamics(const wpi::math::Vectord<5>& x,
const wpi::math::Vectord<2>& u) {
const wpi::math::Vectord<2>& u) {
auto motors = wpi::math::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
@@ -58,12 +58,14 @@ wpi::math::Vectord<5> DriveDynamics(const wpi::math::Vectord<5>& x,
}
wpi::math::Vectord<3> DriveLocalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
const wpi::math::Vectord<5>& x,
[[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<3>{x(2), x(3), x(4)};
}
wpi::math::Vectord<5> DriveGlobalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
const wpi::math::Vectord<5>& x,
[[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
@@ -71,15 +73,15 @@ TEST(S3UKFTest, DriveInit) {
constexpr auto dt = 5_ms;
wpi::math::S3UKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
wpi::math::AngleMean<5, 5 + 2>(2),
wpi::math::AngleMean<3, 5 + 2>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
wpi::math::AngleMean<5, 5 + 2>(2),
wpi::math::AngleMean<3, 5 + 2>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
wpi::math::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
@@ -88,9 +90,10 @@ TEST(S3UKFTest, DriveInit) {
auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
wpi::math::AngleMean<5, 5 + 2>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2));
observer.Correct<5>(
u, globalY, DriveGlobalMeasurementModel, R,
wpi::math::AngleMean<5, 5 + 2>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2));
}
TEST(S3UKFTest, DriveConvergence) {
@@ -98,19 +101,19 @@ TEST(S3UKFTest, DriveConvergence) {
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
wpi::math::S3UKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
wpi::math::AngleMean<5, 5 + 2>(2),
wpi::math::AngleMean<3, 5 + 2>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
wpi::math::AngleMean<5, 5 + 2>(2),
wpi::math::AngleMean<3, 5 + 2>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
auto waypoints =
std::vector<wpi::math::Pose2d>{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto waypoints = std::vector<wpi::math::Pose2d>{
wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
@@ -118,7 +121,8 @@ TEST(S3UKFTest, DriveConvergence) {
wpi::math::Vectord<2> u = wpi::math::Vectord<2>::Zero();
auto B = wpi::math::NumericalJacobianU<5, 5, 2>(
DriveDynamics, wpi::math::Vectord<5>::Zero(), wpi::math::Vectord<2>::Zero());
DriveDynamics, wpi::math::Vectord<5>::Zero(),
wpi::math::Vectord<2>::Zero());
observer.SetXhat(wpi::math::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
@@ -139,12 +143,14 @@ TEST(S3UKFTest, DriveConvergence) {
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY = DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
observer.Correct(u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
auto localY =
DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
observer.Correct(
u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot -
DriveDynamics(r, wpi::math::Vectord<2>::Zero()));
u = B.householderQr().solve(
rdot - DriveDynamics(r, wpi::math::Vectord<2>::Zero()));
observer.Predict(u, dt);
@@ -158,7 +164,8 @@ TEST(S3UKFTest, DriveConvergence) {
auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
wpi::math::AngleMean<5, 5 + 2>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleMean<5, 5 + 2>(2),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2)
);
@@ -176,8 +183,9 @@ TEST(S3UKFTest, DriveConvergence) {
TEST(S3UKFTest, LinearUKF) {
constexpr wpi::units::second_t dt = 20_ms;
auto plant = wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
auto plant =
wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
wpi::math::S3UKF<1, 1, 1> observer{
[&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return plant.A() * x + plant.B() * u;
@@ -209,8 +217,12 @@ TEST(S3UKFTest, RoundTripP) {
constexpr auto dt = 5_ms;
wpi::math::S3UKF<2, 2, 2> observer{
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) {
return x;
},
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) {
return x;
},
{0.0, 0.0},
{0.0, 0.0},
dt};
@@ -223,7 +235,7 @@ TEST(S3UKFTest, RoundTripP) {
// Second system, single motor feedforward estimator
wpi::math::Vectord<4> MotorDynamics(const wpi::math::Vectord<4>& x,
const wpi::math::Vectord<1>& u) {
const wpi::math::Vectord<1>& u) {
double v = x(1);
double kV = x(2);
double kA = x(3);
@@ -235,7 +247,7 @@ wpi::math::Vectord<4> MotorDynamics(const wpi::math::Vectord<4>& x,
}
wpi::math::Vectord<3> MotorMeasurementModel(const wpi::math::Vectord<4>& x,
const wpi::math::Vectord<1>& u) {
const wpi::math::Vectord<1>& u) {
double p = x(0);
double v = x(1);
double kV = x(2);
@@ -271,9 +283,9 @@ TEST(S3UKFTest, MotorConvergence) {
states[0] = wpi::math::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
constexpr wpi::math::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
{0.0, -true_kV / true_kA, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0}};
{0.0, -true_kV / true_kA, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0}};
constexpr wpi::math::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
wpi::math::Matrixd<4, 4> discA;
@@ -291,7 +303,8 @@ TEST(S3UKFTest, MotorConvergence) {
wpi::math::Vectord<4> P0{0.001, 0.001, 10, 10};
wpi::math::S3UKF<4, 1, 3> observer{
MotorDynamics, MotorMeasurementModel, wpi::util::array{0.1, 1.0, 1e-10, 1e-10},
MotorDynamics, MotorMeasurementModel,
wpi::util::array{0.1, 1.0, 1e-10, 1e-10},
wpi::util::array{pos_stddev, vel_stddev, accel_stddev}, dt};
observer.SetXhat(wpi::math::Vectord<4>{0.0, 0.0, 2.0, 2.0});

207
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp
View File

@@ -29,7 +29,8 @@ void testFollowTrajectory(
const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
wpi::util::array<wpi::math::SwerveModulePosition, 4> positions{wpi::util::empty_array};
wpi::util::array<wpi::math::SwerveModulePosition, 4> positions{
wpi::util::empty_array};
estimator.ResetPosition(wpi::math::Rotation3d{}, positions,
wpi::math::Pose3d{startingPose});
@@ -40,14 +41,16 @@ void testFollowTrajectory(
wpi::units::second_t t = 0_s;
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
std::vector<
std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
if (debug) {
wpi::util::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
wpi::util::print(
"time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
}
while (t < trajectory.TotalTime()) {
@@ -59,9 +62,10 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Transform2d{
wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -87,9 +91,10 @@ void testFollowTrajectory(
auto xhat = estimator.UpdateWithTime(
t,
wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation()},
wpi::math::Rotation3d{
groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation()},
positions);
if (debug) {
@@ -113,7 +118,8 @@ void testFollowTrajectory(
}
if (debug) {
wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::print(
"apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
@@ -121,9 +127,9 @@ void testFollowTrajectory(
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
}
}
@@ -149,7 +155,8 @@ void testFollowTrajectory(
TEST(SwerveDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
@@ -157,31 +164,35 @@ TEST(SwerveDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
wpi::math::SwerveModulePosition br;
wpi::math::SwerveDrivePoseEstimator3d<4> estimator{
kinematics, wpi::math::Rotation3d{}, {fl, fr, bl, br},
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
kinematics, wpi::math::Rotation3d{}, {fl, fr, bl, br},
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory =
wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, wpi::math::Rotation2d{45_deg}}, {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
20_ms, 100_ms, 250_ms, true, false);
{0_m, 0_m, wpi::math::Rotation2d{45_deg}},
{0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms, 250_ms, true,
false);
}
TEST(SwerveDrivePoseEstimator3dTest, BadInitialPose) {
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
@@ -189,15 +200,17 @@ TEST(SwerveDrivePoseEstimator3dTest, BadInitialPose) {
wpi::math::SwerveModulePosition br;
wpi::math::SwerveDrivePoseEstimator3d<4> estimator{
kinematics, wpi::math::Rotation3d{}, {fl, fr, bl, br},
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.9, 0.9, 0.9, 0.9}};
kinematics, wpi::math::Rotation3d{}, {fl, fr, bl, br},
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.9, 0.9, 0.9, 0.9}};
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory =
wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
for (wpi::units::degree_t offset_direction_degs = 0_deg;
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
@@ -206,21 +219,21 @@ TEST(SwerveDrivePoseEstimator3dTest, BadInitialPose) {
auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
auto initial_pose = trajectory.InitialPose() +
wpi::math::Transform2d{
wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, false, false);
}
}
}
@@ -233,7 +246,8 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
// pose would converge to that measurement.
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
@@ -244,7 +258,8 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
kinematics,
wpi::math::Rotation3d{},
{fl, fr, bl, br},
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
wpi::math::Pose3d{1_m, 2_m, 0_m,
wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
{0.1, 0.1, 0.1, 0.1},
{0.45, 0.45, 0.45, 0.45}};
@@ -252,11 +267,16 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
wpi::math::Pose3d{0_m, 0_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
wpi::math::Pose3d{0_m, 0_m, 0_m,
wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}},
0_s);
estimator.AddVisionMeasurement(
wpi::math::Pose3d{3_m, 1_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
wpi::math::Pose3d{3_m, 1_m, 0_m,
wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}},
0_s);
estimator.AddVisionMeasurement(
wpi::math::Pose3d{2_m, 4_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
wpi::math::Pose3d{2_m, 4_m, 0_m,
wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
0_s);
}
@@ -294,7 +314,8 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
TEST(SwerveDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
@@ -302,8 +323,8 @@ TEST(SwerveDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
wpi::math::SwerveModulePosition br;
wpi::math::SwerveDrivePoseEstimator3d<4> estimator{
kinematics, wpi::math::Rotation3d{}, {fl, fr, bl, br},
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
kinematics, wpi::math::Rotation3d{}, {fl, fr, bl, br},
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
// Add enough measurements to fill up the buffer
for (auto time = 0_s; time < 4_s; time += 20_ms) {
@@ -314,8 +335,9 @@ TEST(SwerveDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
wpi::math::Pose3d{10_m, 10_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
{0.1, 0.1, 0.1, 0.1});
wpi::math::Pose3d{10_m, 10_m, 0_m,
wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}},
1_s, {0.1, 0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
estimator.GetEstimatedPosition().X().value(), 1e-6);
@@ -334,7 +356,8 @@ TEST(SwerveDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
TEST(SwerveDrivePoseEstimator3dTest, TestSampleAt) {
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDrivePoseEstimator3d estimator{
kinematics,
wpi::math::Rotation3d{},
@@ -352,39 +375,52 @@ TEST(SwerveDrivePoseEstimator3dTest, TestSampleAt) {
// error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
wpi::util::array<wpi::math::SwerveModulePosition, 4> wheelPositions{
{wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}}}};
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation3d{},
wheelPositions);
{wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
wpi::math::Rotation2d{}}}};
estimator.UpdateWithTime(wpi::units::second_t{time},
wpi::math::Rotation3d{}, wheelPositions);
}
// Sample at an added time
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(
std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Sampling after the newest value returns the newest value
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
EXPECT_EQ(
std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(
wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
wpi::math::Pose3d{2_m, 0_m, 0_m,
wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(
std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
@@ -392,26 +428,32 @@ TEST(SwerveDrivePoseEstimator3dTest, TestSampleAt) {
wpi::math::Pose3d{1_m, 0.2_m, 0_m, wpi::math::Rotation3d{}}, 0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
EXPECT_EQ(std::optional(
wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
}
TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDrivePoseEstimator3d estimator{
kinematics,
wpi::math::Rotation3d{},
{wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{},
wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{}},
wpi::math::Pose3d{-1_m, -1_m, -1_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
wpi::math::Pose3d{-1_m, -1_m, -1_m,
wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
@@ -425,7 +467,8 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
// Test reset position
{
wpi::math::SwerveModulePosition modulePosition{1_m, wpi::math::Rotation2d{}};
wpi::math::SwerveModulePosition modulePosition{1_m,
wpi::math::Rotation2d{}};
estimator.ResetPosition(
wpi::math::Rotation3d{},
{modulePosition, modulePosition, modulePosition, modulePosition},
@@ -441,9 +484,10 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
// Test orientation and wheel positions
{
wpi::math::SwerveModulePosition modulePosition{2_m, wpi::math::Rotation2d{}};
wpi::math::SwerveModulePosition modulePosition{2_m,
wpi::math::Rotation2d{}};
estimator.Update(wpi::math::Rotation3d{}, {modulePosition, modulePosition,
modulePosition, modulePosition});
modulePosition, modulePosition});
}
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
@@ -466,9 +510,10 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
// Test orientation
{
wpi::math::SwerveModulePosition modulePosition{3_m, wpi::math::Rotation2d{}};
wpi::math::SwerveModulePosition modulePosition{3_m,
wpi::math::Rotation2d{}};
estimator.Update(wpi::math::Rotation3d{}, {modulePosition, modulePosition,
modulePosition, modulePosition});
modulePosition, modulePosition});
}
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());

205
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
View File

@@ -30,7 +30,8 @@ void testFollowTrajectory(
const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
wpi::util::array<wpi::math::SwerveModulePosition, 4> positions{wpi::util::empty_array};
wpi::util::array<wpi::math::SwerveModulePosition, 4> positions{
wpi::util::empty_array};
estimator.ResetPosition(wpi::math::Rotation2d{}, positions, startingPose);
@@ -40,14 +41,16 @@ void testFollowTrajectory(
wpi::units::second_t t = 0_s;
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
std::vector<
std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
if (debug) {
wpi::util::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
wpi::util::print(
"time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
}
while (t < trajectory.TotalTime()) {
@@ -59,9 +62,10 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Transform2d{
wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -92,11 +96,11 @@ void testFollowTrajectory(
positions);
if (debug) {
wpi::util::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().Radians().value(),
groundTruthState.pose.X().value(),
groundTruthState.pose.Y().value(),
groundTruthState.pose.Rotation().Radians().value());
wpi::util::print(
"{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().Radians().value(),
groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
groundTruthState.pose.Rotation().Radians().value());
}
double error = groundTruthState.pose.Translation()
@@ -112,7 +116,8 @@ void testFollowTrajectory(
}
if (debug) {
wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::print(
"apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
@@ -120,9 +125,9 @@ void testFollowTrajectory(
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
}
}
@@ -144,7 +149,8 @@ void testFollowTrajectory(
TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
@@ -152,31 +158,35 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
wpi::math::SwerveModulePosition br;
wpi::math::SwerveDrivePoseEstimator<4> estimator{
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory =
wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, wpi::math::Rotation2d{45_deg}}, {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
20_ms, 100_ms, 250_ms, true, false);
{0_m, 0_m, wpi::math::Rotation2d{45_deg}},
{0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms, 250_ms, true,
false);
}
TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
@@ -184,15 +194,17 @@ TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
wpi::math::SwerveModulePosition br;
wpi::math::SwerveDrivePoseEstimator<4> estimator{
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}};
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}};
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory =
wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
for (wpi::units::degree_t offset_direction_degs = 0_deg;
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
@@ -201,21 +213,21 @@ TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
auto initial_pose = trajectory.InitialPose() +
wpi::math::Transform2d{
wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, false, false);
}
}
}
@@ -228,7 +240,8 @@ TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
// pose would converge to that measurement.
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
@@ -236,9 +249,12 @@ TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
wpi::math::SwerveModulePosition br;
wpi::math::SwerveDrivePoseEstimator<4> estimator{
kinematics, wpi::math::Rotation2d{},
{fl, fr, bl, br}, wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
kinematics,
wpi::math::Rotation2d{},
{fl, fr, bl, br},
wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
{0.1, 0.1, 0.1},
{0.45, 0.45, 0.45}};
estimator.UpdateWithTime(0_s, wpi::math::Rotation2d{}, {fl, fr, bl, br});
@@ -282,7 +298,8 @@ TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
@@ -290,8 +307,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
wpi::math::SwerveModulePosition br;
wpi::math::SwerveDrivePoseEstimator<4> estimator{
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
// Add enough measurements to fill up the buffer
for (auto time = 0_s; time < 4_s; time += 20_ms) {
@@ -302,7 +319,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m}, wpi::math::Rotation2d{0.1_rad}},
wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m},
wpi::math::Rotation2d{0.1_rad}},
1_s, {0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
@@ -317,7 +335,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) {
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDrivePoseEstimator estimator{
kinematics,
wpi::math::Rotation2d{},
@@ -335,20 +354,26 @@ TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) {
// error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
wpi::util::array<wpi::math::SwerveModulePosition, 4> wheelPositions{
{wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}}}};
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation2d{},
wheelPositions);
{wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
wpi::math::Rotation2d{}}}};
estimator.UpdateWithTime(wpi::units::second_t{time},
wpi::math::Rotation2d{}, wheelPositions);
}
// Sample at an added time
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
@@ -358,37 +383,44 @@ TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) {
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}},
2.2_s);
estimator.AddVisionMeasurement(
wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}}, 2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
estimator.AddVisionMeasurement(wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}},
0.9_s);
estimator.AddVisionMeasurement(
wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}}, 0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(
std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
}
TEST(SwerveDrivePoseEstimatorTest, TestReset) {
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDrivePoseEstimator estimator{
kinematics,
wpi::math::Rotation2d{},
@@ -406,7 +438,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
// Test reset position
{
wpi::math::SwerveModulePosition modulePosition{1_m, wpi::math::Rotation2d{}};
wpi::math::SwerveModulePosition modulePosition{1_m,
wpi::math::Rotation2d{}};
estimator.ResetPosition(
wpi::math::Rotation2d{},
{modulePosition, modulePosition, modulePosition, modulePosition},
@@ -420,9 +453,10 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
// Test orientation and wheel positions
{
wpi::math::SwerveModulePosition modulePosition{2_m, wpi::math::Rotation2d{}};
wpi::math::SwerveModulePosition modulePosition{2_m,
wpi::math::Rotation2d{}};
estimator.Update(wpi::math::Rotation2d{}, {modulePosition, modulePosition,
modulePosition, modulePosition});
modulePosition, modulePosition});
}
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
@@ -441,9 +475,10 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
// Test orientation
{
wpi::math::SwerveModulePosition modulePosition{3_m, wpi::math::Rotation2d{}};
wpi::math::SwerveModulePosition modulePosition{3_m,
wpi::math::Rotation2d{}};
estimator.Update(wpi::math::Rotation2d{}, {modulePosition, modulePosition,
modulePosition, modulePosition});
modulePosition, modulePosition});
}
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());