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

[wpimath] Rewrite pose estimator docs (#4807)

Browse Source
This commit is contained in:
Tyler Veness
2022-12-12 20:30:52 -08:00
committed by GitHub
parent be39678447
commit 3270d4fc86
6 changed files with 208 additions and 298 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
View File

@@ -33,14 +33,6 @@ import java.util.Objects;
*
* <p>{@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.
*
* <p>The state-space system used internally has the following states (x), and outputs (y):
*
* <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
* position, and heading.
*
* <p><strong> y = [x, y, theta]ᵀ </strong> 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.

52
wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
View File

@@ -32,14 +32,6 @@ import java.util.Objects;
*
* <p>{@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.
*
* <p>The state-space system used internally has the following states (x) and outputs (y):
*
* <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
* position, and heading.
*
* <p><strong> y = [x, y, theta]ᵀ </strong> 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.

52
wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
View File

@@ -31,14 +31,6 @@ import java.util.Objects;
*
* <p>{@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.
*
* <p>The state-space system used internally has the following states (x) and outputs (y):
*
* <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
* position, and heading.
*
* <p><strong> y = [x, y, theta]ᵀ </strong> 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.