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

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-11-08 00:03:50 -08:00
parent 2109161534 b8d6bc2eb1
commit 161f8d107b
6 changed files with 90 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp
View File

@@ -12,6 +12,7 @@
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/geometry/Rotation2d.hpp"
#include "wpi/math/geometry/Transform2d.hpp"
#include "wpi/math/geometry/Translation2d.hpp"
#include "wpi/math/interpolation/TimeInterpolatableBuffer.hpp"
#include "wpi/math/kinematics/Kinematics.hpp"
@@ -261,25 +262,26 @@ class WPILIB_DLLEXPORT PoseEstimator {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision
// pose.
auto twist = (visionRobotPose - visionSample.value()).Log();
// 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{wpi::units::meter_t{k_times_twist(0)},
wpi::units::meter_t{k_times_twist(1)},
wpi::units::radian_t{k_times_twist(2)}};
// Step 6: Convert back to Transform2d.
Transform2d scaledTransform{
wpi::units::meter_t{k_times_transform(0)},
wpi::units::meter_t{k_times_transform(1)},
Rotation2d{wpi::units::radian_t{k_times_transform(2)}}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample.value() + scaledTwist.Exp(),
*odometrySample};
VisionUpdate visionUpdate{*visionSample + scaledTransform, *odometrySample};
m_visionUpdates[timestamp] = visionUpdate;
// Step 8: Remove later vision measurements. (Matches previous behavior)

42
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp
View File

@@ -13,6 +13,7 @@
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/geometry/Rotation2d.hpp"
#include "wpi/math/geometry/Transform3d.hpp"
#include "wpi/math/geometry/Translation2d.hpp"
#include "wpi/math/interpolation/TimeInterpolatableBuffer.hpp"
#include "wpi/math/kinematics/Kinematics.hpp"
@@ -270,29 +271,32 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision
// pose.
auto twist = (visionRobotPose - visionSample.value()).Log();
// 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.
wpi::math::Vectord<6> k_times_twist =
m_visionK * wpi::math::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.
wpi::math::Vectord<6> k_times_transform =
m_visionK * wpi::math::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{wpi::units::meter_t{k_times_twist(0)},
wpi::units::meter_t{k_times_twist(1)},
wpi::units::meter_t{k_times_twist(2)},
wpi::units::radian_t{k_times_twist(3)},
wpi::units::radian_t{k_times_twist(4)},
wpi::units::radian_t{k_times_twist(5)}};
// Step 6: Convert back to Transform3d.
Transform3d scaledTransform{
wpi::units::meter_t{k_times_transform(0)},
wpi::units::meter_t{k_times_transform(1)},
wpi::units::meter_t{k_times_transform(2)},
Rotation3d{wpi::units::radian_t{k_times_transform(3)},
wpi::units::radian_t{k_times_transform(4)},
wpi::units::radian_t{k_times_transform(5)}}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample.value() + scaledTwist.Exp(),
*odometrySample};
VisionUpdate visionUpdate{*visionSample + scaledTransform, *odometrySample};
m_visionUpdates[timestamp] = visionUpdate;
// Step 8: Remove later vision measurements. (Matches previous behavior)