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

[wpimath] Scale transforms instead of twists in PoseEstimator (#8333)

Browse Source 
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.
This commit is contained in:
Joseph Eng
2025-11-07 18:07:43 -08:00
committed by GitHub
parent 688535298b
commit b8d6bc2eb1
4 changed files with 85 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
View File

@@ -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<T> {
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)

41
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
View File

@@ -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<T> {
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)

31
wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
View File

@@ -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)

45
wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
View File

@@ -14,9 +14,10 @@
#include <wpi/array.h>
#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)