Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +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.

112
wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
View File

@@ -8,7 +8,6 @@
#include <wpi/array.h>
#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):
*
* <strong> x = [x, y, theta]ᵀ </strong> in the field coordinate
* system containing x position, y position, and heading.
*
* <strong> y = [x, y, theta]ᵀ </strong> 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<double, 3>& 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<double, 3>& 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.

116
wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
View File

@@ -10,7 +10,6 @@
#include <wpi/array.h>
#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):
*
* <strong> x = [x, y, theta]ᵀ </strong> in the field
* coordinate system containing x position, y position, and heading.
*
* <strong> y = [x, y, theta]ᵀ </strong> 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<double, 3>& 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<double, 3>& 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.

118
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

@@ -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):
*
* <strong> x = [x, y, theta]ᵀ </strong> in the field coordinate
* system containing x position, y position, and heading.
*
* <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
* position, and heading.
*/
template <size_t NumModules>
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<NumModules>& 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<NumModules>& 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<double, 3>& 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.