add vision pose ambiguity scalar

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

@@ -537,6 +537,9 @@ public static class VisionConstants {
     public static final int NUM_LAST_EST_POSES = 3;
     public static final double STD_DEV_SCALAR = 30;
 
+    public static final double MAX_ROBOT_VISION_VELOCITY = 2.0; // untuned 3/29
+    public static final double AMBIGUITY_MATRIX_SCALAR = 4.0; // untuned 3/29
+
     // --- vision subsystem ---
     // (camera setup)
     // DEFAULT CONSTANTS (robot specific constants are below) 

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

@@ -95,7 +95,7 @@ public boolean isMagnitudeGreaterThan(double threshold) {
   public final CommandSwerveDrivetrain m_drivetrain = TunerConstants.createDrivetrain();
   public final RollerSubsystem m_roller = new RollerSubsystem();
   public final ShooterSubsystem m_shooter = new ShooterSubsystem();
-  public final VisionSubsystem m_vision = new VisionSubsystem(() -> m_drivetrain.getState().Pose.getRotation());
+  public final VisionSubsystem m_vision = new VisionSubsystem(() -> m_drivetrain.getState());
   public final IndexerSubsystem m_indexer = new IndexerSubsystem();
   public final PivotSubsystem m_pivot = new PivotSubsystem();
   public final ColumnSubsystem m_column = new ColumnSubsystem();

src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java

@@ -12,6 +12,7 @@
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StructPublisher;
+import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
@@ -25,18 +26,21 @@
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
+import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
+
 import frc.robot.Constants.VisionConstants;
 
 public class LocalizationCamera {
 
   private final PhotonCamera m_camera;
   private final String m_cameraName;
   private final String m_logString;
+  private final boolean m_isFront; // boolean that stores camera location (front or back)
 
   private AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
   private final Field2d m_estPoseField = new Field2d(); // field pose estimator
   private PhotonPoseEstimator poseEstimator;
-  private final Supplier<Rotation2d> m_robotRotationSupplier;
+  private final Supplier<SwerveDriveState> m_robotSwerveStateSupplier;
 
   private LocalVisionFilterPipeline m_filterPipeline = new LocalVisionFilterPipeline(); // filtering pipeline for each camera, initalizes as empty pipeline
 
@@ -50,11 +54,19 @@ public class LocalizationCamera {
   // every camera periodically creates a new CameraReading containing robot pose, std dev, timestamp, and number of targets seen.
   public static record CameraReading(String cameraName, EstimatedRobotPose robotPose, Matrix<N3, N1> stdDevs, double timestampSeconds, int numTargets) {}
 
-  public LocalizationCamera(String cameraName, Transform3d robotToCam, Supplier<Rotation2d> robotHeadingSupplier) {
+  public LocalizationCamera(String cameraName, Transform3d robotToCam, Supplier<SwerveDriveState> robotSwerveStateSupplier) {
     m_cameraName = cameraName;
     m_camera = new PhotonCamera(m_cameraName);
     m_logString = "vision/" + m_cameraName;
-    m_robotRotationSupplier = robotHeadingSupplier;
+    m_robotSwerveStateSupplier = robotSwerveStateSupplier;
+
+    // boolean that stores camera location
+    if (m_cameraName.indexOf("front") == -1) {
+      m_isFront = false;
+    }
+    else {
+      m_isFront = true;
+    }
 
     poseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCam);
 
@@ -229,11 +241,18 @@ private Optional<EstimatedRobotPose> resolveHighAmbiguityPose(
     var bestPose = estimateSingleTagRobotPose(candidatePoseEstimator, target, target.getBestCameraToTarget());
     var alternatePose = estimateSingleTagRobotPose(candidatePoseEstimator, target, target.getAlternateCameraToTarget());
 
+    SwerveDriveState robotState = m_robotSwerveStateSupplier.get();
+
+    // null check for robot swerve state. If it's null, can't resolve heading.
+    if (robotState == null) {
+      return Optional.empty();
+    }
+
     if (bestPose.isEmpty() || alternatePose.isEmpty()) {
       return Optional.empty();
     }
 
-    Rotation2d currentHeading = m_robotRotationSupplier.get();
+    Rotation2d currentHeading = robotState.Pose.getRotation();
     Pose3d chosenPose = bestPose.get();
     // Prefer the candidate whose field-relative heading is closer to the drivetrain heading.
     if (headingDistance(alternatePose.get().toPose2d().getRotation(), currentHeading)
@@ -275,6 +294,8 @@ private Matrix<N3, N1> calculateEstimationStdDevs(EstimatedRobotPose estimatedPo
     // Pose present. Start running Heuristic
     int numTags = 0;
     double avgDist = 0;
+    double matrixScalar = 1.0;
+
 
     // Precalculation - see how many tags we found, and calculate an
     // average-distance metric
@@ -290,19 +311,32 @@ private Matrix<N3, N1> calculateEstimationStdDevs(EstimatedRobotPose estimatedPo
           .getDistance(estimatedPose.estimatedPose.toPose2d().getTranslation());
     }
 
+    // if robot is moving, want to trust front camera data less.
+    // NOTE: if state is null, matrixScalar stays at 1.0
+    SwerveDriveState robotState = m_robotSwerveStateSupplier.get();
+
+    if (robotState != null && robotState.Speeds != null) { // necessary null check
+      // if camera is in front and robot is above max velocity, update matrixScalar
+      double linearRobotSpeed = Math.hypot(robotState.Speeds.vxMetersPerSecond, 
+                                            robotState.Speeds.vyMetersPerSecond);
+      if (m_isFront && linearRobotSpeed > VisionConstants.MAX_ROBOT_VISION_VELOCITY) {
+        matrixScalar = VisionConstants.AMBIGUITY_MATRIX_SCALAR;
+      }
+    }
+
     if (numTags == 0) {
       // No tags visible. Default to single-tag std devs
-      SmartDashboard.putString(m_logString + "/standardDeviationState", "no tags visible");
-      return VisionConstants.kSingleTagStdDevs;
+      SmartDashboard.putString("vision/" + m_cameraName + "/standardDeviationState", "no tags visible");
+      return VisionConstants.kSingleTagStdDevs.times(matrixScalar);
     } else if (numTags == 1 && avgDist > VisionConstants.VISION_DISTANCE_DISCARD) {
       SmartDashboard.putString(m_logString + "/standardDeviationState", "target too far");
       return VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
     } else {
       var unscaledStdDevs = numTags > 1 ? VisionConstants.kMultiTagStdDevs : VisionConstants.kSingleTagStdDevs;
       avgDist /= numTags;
       // increase std devs based on (average) distance
-      SmartDashboard.putString(m_logString + "/standardDeviationState", "good :)");
-      return unscaledStdDevs.times(1 + (avgDist * avgDist / VisionConstants.STD_DEV_SCALAR));
+      SmartDashboard.putString("vision/" + m_cameraName + "/standardDeviationState", "good :)");
+      return unscaledStdDevs.times(1 + (avgDist * avgDist / VisionConstants.STD_DEV_SCALAR)).times(matrixScalar);
     }
   }
 }

src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java

@@ -22,6 +22,7 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -43,6 +44,7 @@
 
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
 
 import frc.robot.Constants;
 import frc.robot.Constants.VisionConstants;
@@ -65,11 +67,11 @@ public class VisionSubsystem extends SubsystemBase {
   private boolean m_rawVideoModeEnabled = false; // true = raw video feed, false = normal AprilTag processing
 
   /** Creates a new Vision Subsystem. */
-  public VisionSubsystem(Supplier<Rotation2d> robotHeadingSupplier) {
-    cameras.add(new LocalizationCamera(VisionConstants.CAMERA1_NAME, VisionConstants.ROBOT_TO_CAM1_3D, robotHeadingSupplier));
-    cameras.add(new LocalizationCamera(VisionConstants.CAMERA2_NAME, VisionConstants.ROBOT_TO_CAM2_3D, robotHeadingSupplier));
-    cameras.add(new LocalizationCamera(VisionConstants.CAMERA3_NAME, VisionConstants.ROBOT_TO_CAM3_3D, robotHeadingSupplier));
-    cameras.add(new LocalizationCamera(VisionConstants.CAMERA4_NAME, VisionConstants.ROBOT_TO_CAM4_3D, robotHeadingSupplier));
+  public VisionSubsystem(Supplier<SwerveDriveState> robotSwerveStateSupplier) {
+    cameras.add(new LocalizationCamera(VisionConstants.CAMERA1_NAME, VisionConstants.ROBOT_TO_CAM1_3D, robotSwerveStateSupplier));
+    cameras.add(new LocalizationCamera(VisionConstants.CAMERA2_NAME, VisionConstants.ROBOT_TO_CAM2_3D, robotSwerveStateSupplier));
+    cameras.add(new LocalizationCamera(VisionConstants.CAMERA3_NAME, VisionConstants.ROBOT_TO_CAM3_3D, robotSwerveStateSupplier));
+    cameras.add(new LocalizationCamera(VisionConstants.CAMERA4_NAME, VisionConstants.ROBOT_TO_CAM4_3D, robotSwerveStateSupplier));
   }
 
   public List<CameraReading> getValidCameraReadings(){

src/test/java/frc/robot/subsystems/vision/LocalizationCameraTest.java

@@ -10,13 +10,18 @@
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 import org.photonvision.EstimatedRobotPose;
 
+import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
+
 import frc.robot.Constants.VisionConstants;
 import frc.robot.subsystems.vision.LocalizationCamera;
 import frc.robot.subsystems.vision.LocalizationCamera.CameraReading;
@@ -26,26 +31,51 @@
 import java.lang.reflect.Field;
 import java.util.LinkedList;
 import java.util.List;
+import java.util.function.Supplier;
 
 class LocalizationCameraTest {
   static final double DELTA = 1e-5; // acceptable deviation range
   static final int NUM_TEST_EST_POSES = 10;
 
   LocalizationCamera m_camera;
   Transform3d m_robotToCam;
+  SwerveDriveState m_mockSwerveDriveState;
+
+  /**
+   * Creates a mock SwerveDriveState for testing purposes.
+   * Returns a state with zero pose and zero velocity.
+   */
+  private Supplier<SwerveDriveState> createMockSwerveDriveStateSupplier() {
+    m_mockSwerveDriveState = new SwerveDriveState();
+    m_mockSwerveDriveState.Pose = new Pose2d();
+    m_mockSwerveDriveState.Speeds = new ChassisSpeeds();
+    m_mockSwerveDriveState.ModuleStates = new SwerveModuleState[] {
+        new SwerveModuleState(), new SwerveModuleState(),
+        new SwerveModuleState(), new SwerveModuleState()
+    };
+    m_mockSwerveDriveState.ModuleTargets = new SwerveModuleState[] {
+        new SwerveModuleState(), new SwerveModuleState(),
+        new SwerveModuleState(), new SwerveModuleState()
+    };
+    m_mockSwerveDriveState.ModulePositions = new SwerveModulePosition[] {
+        new SwerveModulePosition(), new SwerveModulePosition(),
+        new SwerveModulePosition(), new SwerveModulePosition()
+    };
+    return () -> m_mockSwerveDriveState;
+  }
 
   @BeforeEach
   void setup() {
     // Initialize the HAL for simulation
     assert HAL.initialize(500, 0);
-    
+
     // Create a test camera transform
     m_robotToCam = new Transform3d(
         new Translation3d(0.2, 0.0, 0.5),
         new Rotation3d(0, Math.toRadians(-15), 0)
     );
-    
-    m_camera = new LocalizationCamera("test_camera", m_robotToCam, Rotation2d::new);
+
+    m_camera = new LocalizationCamera("test_camera", m_robotToCam, createMockSwerveDriveStateSupplier());
   }
 
   @AfterEach