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

[wpimath] Replace UKF implementation with square root form (#4168)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Connor Worley
2022-06-08 22:19:01 -07:00
committed by GitHub
parent 45b7fc445b
commit a99c11c14c
22 changed files with 494 additions and 297 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -93,6 +93,6 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
}
EXPECT_NEAR(0.0, errorSum / (trajectory.TotalTime().value() / dt.value()),
0.2);
EXPECT_NEAR(0.0, maxError, 0.4);
0.05);
EXPECT_NEAR(0.0, maxError, 0.1);
}

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

@@ -84,6 +84,6 @@ TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
t += dt;
}
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
EXPECT_LT(maxError, 0.4);
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
EXPECT_LT(maxError, 0.1);
}

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

@@ -10,7 +10,7 @@ namespace drake::math {
namespace {
TEST(MerweScaledSigmaPointsTest, ZeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SigmaPoints(
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<2>{0.0, 0.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
EXPECT_TRUE(
@@ -21,8 +21,9 @@ TEST(MerweScaledSigmaPointsTest, ZeroMean) {
TEST(MerweScaledSigmaPointsTest, NonzeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SigmaPoints(
frc::Vectord<2>{1.0, 2.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 10.0}});
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<2>{1.0, 2.0},
frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
EXPECT_TRUE(
(points - frc::Matrixd<2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},

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

@@ -84,6 +84,6 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
t += dt;
}
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
EXPECT_LT(maxError, 0.4);
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
EXPECT_LT(maxError, 0.1);
}

44
wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
View File

@@ -23,7 +23,7 @@ namespace {
frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
// constexpr double Glow = 15.32; // Low gear ratio
constexpr double Ghigh = 7.08; // High gear ratio
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
constexpr auto r = 0.0746125_m; // Wheel radius
@@ -71,6 +71,11 @@ TEST(UnscentedKalmanFilterTest, Init) {
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
frc::AngleMean<5, 5>(2),
frc::AngleMean<3, 5>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
dt};
frc::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
@@ -93,6 +98,11 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
frc::AngleMean<5, 5>(2),
frc::AngleMean<3, 5>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
dt};
auto waypoints =
@@ -150,12 +160,28 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
);
auto finalPosition = trajectory.Sample(trajectory.TotalTime());
ASSERT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
1.0);
ASSERT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
1.0);
ASSERT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
1.0);
ASSERT_NEAR(0.0, observer.Xhat(3), 1.0);
ASSERT_NEAR(0.0, observer.Xhat(4), 1.0);
EXPECT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
0.055);
EXPECT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
0.15);
EXPECT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
0.000005);
EXPECT_NEAR(0.0, observer.Xhat(3), 0.1);
EXPECT_NEAR(0.0, observer.Xhat(4), 0.1);
}
TEST(UnscentedKalmanFilterTest, RoundTripP) {
constexpr auto dt = 5_ms;
frc::UnscentedKalmanFilter<2, 2, 2> observer{
[](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
[](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
{0.0, 0.0},
{0.0, 0.0},
dt};
frc::Matrixd<2, 2> P({{2, 1}, {1, 2}});
observer.SetP(P);
ASSERT_TRUE(observer.P().isApprox(P));
}