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 4c31748329..4cae330248 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 @@ -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 { private final Odometry m_odometry; - private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1()); - private final Matrix 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 { 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 { * theta]ᵀ, with units in meters and radians. */ public final void setVisionMeasurementStdDevs(Matrix 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 { // 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 { 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()); } 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 ad0d1404b8..99feba9632 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 @@ -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 { private final Odometry3d m_odometry; - private final Matrix m_q = new Matrix<>(Nat.N4(), Nat.N1()); - private final Matrix 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 { 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 { * theta]ᵀ, with units in meters and radians. */ public final void setVisionMeasurementStdDevs(Matrix 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 { // 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 { 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()); } diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h index 9187889ff6..850ea1707f 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h @@ -86,6 +86,7 @@ class WPILIB_DLLEXPORT PoseEstimator { */ void SetVisionMeasurementStdDevs( const wpi::array& visionMeasurementStdDevs) { + // Diagonal of measurement covariance matrix R wpi::array 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& m_odometry; + + // Diagonal of process noise covariance matrix Q wpi::array m_q{wpi::empty_array}; - Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero(); + + // Kalman gain matrix K + Eigen::DiagonalMatrix m_vision_K = + Eigen::DiagonalMatrix::Zero(); // Maps timestamps to odometry-only pose estimates TimeInterpolatableBuffer m_odometryPoseBuffer{kBufferDuration}; diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h index 490a6cbcb2..6699e98d76 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h @@ -92,6 +92,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d { */ void SetVisionMeasurementStdDevs( const wpi::array& visionMeasurementStdDevs) { + // Diagonal of measurement noise covariance matrix R wpi::array 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& m_odometry; + + // Diagonal of process noise covariance matrix Q wpi::array m_q{wpi::empty_array}; - frc::Matrixd<6, 6> m_visionK = frc::Matrixd<6, 6>::Zero(); + + // Kalman gain matrix K + Eigen::DiagonalMatrix m_vision_K = + Eigen::DiagonalMatrix::Zero(); // Maps timestamps to odometry-only pose estimates TimeInterpolatableBuffer m_odometryPoseBuffer{kBufferDuration};