diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
index 71a57110..35198a76 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
@@ -18,7 +18,6 @@
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PathPlannerLogging;
 import com.pathplanner.lib.util.ReplanningConfig;
-import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -28,8 +27,6 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -45,7 +42,6 @@
 import java.util.Optional;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
-import java.util.stream.Collectors;
 import java.util.stream.Stream;
 
 import frc.robot.util.PoseEstimator;
@@ -68,7 +64,6 @@ public class DriveSubsystem extends SubsystemBase {
   private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
   private final PoseEstimator m_poseEstimator;
   private Pose2d pose = new Pose2d();
-  private Pose2d visionPose = new Pose2d();
   private Rotation2d lastGyroRotation = new Rotation2d();
 
   private final VisionSubsystem[] m_cameras;
@@ -186,7 +181,7 @@ public void periodic() {
             .flatMap((VisionSubsystem camera) -> Stream.of(camera.getPose(m_poseEstimator.getLatestPose())))
             .filter(Optional::isPresent)
             .flatMap(update -> Stream.of(update.get()))
-            .collect(Collectors.toList()));
+            .toList());
   }
 
   /**
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index 1fa4d1e1..cde52a21 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -12,10 +12,7 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.util.PoseEstimator;
-import org.littletonrobotics.junction.AutoLog;
 import org.littletonrobotics.junction.Logger;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
@@ -30,7 +27,6 @@ public class VisionSubsystem {
   private final PhotonCamera m_camera;
   private final Transform3d robotToCam;
   private PhotonPoseEstimator m_photonPoseEstimator;
-  private DriverStation.Alliance m_alliance;
   private AprilTagFieldLayout m_aprilTagFieldLayout;
   private final String m_name;
 
diff --git a/src/main/java/frc/robot/util/PoseEstimator.java b/src/main/java/frc/robot/util/PoseEstimator.java
index ec3de406..61c9baec 100644
--- a/src/main/java/frc/robot/util/PoseEstimator.java
+++ b/src/main/java/frc/robot/util/PoseEstimator.java
@@ -23,7 +23,7 @@
 import lib.GeomUtil;
 
 public class PoseEstimator {
-  private static final double historyLengthSecs = 0.3;
+  private static final double HISTORY_LENGTH_SECS = 0.3;
 
   private Pose2d basePose = new Pose2d();
   private Pose2d latestPose = new Pose2d();
@@ -104,7 +104,7 @@ public void addVisionData(List<TimestampedVisionUpdate> visionData) {
   private void update() {
     // Clear old data and update base pose
     while (updates.size() > 1
-        && updates.firstKey() < Timer.getFPGATimestamp() - historyLengthSecs) {
+        && updates.firstKey() < Timer.getFPGATimestamp() - HISTORY_LENGTH_SECS) {
       var update = updates.pollFirstEntry();
       basePose = update.getValue().apply(basePose, q);
     }
@@ -163,11 +163,9 @@ public Pose2d apply(Pose2d lastPose, Matrix<N3, N1> q) {
   /** Represents a single vision pose with associated standard deviations. */
   public record VisionUpdate(Pose2d pose, Matrix<N3, N1> stdDevs) {
     public static final Comparator<VisionUpdate> compareDescStdDev =
-        (VisionUpdate a, VisionUpdate b) -> {
-          return -Double.compare(
-              a.stdDevs().get(0, 0) + a.stdDevs().get(1, 0),
-              b.stdDevs().get(0, 0) + b.stdDevs().get(1, 0));
-        };
+        (VisionUpdate a, VisionUpdate b) -> -Double.compare(
+            a.stdDevs().get(0, 0) + a.stdDevs().get(1, 0),
+            b.stdDevs().get(0, 0) + b.stdDevs().get(1, 0));
   }
 
   /** Represents a single vision pose with a timestamp and associated standard deviations. */
diff --git a/src/main/java/lib/GeomUtil.java b/src/main/java/lib/GeomUtil.java
index 21e0957f..ea570e48 100644
--- a/src/main/java/lib/GeomUtil.java
+++ b/src/main/java/lib/GeomUtil.java
@@ -18,6 +18,8 @@
 
 /** Geometry utilities for working with translations, rotations, transforms, and poses. */
 public class GeomUtil {
+  private GeomUtil() {}
+
   /**
    * Creates a pure translating transform
    *