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] 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

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

@@ -8,8 +8,6 @@ import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
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;
@@ -40,8 +38,12 @@ import java.util.TreeMap;
*/
public class PoseEstimator<T> {
private final Odometry<T> m_odometry;
private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
private final Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
// Diagonal of process noise covariance matrix Q
private final double[] m_q = new double[] {0.0, 0.0, 0.0};
// Diagonal of Kalman gain matrix K
private final double[] m_vision_k = new double[] {0.0, 0.0, 0.0};
private static final double kBufferDuration = 1.5;
// Maps timestamps to odometry-only pose estimates
@@ -79,7 +81,7 @@ public class PoseEstimator<T> {
m_poseEstimate = m_odometry.getPoseMeters();
for (int i = 0; i < 3; ++i) {
m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
m_q[i] = stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0);
}
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_PoseEstimator, 1);
@@ -95,6 +97,7 @@ public class PoseEstimator<T> {
* theta]ᵀ, with units in meters and radians.
*/
public final void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
// Diagonal of measurement noise covariance matrix R
var r = new double[3];
for (int i = 0; i < 3; ++i) {
r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
@@ -103,11 +106,10 @@ public class PoseEstimator<T> {
// Solve for closed form Kalman gain for continuous Kalman filter with A = 0
// and C = I. See wpimath/algorithms.md.
for (int row = 0; row < 3; ++row) {
if (m_q.get(row, 0) == 0.0) {
m_visionK.set(row, row, 0.0);
if (m_q[row] == 0.0) {
m_vision_k[row] = 0.0;
} else {
m_visionK.set(
row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
m_vision_k[row] = m_q[row] / (m_q[row] + Math.sqrt(m_q[row] * r[row]));
}
}
}
@@ -307,29 +309,23 @@ public class PoseEstimator<T> {
var transform = visionRobotPoseMeters.minus(visionSample.get());
// 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_transform =
m_visionK.times(
VecBuilder.fill(
transform.getX(), transform.getY(), transform.getRotation().getRadians()));
// Step 6: Convert back to Transform2d.
// Kalman gain matrix representing how much we trust vision measurements compared to our current
// pose. Then, we convert the result back to a 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)));
m_vision_k[0] * transform.getX(),
m_vision_k[1] * transform.getY(),
Rotation2d.fromRadians(m_vision_k[2] * transform.getRotation().getRadians()));
// Step 7: Calculate and record the vision update.
// Step 6: Calculate and record the vision update.
var visionUpdate =
new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get());
m_visionUpdates.put(timestampSeconds, visionUpdate);
// Step 8: Remove later vision measurements. (Matches previous behavior)
// Step 7: Remove later vision measurements. (Matches previous behavior)
m_visionUpdates.tailMap(timestampSeconds, false).entrySet().clear();
// Step 9: Update latest pose estimate. Since we cleared all updates after this vision update,
// Step 8: Update latest pose estimate. Since we cleared all updates after this vision update,
// it's guaranteed to be the latest vision update.
m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
}

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

@@ -8,8 +8,6 @@ import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
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.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
@@ -22,7 +20,6 @@ import edu.wpi.first.math.kinematics.Kinematics;
import edu.wpi.first.math.kinematics.Odometry3d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N4;
import edu.wpi.first.math.numbers.N6;
import java.util.NavigableMap;
import java.util.Optional;
import java.util.TreeMap;
@@ -48,8 +45,12 @@ import java.util.TreeMap;
*/
public class PoseEstimator3d<T> {
private final Odometry3d<T> m_odometry;
private final Matrix<N4, N1> m_q = new Matrix<>(Nat.N4(), Nat.N1());
private final Matrix<N6, N6> m_visionK = new Matrix<>(Nat.N6(), Nat.N6());
// Diagonal of process noise covariance matrix Q
private final double[] m_q = new double[] {0.0, 0.0, 0.0, 0.0};
// Diagonal of Kalman gain matrix K
private final double[] m_vision_k = new double[] {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
private static final double kBufferDuration = 1.5;
// Maps timestamps to odometry-only pose estimates
@@ -87,7 +88,7 @@ public class PoseEstimator3d<T> {
m_poseEstimate = m_odometry.getPoseMeters();
for (int i = 0; i < 4; ++i) {
m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
m_q[i] = stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0);
}
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_PoseEstimator3d, 1);
@@ -103,6 +104,7 @@ public class PoseEstimator3d<T> {
* theta]ᵀ, with units in meters and radians.
*/
public final void setVisionMeasurementStdDevs(Matrix<N4, N1> visionMeasurementStdDevs) {
// Diagonal of measurement covariance matrix R
var r = new double[4];
for (int i = 0; i < 4; ++i) {
r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
@@ -111,17 +113,16 @@ public class PoseEstimator3d<T> {
// Solve for closed form Kalman gain for continuous Kalman filter with A = 0
// and C = I. See wpimath/algorithms.md.
for (int row = 0; row < 4; ++row) {
if (m_q.get(row, 0) == 0.0) {
m_visionK.set(row, row, 0.0);
if (m_q[row] == 0.0) {
m_vision_k[row] = 0.0;
} else {
m_visionK.set(
row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
m_vision_k[row] = m_q[row] / (m_q[row] + Math.sqrt(m_q[row] * r[row]));
}
}
// Fill in the gains for the other components of the rotation vector
double angle_gain = m_visionK.get(3, 3);
m_visionK.set(4, 4, angle_gain);
m_visionK.set(5, 5, angle_gain);
double angle_gain = m_vision_k[3];
m_vision_k[4] = angle_gain;
m_vision_k[5] = angle_gain;
}
/**
@@ -320,38 +321,27 @@ public class PoseEstimator3d<T> {
var transform = visionRobotPoseMeters.minus(visionSample.get());
// 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_transform =
m_visionK.times(
VecBuilder.fill(
transform.getX(),
transform.getY(),
transform.getZ(),
transform.getRotation().getX(),
transform.getRotation().getY(),
transform.getRotation().getZ()));
// Step 6: Convert back to Transform3d.
// Kalman gain matrix representing how much we trust vision measurements compared to our current
// pose. Then we convert the result back to a Transform3d.
var scaledTransform =
new Transform3d(
k_times_transform.get(0, 0),
k_times_transform.get(1, 0),
k_times_transform.get(2, 0),
m_vision_k[0] * transform.getX(),
m_vision_k[1] * transform.getY(),
m_vision_k[2] * transform.getZ(),
new Rotation3d(
k_times_transform.get(3, 0),
k_times_transform.get(4, 0),
k_times_transform.get(5, 0)));
m_vision_k[3] * transform.getRotation().getX(),
m_vision_k[4] * transform.getRotation().getY(),
m_vision_k[5] * transform.getRotation().getZ()));
// Step 7: Calculate and record the vision update.
// Step 6: Calculate and record the vision update.
var visionUpdate =
new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get());
m_visionUpdates.put(timestampSeconds, visionUpdate);
// Step 8: Remove later vision measurements. (Matches previous behavior)
// Step 7: Remove later vision measurements. (Matches previous behavior)
m_visionUpdates.tailMap(timestampSeconds, false).entrySet().clear();
// Step 9: Update latest pose estimate. Since we cleared all updates after this vision update,
// Step 8: Update latest pose estimate. Since we cleared all updates after this vision update,
// it's guaranteed to be the latest vision update.
m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
}

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};