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

[wpimath] Speed up pose estimator correction computation (#8574)

Browse Source 
In C++, we use a diagonal matrix to avoid an expensive matrix
multiplication. EJML doesn't have a diagonal matrix type, so in Java, we
use a double array and implement the multiplication manually.
This commit is contained in:
Tyler Veness
2026-02-06 21:47:49 -08:00
committed by GitHub
parent 7cd3790c7c
commit 2c5529d714
4 changed files with 74 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -86,6 +86,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs) {
// Diagonal of measurement covariance matrix R
wpi::array<double, 3> r{wpi::empty_array};
for (size_t i = 0; i < 3; ++i) {
r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
@@ -95,9 +96,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
// and C = I. See wpimath/algorithms.md.
for (size_t row = 0; row < 3; ++row) {
if (m_q[row] == 0.0) {
m_visionK(row, row) = 0.0;
m_vision_K.diagonal()[row] = 0.0;
} else {
m_visionK(row, row) =
m_vision_K.diagonal()[row] =
m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
}
}
@@ -318,9 +319,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
// 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()};
m_vision_K * Eigen::Vector3d{transform.X().value(),
transform.Y().value(),
transform.Rotation().Radians().value()};
// Step 6: Convert back to Transform2d.
Transform2d scaledTransform{
@@ -475,8 +476,13 @@ class WPILIB_DLLEXPORT PoseEstimator {
static constexpr units::second_t kBufferDuration = 1.5_s;
Odometry<WheelSpeeds, WheelPositions>& m_odometry;
// Diagonal of process noise covariance matrix Q
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
// Kalman gain matrix K
Eigen::DiagonalMatrix<double, 3> m_vision_K =
Eigen::DiagonalMatrix<double, 3>::Zero();
// Maps timestamps to odometry-only pose estimates
TimeInterpolatableBuffer<Pose2d> m_odometryPoseBuffer{kBufferDuration};

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

@@ -92,6 +92,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 4>& visionMeasurementStdDevs) {
// Diagonal of measurement noise covariance matrix R
wpi::array<double, 4> r{wpi::empty_array};
for (size_t i = 0; i < 4; ++i) {
r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
@@ -101,15 +102,15 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
// and C = I. See wpimath/algorithms.md.
for (size_t row = 0; row < 4; ++row) {
if (m_q[row] == 0.0) {
m_visionK(row, row) = 0.0;
m_vision_K.diagonal()[row] = 0.0;
} else {
m_visionK(row, row) =
m_vision_K.diagonal()[row] =
m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
}
}
double angle_gain = m_visionK(3, 3);
m_visionK(4, 4) = angle_gain;
m_visionK(5, 5) = angle_gain;
double angle_gain = m_vision_K.diagonal()[3];
m_vision_K.diagonal()[4] = angle_gain;
m_vision_K.diagonal()[5] = angle_gain;
}
/**
@@ -327,12 +328,12 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
// 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()};
m_vision_K * 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 Transform3d.
Transform3d scaledTransform{
@@ -490,8 +491,13 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
static constexpr units::second_t kBufferDuration = 1.5_s;
Odometry3d<WheelSpeeds, WheelPositions>& m_odometry;
// Diagonal of process noise covariance matrix Q
wpi::array<double, 4> m_q{wpi::empty_array};
frc::Matrixd<6, 6> m_visionK = frc::Matrixd<6, 6>::Zero();
// Kalman gain matrix K
Eigen::DiagonalMatrix<double, 6> m_vision_K =
Eigen::DiagonalMatrix<double, 6>::Zero();
// Maps timestamps to odometry-only pose estimates
TimeInterpolatableBuffer<Pose3d> m_odometryPoseBuffer{kBufferDuration};