[wpimath] Document pose estimator states, inputs, and outputs (#3698)

Fixes #3244.

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.
  *
- * <p>Our state-space system is:
+ * <p>The state-space system used internally has the following states (x), inputs (u), and outputs
+ * (y):
  *
- * <p><strong> x = [[x, y, theta, dist_l, dist_r]]ᵀ </strong> in the field coordinate system (dist_*
- * are wheel distances.)
+ * <p><strong> x = [x, y, theta, dist_l, dist_r]ᵀ </strong> in the field coordinate system
+ * containing x position, y position, heading, left encoder distance, and right encoder distance.
  *
- * <p><strong> u = [[vx, vy, omega]]ᵀ </strong> (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.
+ * <p><strong> u = [v_l, v_r, dtheta]ᵀ </strong> containing left wheel velocity, right wheel
+ * velocity, and change in gyro heading.
  *
- * <p><strong>y = [[x, y, theta]]ᵀ </strong> from vision, or <strong>y = [[dist_l, dist_r, theta]]
- * </strong> from encoders and gyro.
+ * <p>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.
+ *
+ * <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
+ * heading; or <strong>y = [dist_l, dist_r, theta] </strong> containing left encoder position, right
+ * encoder position, and gyro heading.
  */
 public class DifferentialDrivePoseEstimator {
   final UnscentedKalmanFilter<N5, N3, N3> m_observer; // Package-private to allow for unit testing

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

@@ -32,14 +32,17 @@ import java.util.function.BiConsumer;
  * <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>Our state-space system is:
+ * <p>The state-space system used internally has the following states (x), inputs (u), and outputs
+ * (y):
  *
- * <p><strong> x = [[x, y, theta]]ᵀ </strong> in the field-coordinate system.
+ * <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
+ * position, and heading.
  *
- * <p><strong> u = [[vx, vy, theta]]ᵀ </strong> in the field-coordinate system.
+ * <p><strong> u = [v_l, v_r, dtheta]ᵀ </strong> containing left wheel velocity, right wheel
+ * velocity, and change in gyro heading.
  *
- * <p><strong> y = [[x, y, theta]]ᵀ </strong> in field coords from vision, or <strong> y =
- * [[theta]]ᵀ </strong> from the gyro.
+ * <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
+ * heading; or <strong> y = [theta]ᵀ </strong> containing gyro heading.
  */
 public class MecanumDrivePoseEstimator {
   private final UnscentedKalmanFilter<N3, N3, N1> m_observer;

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

@@ -32,14 +32,17 @@ import java.util.function.BiConsumer;
  * <p>{@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.
  *
- * <p>Our state-space system is:
+ * <p>The state-space system used internally has the following states (x), inputs (u), and outputs
+ * (y):
  *
- * <p><strong> x = [[x, y, theta]]ᵀ </strong> in the field-coordinate system.
+ * <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
+ * position, and heading.
  *
- * <p><strong> u = [[vx, vy, omega]]ᵀ </strong> in the field-coordinate system.
+ * <p><strong> u = [v_l, v_r, dtheta]ᵀ </strong> containing left wheel velocity, right wheel
+ * velocity, and change in gyro heading.
  *
- * <p><strong> y = [[x, y, theta]]ᵀ </strong> in field coords from vision, or <strong> y =
- * [[theta]]ᵀ </strong> from the gyro.
+ * <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
+ * heading; or <strong> y = [theta]ᵀ </strong> containing gyro heading.
  */
 public class SwerveDrivePoseEstimator {
   private final UnscentedKalmanFilter<N3, N3, N1> m_observer;