From b84644740dcc539b5ca07a6a2cff5e014ed0576e Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 29 Oct 2021 17:37:13 -0700 Subject: [PATCH] [wpimath] Document pose estimator states, inputs, and outputs (#3698) Fixes #3244. --- .../DifferentialDrivePoseEstimator.java | 23 +++++++++++-------- .../estimator/MecanumDrivePoseEstimator.java | 13 +++++++---- .../estimator/SwerveDrivePoseEstimator.java | 13 +++++++---- .../DifferentialDrivePoseEstimator.h | 21 ++++++++++------- .../frc/estimator/MecanumDrivePoseEstimator.h | 16 +++++++------ .../frc/estimator/SwerveDrivePoseEstimator.h | 15 +++++++----- 6 files changed, 61 insertions(+), 40 deletions(-) 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 6e0a231ac5..074362e359 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 @@ -34,18 +34,23 @@ import java.util.function.BiConsumer; * can be called as infrequently as you want; if you never call it then this class will behave * exactly like regular encoder odometry. * - *

Our state-space system is: + *

The state-space system used internally has the following states (x), inputs (u), and outputs + * (y): * - *

x = [[x, y, theta, dist_l, dist_r]]ᵀ in the field coordinate system (dist_* - * are wheel distances.) + *

x = [x, y, theta, dist_l, dist_r]ᵀ in the field coordinate system + * containing x position, y position, heading, left encoder distance, and right encoder distance. * - *

u = [[vx, vy, omega]]ᵀ (robot-relative velocities) -- NB: using velocities - * make things considerably easier, because it means that teams don't have to worry about getting an - * accurate model. Basically, we suspect that it's easier for teams to get good encoder data than it - * is for them to perform system identification well enough to get a good model. + *

u = [v_l, v_r, dtheta]ᵀ containing left wheel velocity, right wheel + * velocity, and change in gyro heading. * - *

y = [[x, y, theta]]ᵀ from vision, or y = [[dist_l, dist_r, theta]] - * from encoders and gyro. + *

NB: Using velocities make things considerably easier, because it means that teams don't have + * to worry about getting an accurate model. Basically, we suspect that it's easier for teams to get + * good encoder data than it is for them to perform system identification well enough to get a good + * model. + * + *

y = [x, y, theta]ᵀ from vision containing x position, y position, and + * heading; or y = [dist_l, dist_r, theta] containing left encoder position, right + * encoder position, and gyro heading. */ public class DifferentialDrivePoseEstimator { final UnscentedKalmanFilter m_observer; // Package-private to allow for unit testing 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 9053fb4e12..42a6adb002 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,17 @@ import java.util.function.BiConsumer; *

{@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. * - *

Our state-space system is: + *

The state-space system used internally has the following states (x), inputs (u), and outputs + * (y): * - *

x = [[x, y, theta]]ᵀ in the field-coordinate system. + *

x = [x, y, theta]ᵀ in the field coordinate system containing x position, y + * position, and heading. * - *

u = [[vx, vy, theta]]ᵀ in the field-coordinate system. + *

u = [v_l, v_r, dtheta]ᵀ containing left wheel velocity, right wheel + * velocity, and change in gyro heading. * - *

y = [[x, y, theta]]ᵀ in field coords from vision, or y = - * [[theta]]ᵀ from the gyro. + *

y = [x, y, theta]ᵀ from vision containing x position, y position, and + * heading; or y = [theta]ᵀ containing gyro heading. */ public class MecanumDrivePoseEstimator { private final UnscentedKalmanFilter m_observer; 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 3bb3edde5b..9e48728909 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 @@ -32,14 +32,17 @@ import java.util.function.BiConsumer; *

{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you * want; if you never call it, then this class will behave mostly like regular encoder odometry. * - *

Our state-space system is: + *

The state-space system used internally has the following states (x), inputs (u), and outputs + * (y): * - *

x = [[x, y, theta]]ᵀ in the field-coordinate system. + *

x = [x, y, theta]ᵀ in the field coordinate system containing x position, y + * position, and heading. * - *

u = [[vx, vy, omega]]ᵀ in the field-coordinate system. + *

u = [v_l, v_r, dtheta]ᵀ containing left wheel velocity, right wheel + * velocity, and change in gyro heading. * - *

y = [[x, y, theta]]ᵀ in field coords from vision, or y = - * [[theta]]ᵀ from the gyro. + *

y = [x, y, theta]ᵀ from vision containing x position, y position, and + * heading; or y = [theta]ᵀ containing gyro heading. */ public class SwerveDrivePoseEstimator { private final UnscentedKalmanFilter m_observer; diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index 698c623833..b957c1ec79 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -31,19 +31,24 @@ 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. * - * Our state-space system is: + * The state-space system used internally has the following states (x), inputs + * (u), and outputs (y): * - * x = [[x, y, theta, dist_l, dist_r]]ᵀ in the field - * coordinate system. + * x = [x, y, theta, dist_l, dist_r]ᵀ in the field coordinate + * system containing x position, y position, heading, left encoder distance, + * and right encoder distance. * - * u = [[d_l, d_r, dtheta]]ᵀ (robot-relative velocities) -- - * NB: using velocities make things considerably easier, because it means that + * u = [v_l, v_r, dtheta]ᵀ containing left wheel velocity, + * right wheel velocity, and change in gyro heading. + * + * NB: Using velocities make things considerably easier, because it means that * teams don't have to worry about getting an accurate model. Basically, we * suspect that it's easier for teams to get good encoder data than it is for - * them to perform system identification well enough to get a good model + * them to perform system identification well enough to get a good model. * - * y = [[x, y, theta]]ᵀ from vision, - * or y = [[dist_l, dist_r, theta]] from encoders and gyro. + * y = [x, y, theta]ᵀ from vision containing x position, y + * position, and heading; or y = [dist_l, dist_r, theta] + * containing left encoder position, right encoder position, and gyro heading. */ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator { public: diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index 76a31a0bf2..93c9f1e42b 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -32,16 +32,18 @@ namespace frc { * never call it, then this class will behave mostly like regular encoder * odometry. * - * Our state-space system is: + * The state-space system used internally has the following states (x), inputs + * (u), and outputs (y): * - * x = [[x, y, theta]]ᵀ in the - * field-coordinate system. + * x = [x, y, theta]ᵀ in the field coordinate system + * containing x position, y position, and heading. * - * u = [[vx, vy, omega]]ᵀ in the field-coordinate system. + * u = [v_l, v_r, dtheta]ᵀ containing left wheel velocity, + * right wheel velocity, and change in gyro heading. * - * y = [[x, y, theta]]ᵀ in field - * coords from vision, or y = [[theta]]ᵀ - * from the gyro. + * y = [x, y, theta]ᵀ from vision containing x position, y + * position, and heading; or y = [theta]ᵀ containing gyro + * heading. */ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator { public: diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 81260dfa86..91e0e50dbe 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -34,15 +34,18 @@ namespace frc { * never call it, then this class will behave mostly like regular encoder * odometry. * - * Our state-space system is: + * The state-space system used internally has the following states (x), inputs + * (u), and outputs (y): * - * x = [[x, y, theta]]ᵀ in the - * field-coordinate system. + * x = [x, y, theta]ᵀ in the field coordinate system + * containing x position, y position, and heading. * - * u = [[vx, vy, omega]]ᵀ in the field-coordinate system. + * u = [v_l, v_r, dtheta]ᵀ containing left wheel velocity, + * right wheel velocity, and change in gyro heading. * - * y = [[x, y, theta]]ᵀ in field coords from vision, - * or y = [[theta]]ᵀ from the gyro. + * y = [x, y, theta]ᵀ from vision containing x position, y + * position, and heading; or y = [theta]ᵀ containing gyro + * heading. */ template class SwerveDrivePoseEstimator {