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 e40f74ee2e..de2d31c7fe 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 @@ -49,7 +49,8 @@ import java.util.function.BiConsumer; * left, front right, rear left, and rear right wheels. * *

y = [x, y, theta]ᵀ from vision containing x position, y position, and - * heading; or y = [theta]ᵀ containing gyro heading. + * heading; or y = [theta, s_fl, s_fr, s_rl, s_rr]ᵀ containing gyro heading, + * followed by the distance driven by the front left, front right, rear left, and rear right wheels. */ 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 09cd6ec21b..6e2b1350ba 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 @@ -43,14 +43,15 @@ import java.util.function.BiConsumer; *

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

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

x = [x, y, theta, s_0, ..., s_n]ᵀ in the field coordinate system containing * x position, y position, and heading, followed by the distance travelled by each wheel. * *

u = [v_x, v_y, omega, v_0, ... v_n]ᵀ containing x velocity, y velocity, and * angular rate in the field coordinate system, followed by the velocity measured at each wheel. * *

y = [x, y, theta]ᵀ from vision containing x position, y position, and - * heading; or y = [theta]ᵀ containing gyro heading. + * heading; or y = [theta, s_0, ..., s_n]ᵀ containing gyro heading, followed by + * the distance travelled by each wheel. */ public class SwerveDrivePoseEstimator { private final UnscentedKalmanFilter m_observer; diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index 44e40a9afa..1a072dcde7 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -46,8 +46,9 @@ namespace frc { * right wheels. * * y = [x, y, theta]ᵀ from vision containing x position, y - * position, and heading; or y = [theta]ᵀ containing gyro - * heading. + * position, and heading; or y = [theta, s_fl, s_fr, s_rl, s_rr]ᵀ + * containing gyro heading, followed by the distance driven by the + * front left, front right, rear left, and rear right wheels. */ 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 2df255d007..327ca0a779 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -38,7 +38,7 @@ namespace frc { * The state-space system used internally has the following states (x), inputs * (u), and outputs (y): * - * x = [x, y, theta, s_0, ... s_n]ᵀ in the field coordinate + * x = [x, y, theta, s_0, ..., s_n]ᵀ in the field coordinate * system containing x position, y position, and heading, followed by the * distance travelled by each wheel. * @@ -47,8 +47,8 @@ namespace frc { * followed by the velocity measured at each wheel. * * y = [x, y, theta]ᵀ from vision containing x position, y - * position, and heading; or y = [theta]ᵀ containing gyro - * heading. + * position, and heading; or y = [theta, s_0, ..., s_n]ᵀ + * containing gyro heading, followed by the distance travelled by each wheel. */ template class SwerveDrivePoseEstimator {