diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
index 8db6a3540f..ff1e6f2fc0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
@@ -33,14 +33,6 @@ import java.util.Objects;
*
* {@link DifferentialDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as
* you want; if you never call it then this class will behave exactly like regular encoder odometry.
- *
- *
The state-space system used internally has the following states (x), and outputs (y):
- *
- *
x = [x, y, theta]ᵀ in the field coordinate system containing x position, y
- * position, and heading.
- *
- *
y = [x, y, theta]ᵀ from vision containing x position, y position, and
- * heading.
*/
public class DifferentialDrivePoseEstimator {
private final DifferentialDriveKinematics m_kinematics;
@@ -85,16 +77,16 @@ public class DifferentialDrivePoseEstimator {
* Constructs a DifferentialDrivePoseEstimator.
*
* @param kinematics A correctly-configured kinematics object for your drivetrain.
- * @param gyroAngle The current gyro angle.
+ * @param gyroAngle The gyro angle of the robot.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
- * @param initialPoseMeters The starting pose estimate.
- * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
- * model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
- * meters and radians.
- * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
- * numbers to trust global measurements from vision less. This matrix is in the form [x, y,
- * theta]ᵀ, with units in meters and radians.
+ * @param initialPoseMeters The estimated initial pose.
+ * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
+ * in meters, and heading in radians). Increase these numbers to trust your state estimate
+ * less.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+ * in meters, y position in meters, and heading in radians). Increase these numbers to trust
+ * the vision pose measurement less.
*/
public DifferentialDrivePoseEstimator(
DifferentialDriveKinematics kinematics,
@@ -188,10 +180,11 @@ public class DifferentialDrivePoseEstimator {
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link
- * DifferentialDrivePoseEstimator#updateWithTime} then you must use a timestamp with an epoch
- * since FPGA startup (i.e. the epoch of this timestamp is the same epoch as
- * Timer.getFPGATimestamp.) This means that you should use Timer.getFPGATimestamp as your time
- * source or sync the epochs.
+ * DifferentialDrivePoseEstimator#updateWithTime(double,Rotation2d,double,double)} then you
+ * must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
+ * the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that
+ * you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
+ * or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
// Step 1: Get the pose odometry measured at the moment the vision measurement was made.
@@ -249,13 +242,14 @@ public class DifferentialDrivePoseEstimator {
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link
- * DifferentialDrivePoseEstimator#updateWithTime} then you must use a timestamp with an epoch
- * since FPGA startup (i.e. the epoch of this timestamp is the same epoch as
- * Timer.getFPGATimestamp.) This means that you should use Timer.getFPGATimestamp as your time
- * source in this case.
- * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
- * numbers to trust global measurements from vision less. This matrix is in the form [x, y,
- * theta]ᵀ, with units in meters and radians.
+ * DifferentialDrivePoseEstimator#updateWithTime(double,Rotation2d,double,double)}, then you
+ * must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
+ * the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}. This means that
+ * you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
+ * in this case.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+ * in meters, y position in meters, and heading in radians). Increase these numbers to trust
+ * the vision pose measurement less.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
@@ -266,8 +260,8 @@ public class DifferentialDrivePoseEstimator {
}
/**
- * Updates the the Kalman Filter using only wheel encoder information. Note that this should be
- * called every loop.
+ * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+ * loop.
*
* @param gyroAngle The current gyro angle.
* @param distanceLeftMeters The total distance travelled by the left wheel in meters.
@@ -281,8 +275,8 @@ public class DifferentialDrivePoseEstimator {
}
/**
- * Updates the the Kalman Filter using only wheel encoder information. Note that this should be
- * called every loop.
+ * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+ * loop.
*
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param gyroAngle The current gyro angle.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
index 46b55aa2c5..4624746ed5 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
@@ -32,14 +32,6 @@ import java.util.Objects;
*
*
{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
* want; if you never call it, then this class will behave mostly like regular encoder odometry.
- *
- *
The state-space system used internally has the following states (x) and outputs (y):
- *
- *
x = [x, y, theta]ᵀ in the field coordinate system containing x position, y
- * position, and heading.
- *
- *
y = [x, y, theta]ᵀ from vision containing x position, y position, and
- * heading.
*/
public class MecanumDrivePoseEstimator {
private final MecanumDriveKinematics m_kinematics;
@@ -82,14 +74,14 @@ public class MecanumDrivePoseEstimator {
*
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
- * @param wheelPositions The distances driven by each wheel.
+ * @param wheelPositions The distance measured by each wheel.
* @param initialPoseMeters The starting pose estimate.
- * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
- * model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
- * meters and radians.
- * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
- * numbers to trust global measurements from vision less. This matrix is in the form [x, y,
- * theta]ᵀ, with units in meters and radians.
+ * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
+ * in meters, and heading in radians). Increase these numbers to trust your state estimate
+ * less.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+ * in meters, y position in meters, and heading in radians). Increase these numbers to trust
+ * the vision pose measurement less.
*/
public MecanumDrivePoseEstimator(
MecanumDriveKinematics kinematics,
@@ -175,10 +167,12 @@ public class MecanumDrivePoseEstimator {
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
- * don't use your own time source by calling {@link MecanumDrivePoseEstimator#updateWithTime}
- * then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
- * timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
- * Timer.getFPGATimestamp as your time source or sync the epochs.
+ * don't use your own time source by calling {@link
+ * MecanumDrivePoseEstimator#updateWithTime(double,Rotation2d,MecanumDriveWheelPositions)}
+ * then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
+ * timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.)
+ * This means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as
+ * your time source or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
// Step 1: Get the pose odometry measured at the moment the vision measurement was made.
@@ -230,13 +224,15 @@ public class MecanumDrivePoseEstimator {
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
- * don't use your own time source by calling {@link MecanumDrivePoseEstimator#updateWithTime}
- * then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
- * timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
- * Timer.getFPGATimestamp as your time source in this case.
- * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
- * numbers to trust global measurements from vision less. This matrix is in the form [x, y,
- * theta]ᵀ, with units in meters and radians.
+ * don't use your own time source by calling {@link
+ * MecanumDrivePoseEstimator#updateWithTime(double,Rotation2d,MecanumDriveWheelPositions)},
+ * then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
+ * timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}. This
+ * means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your
+ * time source in this case.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+ * in meters, y position in meters, and heading in radians). Increase these numbers to trust
+ * the vision pose measurement less.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
@@ -247,7 +243,7 @@ public class MecanumDrivePoseEstimator {
}
/**
- * Updates the Kalman Filter using only wheel encoder information. This should be called every
+ * Updates the pose estimator with wheel encoder and gyro information. This should be called every
* loop.
*
* @param gyroAngle The current gyro angle.
@@ -259,7 +255,7 @@ public class MecanumDrivePoseEstimator {
}
/**
- * Updates the Kalman Filter using only wheel encoder information. This should be called every
+ * Updates the pose estimator with wheel encoder and gyro information. This should be called every
* loop.
*
* @param currentTimeSeconds Time at which this method was called, in seconds.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
index bc01f28b4a..474b524db6 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
@@ -31,14 +31,6 @@ import java.util.Objects;
*
*
{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
* want; if you never call it, then this class will behave as regular encoder odometry.
- *
- *
The state-space system used internally has the following states (x) and outputs (y):
- *
- *
x = [x, y, theta]ᵀ in the field coordinate system containing x position, y
- * position, and heading.
- *
- *
y = [x, y, theta]ᵀ from vision containing x position, y position, and
- * heading.
*/
public class SwerveDrivePoseEstimator {
private final SwerveDriveKinematics m_kinematics;
@@ -82,14 +74,14 @@ public class SwerveDrivePoseEstimator {
*
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
- * @param modulePositions The current distance measurements and rotations of the swerve modules.
+ * @param modulePositions The current distance and rotation measurements of the swerve modules.
* @param initialPoseMeters The starting pose estimate.
- * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
- * model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
- * meters and radians.
- * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
- * numbers to trust global measurements from vision less. This matrix is in the form [x, y,
- * theta]ᵀ, with units in meters and radians.
+ * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
+ * in meters, and heading in radians). Increase these numbers to trust your state estimate
+ * less.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+ * in meters, y position in meters, and heading in radians). Increase these numbers to trust
+ * the vision pose measurement less.
*/
public SwerveDrivePoseEstimator(
SwerveDriveKinematics kinematics,
@@ -176,10 +168,12 @@ public class SwerveDrivePoseEstimator {
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
- * don't use your own time source by calling {@link SwerveDrivePoseEstimator#updateWithTime}
- * then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
- * timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
- * Timer.getFPGATimestamp as your time source or sync the epochs.
+ * don't use your own time source by calling {@link
+ * SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])} then you
+ * must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
+ * the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that
+ * you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
+ * or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
// Step 1: Get the pose odometry measured at the moment the vision measurement was made.
@@ -231,13 +225,15 @@ public class SwerveDrivePoseEstimator {
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
- * don't use your own time source by calling {@link SwerveDrivePoseEstimator#updateWithTime}
- * then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
- * timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
- * Timer.getFPGATimestamp as your time source in this case.
- * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
- * numbers to trust global measurements from vision less. This matrix is in the form [x, y,
- * theta]ᵀ, with units in meters and radians.
+ * don't use your own time source by calling {@link
+ * SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])}, then
+ * you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
+ * timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}. This
+ * means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your
+ * time source in this case.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+ * in meters, y position in meters, and heading in radians). Increase these numbers to trust
+ * the vision pose measurement less.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
@@ -248,7 +244,7 @@ public class SwerveDrivePoseEstimator {
}
/**
- * Updates the Kalman Filter using only wheel encoder information. This should be called every
+ * Updates the pose estimator with wheel encoder and gyro information. This should be called every
* loop.
*
* @param gyroAngle The current gyro angle.
@@ -260,7 +256,7 @@ public class SwerveDrivePoseEstimator {
}
/**
- * Updates the Kalman Filter using only wheel encoder information. This should be called every
+ * Updates the pose estimator with wheel encoder and gyro information. This should be called every
* loop.
*
* @param currentTimeSeconds Time at which this method was called, in seconds.
diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
index 31ba550855..339ccc98d3 100644
--- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
@@ -8,7 +8,6 @@
#include
#include "frc/EigenCore.h"
-#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
@@ -32,15 +31,6 @@ namespace frc {
*
* AddVisionMeasurement() can be called as infrequently as you want; if you
* never call it, then this class will behave like regular encoder odometry.
- *
- * The state-space system used internally has the following states (x) and
- * outputs (y):
- *
- * x = [x, y, theta]ᵀ in the field coordinate
- * system containing x position, y position, and heading.
- *
- * y = [x, y, theta]ᵀ from vision containing x position, y
- * position, and heading.
*/
class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
public:
@@ -53,12 +43,12 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
* The default standard deviations of the vision measurements are
* 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
*
- * @param kinematics A correctly-configured kinematics object
- * for your drivetrain.
- * @param gyroAngle The gyro angle of the robot.
+ * @param kinematics A correctly-configured kinematics object for your
+ * drivetrain.
+ * @param gyroAngle The gyro angle of the robot.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
- * @param initialPose The estimated initial pose.
+ * @param initialPose The estimated initial pose.
*/
DifferentialDrivePoseEstimator(DifferentialDriveKinematics& kinematics,
const Rotation2d& gyroAngle,
@@ -69,24 +59,19 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
/**
* Constructs a DifferentialDrivePoseEstimator.
*
- * @param kinematics A correctly-configured kinematics object
- * for your drivetrain.
- * @param gyroAngle The gyro angle of the robot.
+ * @param kinematics A correctly-configured kinematics object for your
+ * drivetrain.
+ * @param gyroAngle The gyro angle of the robot.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
- * @param initialPose The estimated initial pose.
- * @param stateStdDevs Standard deviations of model states.
- * Increase these numbers to trust your
- * model's state estimates less. This matrix
- * is in the form
- * [x, y, theta]ᵀ,
- * with units in meters and radians.
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from
- * vision less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
+ * @param initialPose The estimated initial pose.
+ * @param stateStdDevs Standard deviations of the pose estimate (x position in
+ * meters, y position in meters, and heading in radians). Increase these
+ * numbers to trust your state estimate less.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
*/
DifferentialDrivePoseEstimator(
DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
@@ -95,16 +80,14 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
const wpi::array& visionMeasurementStdDevs);
/**
- * Sets the pose estimator's trust of global measurements. This might be used
+ * Sets the pose estimator's trust in vision measurements. This might be used
* to change trust in vision measurements after the autonomous period, or to
* change trust as distance to a vision target increases.
*
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array& visionMeasurementStdDevs);
@@ -139,15 +122,13 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
* one meter or so of the current pose estimate.
*
* @param visionRobotPose The pose of the robot as measured by the vision
- * camera.
- * @param timestamp The timestamp of the vision measurement in seconds.
- * Note that if you don't use your own time source by
- * calling UpdateWithTime(), then you must use a
- * timestamp with an epoch since FPGA startup (i.e. the
- * epoch of this timestamp is the same epoch as
- * frc::Timer::GetFPGATimestamp(). This means that
- * you should use frc::Timer::GetFPGATimestamp() as
- * your time source in this case.
+ * camera.
+ * @param timestamp The timestamp of the vision measurement in seconds. Note
+ * that if you don't use your own time source by calling UpdateWithTime(),
+ * then you must use a timestamp with an epoch since FPGA startup (i.e.,
+ * the epoch of this timestamp is the same epoch as
+ * frc::Timer::GetFPGATimestamp(). This means that you should use
+ * frc::Timer::GetFPGATimestamp() as your time source in this case.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
@@ -167,25 +148,18 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
* method will continue to apply to future measurements until a subsequent
* call to SetVisionMeasurementStdDevs() or this method.
*
- * @param visionRobotPose The pose of the robot as measured by the
- * vision camera.
- * @param timestamp The timestamp of the vision measurement in
- * seconds. Note that if you don't use your
- * own time source by calling
- * UpdateWithTime(), then you must use a
- * timestamp with an epoch since FPGA startup
- * (i.e. the epoch of this timestamp is the
- * same epoch as
- * frc::Timer::GetFPGATimestamp(). This means
- * that you should use
- * frc::Timer::GetFPGATimestamp() as your
- * time source in this case.
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
+ * @param visionRobotPose The pose of the robot as measured by the vision
+ * camera.
+ * @param timestamp The timestamp of the vision measurement in seconds. Note
+ * that if you don't use your own time source by calling UpdateWithTime(),
+ * then you must use a timestamp with an epoch since FPGA startup (i.e.,
+ * the epoch of this timestamp is the same epoch as
+ * frc::Timer::GetFPGATimestamp(). This means that you should use
+ * frc::Timer::GetFPGATimestamp() as your time source in this case.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
@@ -195,8 +169,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
}
/**
- * Updates the Unscented Kalman Filter using only wheel encoder information.
- * Note that this should be called every loop iteration.
+ * Updates the pose estimator with wheel encoder and gyro information. This
+ * should be called every loop.
*
* @param gyroAngle The current gyro angle.
* @param leftDistance The distance traveled by the left encoder.
@@ -208,8 +182,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
units::meter_t rightDistance);
/**
- * Updates the Unscented Kalman Filter using only wheel encoder information.
- * Note that this should be called every loop iteration.
+ * Updates the pose estimator with wheel encoder and gyro information. This
+ * should be called every loop.
*
* @param currentTime The time at which this method was called.
* @param gyroAngle The current gyro angle.
diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
index d07d77095b..d1967e9db9 100644
--- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
@@ -10,7 +10,6 @@
#include
#include "frc/EigenCore.h"
-#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
@@ -32,15 +31,6 @@ namespace frc {
* AddVisionMeasurement() can be called as infrequently as you want; if you
* never call it, then this class will behave mostly like regular encoder
* odometry.
- *
- * The state-space system used internally has the following states (x) and
- * outputs (y):
- *
- * x = [x, y, theta]ᵀ in the field
- * coordinate system containing x position, y position, and heading.
- *
- * y = [x, y, theta]ᵀ from vision containing x position, y
- * position, and heading.
*/
class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
public:
@@ -53,11 +43,11 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
* The default standard deviations of the vision measurements are
* 0.45 meters for x, 0.45 meters for y, and 0.45 radians for heading.
*
- * @param kinematics A correctly-configured kinematics object
- * for your drivetrain.
- * @param gyroAngle The current gyro angle.
- * @param wheelPositions The distance measured by each wheel.
- * @param initialPose The starting pose estimate.
+ * @param kinematics A correctly-configured kinematics object for your
+ * drivetrain.
+ * @param gyroAngle The current gyro angle.
+ * @param wheelPositions The distance measured by each wheel.
+ * @param initialPose The starting pose estimate.
*/
MecanumDrivePoseEstimator(MecanumDriveKinematics& kinematics,
const Rotation2d& gyroAngle,
@@ -67,23 +57,18 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
/**
* Constructs a MecanumDrivePoseEstimator.
*
- * @param kinematics A correctly-configured kinematics object
- * for your drivetrain.
- * @param gyroAngle The current gyro angle.
- * @param wheelPositions The distance measured by each wheel.
- * @param initialPose The starting pose estimate.
-
- * @param stateStdDevs Standard deviations of model states.
- * Increase these numbers to trust your
- * model's state estimates less. This matrix
- * is in the form [x, y, theta]ᵀ, with units
- * in meters and radians.
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
+ * @param kinematics A correctly-configured kinematics object for your
+ * drivetrain.
+ * @param gyroAngle The current gyro angle.
+ * @param wheelPositions The distance measured by each wheel.
+ * @param initialPose The starting pose estimate.
+ * @param stateStdDevs Standard deviations of the pose estimate (x position in
+ * meters, y position in meters, and heading in radians). Increase these
+ * numbers to trust your state estimate less.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
*/
MecanumDrivePoseEstimator(
MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
@@ -92,16 +77,14 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
const wpi::array& visionMeasurementStdDevs);
/**
- * Sets the pose estimator's trust of global measurements. This might be used
+ * Sets the pose estimator's trust in vision measurements. This might be used
* to change trust in vision measurements after the autonomous period, or to
* change trust as distance to a vision target increases.
*
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array& visionMeasurementStdDevs);
@@ -139,15 +122,13 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
* one meter or so of the current pose estimate.
*
* @param visionRobotPose The pose of the robot as measured by the vision
- * camera.
- * @param timestamp The timestamp of the vision measurement in seconds.
- * Note that if you don't use your own time source by
- * calling UpdateWithTime() then you must use a
- * timestamp with an epoch since FPGA startup
- * (i.e. the epoch of this timestamp is the same
- * epoch as Timer#GetFPGATimestamp.) This means
- * that you should use Timer#GetFPGATimestamp as your
- * time source or sync the epochs.
+ * camera.
+ * @param timestamp The timestamp of the vision measurement in seconds. Note
+ * that if you don't use your own time source by calling UpdateWithTime()
+ * then you must use a timestamp with an epoch since FPGA startup (i.e.,
+ * the epoch of this timestamp is the same epoch as
+ * frc::Timer::GetFPGATimestamp().) This means that you should use
+ * frc::Timer::GetFPGATimestamp() as your time source or sync the epochs.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
@@ -167,25 +148,18 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
* method will continue to apply to future measurements until a subsequent
* call to SetVisionMeasurementStdDevs() or this method.
*
- * @param visionRobotPose The pose of the robot as measured by the
- * vision camera.
- * @param timestamp The timestamp of the vision measurement in
- * seconds. Note that if you don't use your
- * own time source by calling
- * UpdateWithTime(), then you must use a
- * timestamp with an epoch since FPGA startup
- * (i.e. the epoch of this timestamp is the
- * same epoch as
- * frc::Timer::GetFPGATimestamp(). This means
- * that you should use
- * frc::Timer::GetFPGATimestamp() as your
- * time source in this case.
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
+ * @param visionRobotPose The pose of the robot as measured by the vision
+ * camera.
+ * @param timestamp The timestamp of the vision measurement in seconds. Note
+ * that if you don't use your own time source by calling UpdateWithTime(),
+ * then you must use a timestamp with an epoch since FPGA startup (i.e.,
+ * the epoch of this timestamp is the same epoch as
+ * frc::Timer::GetFPGATimestamp(). This means that you should use
+ * frc::Timer::GetFPGATimestamp() as your time source in this case.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
@@ -195,8 +169,8 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
}
/**
- * Updates the the Kalman Filter using only wheel encoder
- * information. This should be called every loop.
+ * Updates the pose estimator with wheel encoder and gyro information. This
+ * should be called every loop.
*
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distances measured at each wheel.
@@ -206,8 +180,8 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
const MecanumDriveWheelPositions& wheelPositions);
/**
- * Updates the the Kalman Filter using only wheel encoder
- * information. This should be called every loop.
+ * Updates the pose estimator with wheel encoder and gyro information. This
+ * should be called every loop.
*
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyroscope angle.
diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
index f8d3461780..d07bafe5ed 100644
--- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
@@ -31,15 +31,6 @@ namespace frc {
* AddVisionMeasurement() can be called as infrequently as you want; if you
* never call it, then this class will behave as regular encoder
* odometry.
- *
- * The state-space system used internally has the following states (x) and
- * outputs (y):
- *
- * x = [x, y, theta]ᵀ in the field coordinate
- * system containing x position, y position, and heading.
- *
- * y = [x, y, theta]ᵀ from vision containing x position, y
- * position, and heading.
*/
template
class SwerveDrivePoseEstimator {
@@ -53,12 +44,12 @@ class SwerveDrivePoseEstimator {
* The default standard deviations of the vision measurements are
* 0.9 meters for x, 0.9 meters for y, and 0.9 radians for heading.
*
- * @param kinematics A correctly-configured kinematics object
- * for your drivetrain.
- * @param gyroAngle The current gyro angle.
- * @param modulePositions The current distance and rotation
- * measurements of the swerve modules.
- * @param initialPose The starting pose estimate.
+ * @param kinematics A correctly-configured kinematics object for your
+ * drivetrain.
+ * @param gyroAngle The current gyro angle.
+ * @param modulePositions The current distance and rotation measurements of
+ * the swerve modules.
+ * @param initialPose The starting pose estimate.
*/
SwerveDrivePoseEstimator(
SwerveDriveKinematics& kinematics,
@@ -72,23 +63,19 @@ class SwerveDrivePoseEstimator {
/**
* Constructs a SwerveDrivePoseEstimator.
*
- * @param kinematics A correctly-configured kinematics object
- * for your drivetrain.
- * @param gyroAngle The current gyro angle.
- * @param modulePositions The current distance and rotation
- * measurements of the swerve modules.
- * @param initialPose The starting pose estimate.
- * @param stateStdDevs Standard deviations of model states.
- * Increase these numbers to trust your
- * model's state estimates less. This matrix
- * is in the form [x, y, theta]ᵀ, with units
- * in meters and radians.
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
+ * @param kinematics A correctly-configured kinematics object for your
+ * drivetrain.
+ * @param gyroAngle The current gyro angle.
+ * @param modulePositions The current distance and rotation measurements of
+ * the swerve modules.
+ * @param initialPose The starting pose estimate.
+ * @param stateStdDevs Standard deviations of the pose estimate (x position in
+ * meters, y position in meters, and heading in radians). Increase these
+ * numbers to trust your state estimate less.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
*/
SwerveDrivePoseEstimator(
SwerveDriveKinematics& kinematics,
@@ -133,16 +120,14 @@ class SwerveDrivePoseEstimator {
Pose2d GetEstimatedPosition() const { return m_odometry.GetPose(); }
/**
- * Sets the pose estimator's trust of global measurements. This might be used
+ * Sets the pose estimator's trust in vision measurements. This might be used
* to change trust in vision measurements after the autonomous period, or to
* change trust as distance to a vision target increases.
*
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array& visionMeasurementStdDevs) {
@@ -175,15 +160,13 @@ class SwerveDrivePoseEstimator {
* one meter or so of the current pose estimate.
*
* @param visionRobotPose The pose of the robot as measured by the vision
- * camera.
- * @param timestamp The timestamp of the vision measurement in seconds.
- * Note that if you don't use your own time source by
- * calling UpdateWithTime() then you must use a
- * timestamp with an epoch since FPGA startup
- * (i.e. the epoch of this timestamp is the same
- * epoch as Timer#GetFPGATimestamp.) This means
- * that you should use Timer#GetFPGATimestamp as your
- * time source or sync the epochs.
+ * camera.
+ * @param timestamp The timestamp of the vision measurement in seconds. Note
+ * that if you don't use your own time source by calling UpdateWithTime()
+ * then you must use a timestamp with an epoch since FPGA startup (i.e.,
+ * the epoch of this timestamp is the same epoch as
+ * frc::Timer::GetFPGATimestamp().) This means that you should use
+ * frc::Timer::GetFPGATimestamp() as your time source or sync the epochs.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp) {
@@ -243,25 +226,18 @@ class SwerveDrivePoseEstimator {
* method will continue to apply to future measurements until a subsequent
* call to SetVisionMeasurementStdDevs() or this method.
*
- * @param visionRobotPose The pose of the robot as measured by the
- * vision camera.
- * @param timestamp The timestamp of the vision measurement in
- * seconds. Note that if you don't use your
- * own time source by calling
- * UpdateWithTime(), then you must use a
- * timestamp with an epoch since FPGA startup
- * (i.e. the epoch of this timestamp is the
- * same epoch as
- * frc::Timer::GetFPGATimestamp(). This means
- * that you should use
- * frc::Timer::GetFPGATimestamp() as your
- * time source in this case.
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
+ * @param visionRobotPose The pose of the robot as measured by the vision
+ * camera.
+ * @param timestamp The timestamp of the vision measurement in seconds. Note
+ * that if you don't use your own time source by calling UpdateWithTime(),
+ * then you must use a timestamp with an epoch since FPGA startup (i.e.,
+ * the epoch of this timestamp is the same epoch as
+ * frc::Timer::GetFPGATimestamp(). This means that you should use
+ * frc::Timer::GetFPGATimestamp() as your time source in this case.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
@@ -271,8 +247,8 @@ class SwerveDrivePoseEstimator {
}
/**
- * Updates the Kalman Filter using only wheel encoder information. This should
- * be called every loop.
+ * Updates the pose estimator with wheel encoder and gyro information. This
+ * should be called every loop.
*
* @param gyroAngle The current gyro angle.
* @param modulePositions The current distance and rotation measurements of
@@ -287,8 +263,8 @@ class SwerveDrivePoseEstimator {
}
/**
- * Updates the Kalman Filter using only wheel encoder information. This should
- * be called every loop.
+ * Updates the pose estimator with wheel encoder and gyro information. This
+ * should be called every loop.
*
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyro angle.