Proposed LL pose filtering. Not tested.

src/main/java/frc/robot/Robot.java

@@ -11,6 +11,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.constants.Constants;
 
 public class Robot extends TimedRobot {
   private Command autoCommand;
@@ -28,7 +29,7 @@ public void robotInit() {
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
-    robotContainer.updatePoseVision();
+    robotContainer.updatePoseVision(Constants.Vision.SPEAKER_LIMELIGHT);
   }
 
   @Override

src/main/java/frc/robot/RobotContainer.java

@@ -16,6 +16,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -521,41 +522,55 @@ public Pose2d getPose() {
     return DRIVETRAIN.getPose();
   }
 
-  public void updatePoseVision() {
+  // Reference FRC 6391
+  // https://github.com/6391-Ursuline-Bearbotics/2024-6391-Crescendo/blob/4759dfd37c960cf3493b0eafd901519c5c36b239/src/main/java/frc/robot/Vision/Limelight.java#L44-L88
+  public void updatePoseVision(final String limelight) {
     ChassisSpeeds currentSpeed = DRIVETRAIN.getCurrentRobotChassisSpeeds();
-    if (Math.abs(currentSpeed.vxMetersPerSecond) > 1.5
-        || Math.abs(currentSpeed.vyMetersPerSecond) > 1.5) {
+    // Reject pose updates when moving fast.
+    if (Math.abs(currentSpeed.vxMetersPerSecond) > 2.0
+        || Math.abs(currentSpeed.vyMetersPerSecond) > 2.0
+        || Math.abs(currentSpeed.omegaRadiansPerSecond) > Math.PI) {
       return;
     }
-    Limelight.PoseEstimate pose = Limelight.getBotPoseEstimate_wpiBlue(SPEAKER_LIMELIGHT);
-    // TODO throw away any botpose that is far away from previous pose
-    // Sometimes teleports when the ll gets confused
-    // Usually at a longer distance, could filter out long distance from tags
-    if (pose.tagCount >= 2) {
+    Limelight.PoseEstimate botPose = Limelight.getBotPoseEstimate_wpiBlue(limelight);
+
+    // Reject an empty pose.
+    if (botPose.tagCount < 1) {
+      return;
+    }
+    // Reject a pose outside of the field.
+    if (!Constants.Vision.fieldBoundary.isPoseWithinArea(botPose.pose)) {
+      return;
+    }
+    // Reject pose from long disance.
+    if ((botPose.tagCount == 1
+            && botPose.avgTagDist > Constants.Vision.maxSingleTagDistanceToAccept)
+        || (botPose.tagCount >= 2
+            && botPose.avgTagDist > Constants.Vision.maxMutiTagDistToAccept)) {
+      return;
+    }
+    // Trust close multi tag pose when disabled with increased confidence.
+    if (DriverStation.isDisabled()
+        && botPose.tagCount >= 2
+        && botPose.avgTagDist < Constants.Vision.maxTagDistToTrust) {
       DRIVETRAIN.addVisionMeasurement(
-          pose.pose, pose.timestampSeconds, VecBuilder.fill(.7, .7, .7));
+          botPose.pose, botPose.timestampSeconds, Constants.Vision.absoluteTrustVector);
+      return;
+    }
+    final double botPoseToPoseDistance =
+        botPose.pose.getTranslation().getDistance(DRIVETRAIN.getPose().getTranslation());
+    // Reject a pose that is far away from the current robot pose.
+    if (botPoseToPoseDistance > 0.5) {
+      return;
+    }
+    double tagDistanceFeet = Units.metersToFeet(botPose.avgTagDist);
+    // Have a lower confidence with single tag pose proportionate to distance.
+    if (botPose.tagCount == 1) {
+      tagDistanceFeet *= 2;
     }
-    // Limelight.PoseEstimate poseAmp = Limelight.getBotPoseEstimate_wpiBlue(AMP_LIMELIGHT);
-    // if (poseAmp.tagCount >= 2) {
-    //   DRIVETRAIN.addVisionMeasurement(
-    //       poseAmp.pose, poseAmp.timestampSeconds, VecBuilder.fill(.9, .9, .9));
-    // }
-    // Limelight.PoseEstimate poseRight = Limelight.getBotPoseEstimate_wpiBlue(RIGHT_LIMEKIGHT);
-    // if (poseRight.tagCount >= 2) {
-    //   DRIVETRAIN.addVisionMeasurement(
-    //       poseRight.pose, poseRight.timestampSeconds, VecBuilder.fill(1.5, 1.5,1.5));
-    // } else {
-    //   if (pose.rawFiducials.length > 0 && pose.rawFiducials[0].ambiguity < 0.07) {
-    //     DriverStation.reportWarning(
-    //         "ADDING POSE BASED ON AMBIGUITY OF "
-    //             + pose.rawFiducials[0].ambiguity
-    //             + " ON TAG "
-    //             + pose.rawFiducials[0].id,
-    //         false);
-    //     DRIVETRAIN.addVisionMeasurement(
-    //         pose.pose, pose.timestampSeconds, VecBuilder.fill(.7, .7, 9999999));
-    //   }
-    // }
+    double confidence = 0.7 + (tagDistanceFeet / 100);
+    DRIVETRAIN.addVisionMeasurement(
+        botPose.pose, botPose.timestampSeconds, VecBuilder.fill(confidence, confidence, 99));
   }
 
   public static RobotContainer get() {

src/main/java/frc/robot/constants/Constants.java

@@ -1,7 +1,13 @@
 package frc.robot.constants;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
 import frc.robot.util.InterpolatingDouble;
 import frc.robot.util.InterpolatingTreeMap;
+import frc.robot.util.RectanglePoseArea;
 
 public class Constants {
 
@@ -297,4 +303,14 @@ public static class Climber {
     // DISTANCE_WRIST_ANGLE_MAP_NONELEVATOR.put(
     //     new InterpolatingDouble(-14.3), new InterpolatingDouble(28.5)); //
   }
+
+  public static class Vision {
+    public static final String SPEAKER_LIMELIGHT = "limelight-speaker";
+    public static final RectanglePoseArea fieldBoundary =
+        new RectanglePoseArea(new Translation2d(0, 0), new Translation2d(16.541, 8.211));
+    public static final double maxMutiTagDistToAccept = Units.feetToMeters(15.0);
+    public static final double maxTagDistToTrust = Units.feetToMeters(10.0);
+    public static final double maxSingleTagDistanceToAccept = Units.feetToMeters(10.0);
+    public static final Vector<N3> absoluteTrustVector = VecBuilder.fill(.2, .2, 99);
+  }
 }

src/main/java/frc/robot/util/RectanglePoseArea.java

@@ -0,0 +1,51 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+public class RectanglePoseArea {
+  private final Translation2d bottomLeft;
+  private final Translation2d topRight;
+
+  /**
+   * Create a 2D rectangular area for pose calculations.
+   *
+   * @param bottomLeft bottom left corner of the rectangle.
+   * @param topRight top right corner of the rectangle.
+   */
+  public RectanglePoseArea(Translation2d bottomLeft, Translation2d topRight) {
+    this.bottomLeft = bottomLeft;
+    this.topRight = topRight;
+  }
+
+  public double getMinX() {
+    return bottomLeft.getX();
+  }
+
+  public double getMaxX() {
+    return topRight.getX();
+  }
+
+  public double getMinY() {
+    return bottomLeft.getY();
+  }
+
+  public double getMaxY() {
+    return topRight.getY();
+  }
+
+  public Translation2d getBottomLeftPoint() {
+    return bottomLeft;
+  }
+
+  public Translation2d getTopRightPoint() {
+    return topRight;
+  }
+
+  public boolean isPoseWithinArea(Pose2d pose) {
+    return pose.getX() >= bottomLeft.getX()
+        && pose.getX() <= topRight.getX()
+        && pose.getY() >= bottomLeft.getY()
+        && pose.getY() <= topRight.getY();
+  }
+}