From b8d6bc2eb1b6cea10d1179939114d041945e172a Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Fri, 7 Nov 2025 18:07:43 -0800 Subject: [PATCH] [wpimath] Scale transforms instead of twists in PoseEstimator (#8333) The spiraling issue occurs when the vision rotation standard deviation is very high relative to the odometry rotation standard deviation and the vision measurements have a large rotation error. (Scaling the rotation component of a twist without scaling the translation component causes the direction of overall translation to change, leading to spiraling around (either towards or away) the vision measurement instead of moving towards it.) Using a transform instead of a twist avoids this issue. In general, scaling twist components is more mathematically correct than scaling transform components. However, although twists are correct for modeling uncertainty in an odometry-only pose estimate, they are not correct for the difference between the odometry-only pose estimate and a vision measurement. Since neither twists nor transforms are completely correct (and the pose estimator as a whole is not mathematically correct), but using transforms can guarantee that the pose estimate approaches the vision measurement (instead of potentially spiraling away), they are the least bad option. --- .../first/math/estimator/PoseEstimator.java | 26 +++++++---- .../first/math/estimator/PoseEstimator3d.java | 41 ++++++++++------- .../include/frc/estimator/PoseEstimator.h | 31 +++++++------ .../include/frc/estimator/PoseEstimator3d.h | 45 +++++++++++-------- 4 files changed, 85 insertions(+), 58 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java index 1f00cb7b18..4e9bf4b3e8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java @@ -11,8 +11,8 @@ import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.Kinematics; import edu.wpi.first.math.kinematics.Odometry; @@ -273,19 +273,27 @@ public class PoseEstimator { return; } - // Step 4: Measure the twist between the old pose estimate and the vision pose. - var twist = visionSample.get().log(visionRobotPoseMeters); + // Step 4: Measure the transform between the old pose estimate and the vision pose. + var transform = visionRobotPoseMeters.minus(visionSample.get()); - // Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman + // Step 5: We should not trust the transform entirely, so instead we scale this transform by a + // Kalman // gain matrix representing how much we trust vision measurements compared to our current pose. - var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta)); + var k_times_transform = + m_visionK.times( + VecBuilder.fill( + transform.getX(), transform.getY(), transform.getRotation().getRadians())); - // Step 6: Convert back to Twist2d. - var scaledTwist = - new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0)); + // Step 6: Convert back to Transform2d. + var scaledTransform = + new Transform2d( + k_times_transform.get(0, 0), + k_times_transform.get(1, 0), + Rotation2d.fromRadians(k_times_transform.get(2, 0))); // Step 7: Calculate and record the vision update. - var visionUpdate = new VisionUpdate(visionSample.get().exp(scaledTwist), odometrySample.get()); + var visionUpdate = + new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get()); m_visionUpdates.put(timestampSeconds, visionUpdate); // Step 8: Remove later vision measurements. (Matches previous behavior) diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java index 9ce96f0243..463cb9481c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java @@ -13,9 +13,9 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.geometry.Twist3d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.Kinematics; import edu.wpi.first.math.kinematics.Odometry3d; @@ -286,27 +286,36 @@ public class PoseEstimator3d { return; } - // Step 4: Measure the twist between the old pose estimate and the vision pose. - var twist = visionSample.get().log(visionRobotPoseMeters); + // Step 4: Measure the transform between the old pose estimate and the vision pose. + var transform = visionRobotPoseMeters.minus(visionSample.get()); - // Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman + // Step 5: We should not trust the transform entirely, so instead we scale this transform by a + // Kalman // gain matrix representing how much we trust vision measurements compared to our current pose. - var k_times_twist = + var k_times_transform = m_visionK.times( - VecBuilder.fill(twist.dx, twist.dy, twist.dz, twist.rx, twist.ry, twist.rz)); + VecBuilder.fill( + transform.getX(), + transform.getY(), + transform.getZ(), + transform.getRotation().getX(), + transform.getRotation().getY(), + transform.getRotation().getZ())); - // Step 6: Convert back to Twist3d. - var scaledTwist = - new Twist3d( - k_times_twist.get(0, 0), - k_times_twist.get(1, 0), - k_times_twist.get(2, 0), - k_times_twist.get(3, 0), - k_times_twist.get(4, 0), - k_times_twist.get(5, 0)); + // Step 6: Convert back to Transform3d. + var scaledTransform = + new Transform3d( + k_times_transform.get(0, 0), + k_times_transform.get(1, 0), + k_times_transform.get(2, 0), + new Rotation3d( + k_times_transform.get(3, 0), + k_times_transform.get(4, 0), + k_times_transform.get(5, 0))); // Step 7: Calculate and record the vision update. - var visionUpdate = new VisionUpdate(visionSample.get().exp(scaledTwist), odometrySample.get()); + var visionUpdate = + new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get()); m_visionUpdates.put(timestampSeconds, visionUpdate); // Step 8: Remove later vision measurements. (Matches previous behavior) diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h index 9c3bc4d6de..1aaf4a39d8 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h @@ -14,6 +14,7 @@ #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Transform2d.h" #include "frc/geometry/Translation2d.h" #include "frc/interpolation/TimeInterpolatableBuffer.h" #include "frc/kinematics/Kinematics.h" @@ -261,24 +262,26 @@ class WPILIB_DLLEXPORT PoseEstimator { return; } - // Step 4: Measure the twist between the old pose estimate and the vision - // pose. - auto twist = visionSample.value().Log(visionRobotPose); + // Step 4: Measure the transform between the old pose estimate and the + // vision transform. + auto transform = visionRobotPose - visionSample.value(); - // Step 5: We should not trust the twist entirely, so instead we scale this - // twist by a Kalman gain matrix representing how much we trust vision - // measurements compared to our current pose. - Eigen::Vector3d k_times_twist = - m_visionK * Eigen::Vector3d{twist.dx.value(), twist.dy.value(), - twist.dtheta.value()}; + // Step 5: We should not trust the transform entirely, so instead we scale + // this transform by a Kalman gain matrix representing how much we trust + // vision measurements compared to our current pose. + Eigen::Vector3d k_times_transform = + m_visionK * Eigen::Vector3d{transform.X().value(), + transform.Y().value(), + transform.Rotation().Radians().value()}; - // Step 6: Convert back to Twist2d. - Twist2d scaledTwist{units::meter_t{k_times_twist(0)}, - units::meter_t{k_times_twist(1)}, - units::radian_t{k_times_twist(2)}}; + // Step 6: Convert back to Transform2d. + Transform2d scaledTransform{ + units::meter_t{k_times_transform(0)}, + units::meter_t{k_times_transform(1)}, + Rotation2d{units::radian_t{k_times_transform(2)}}}; // Step 7: Calculate and record the vision update. - VisionUpdate visionUpdate{visionSample->Exp(scaledTwist), *odometrySample}; + VisionUpdate visionUpdate{*visionSample + scaledTransform, *odometrySample}; m_visionUpdates[timestamp] = visionUpdate; // Step 8: Remove later vision measurements. (Matches previous behavior) diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h index bc8bdb08a6..ecf9ccbf88 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h @@ -14,9 +14,10 @@ #include #include "frc/EigenCore.h" -#include "frc/geometry/Pose2d.h" -#include "frc/geometry/Rotation2d.h" -#include "frc/geometry/Translation2d.h" +#include "frc/geometry/Pose3d.h" +#include "frc/geometry/Rotation3d.h" +#include "frc/geometry/Transform3d.h" +#include "frc/geometry/Translation3d.h" #include "frc/interpolation/TimeInterpolatableBuffer.h" #include "frc/kinematics/Kinematics.h" #include "frc/kinematics/Odometry3d.h" @@ -270,26 +271,32 @@ class WPILIB_DLLEXPORT PoseEstimator3d { return; } - // Step 4: Measure the twist between the old pose estimate and the vision - // pose. - auto twist = visionSample.value().Log(visionRobotPose); + // Step 4: Measure the transform between the old pose estimate and the + // vision pose. + auto transform = visionRobotPose - visionSample.value(); - // Step 5: We should not trust the twist entirely, so instead we scale this - // twist by a Kalman gain matrix representing how much we trust vision - // measurements compared to our current pose. - frc::Vectord<6> k_times_twist = - m_visionK * frc::Vectord<6>{twist.dx.value(), twist.dy.value(), - twist.dz.value(), twist.rx.value(), - twist.ry.value(), twist.rz.value()}; + // Step 5: We should not trust the transform entirely, so instead we scale + // this transform by a Kalman gain matrix representing how much we trust + // vision measurements compared to our current pose. + frc::Vectord<6> k_times_transform = + m_visionK * frc::Vectord<6>{transform.X().value(), + transform.Y().value(), + transform.Z().value(), + transform.Rotation().X().value(), + transform.Rotation().Y().value(), + transform.Rotation().Z().value()}; - // Step 6: Convert back to Twist3d. - Twist3d scaledTwist{ - units::meter_t{k_times_twist(0)}, units::meter_t{k_times_twist(1)}, - units::meter_t{k_times_twist(2)}, units::radian_t{k_times_twist(3)}, - units::radian_t{k_times_twist(4)}, units::radian_t{k_times_twist(5)}}; + // Step 6: Convert back to Transform3d. + Transform3d scaledTransform{ + units::meter_t{k_times_transform(0)}, + units::meter_t{k_times_transform(1)}, + units::meter_t{k_times_transform(2)}, + Rotation3d{units::radian_t{k_times_transform(3)}, + units::radian_t{k_times_transform(4)}, + units::radian_t{k_times_transform(5)}}}; // Step 7: Calculate and record the vision update. - VisionUpdate visionUpdate{visionSample->Exp(scaledTwist), *odometrySample}; + VisionUpdate visionUpdate{*visionSample + scaledTransform, *odometrySample}; m_visionUpdates[timestamp] = visionUpdate; // Step 8: Remove later vision measurements. (Matches previous behavior)