diff --git a/src/main/java/frc/robot/commands/collision/CheckCollision.java b/src/main/java/frc/robot/commands/collision/CheckCollision.java
index 1cc3e678..4b8a7c00 100644
--- a/src/main/java/frc/robot/commands/collision/CheckCollision.java
+++ b/src/main/java/frc/robot/commands/collision/CheckCollision.java
@@ -1,6 +1,9 @@
 package frc.robot.commands.collision;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.numbers.N2;
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.gyro.ThreadedGyro;
 import frc.robot.subsystems.swervev3.SwerveDrivetrain;
@@ -12,8 +15,8 @@ public class CheckCollision extends LoggableCommand {
   private DoubleSupplier horizSupplier;
   private final DoubleSupplier vertSupplier;
   private ThreadedGyro accelerometer;
-  private double currentVelValue;
-  private double predictedVelValue;
+  private Vector<N2> currentVelValue;
+  private Vector<N2> predictedVelValue;
   private boolean trustOdometry;
 
   public CheckCollision(
@@ -39,8 +42,9 @@ public void execute() {
     double y = MathUtil.applyDeadband(horizVal, 0.05) * Constants.MAX_VELOCITY / 10;
     double x = MathUtil.applyDeadband(vertVal, 0.05) * Constants.MAX_VELOCITY / 10;
     currentVelValue = accelerometer.getVelocityValue();
-    predictedVelValue = Math.sqrt(Math.pow(y, 2) + Math.pow(x, 2));
-    if (Math.abs(currentVelValue - predictedVelValue) > Constants.COLLISION_VALUE) {
+    predictedVelValue = VecBuilder.fill(x, y);
+    Vector<N2> diff = currentVelValue.minus(predictedVelValue);
+    if (diff.dot(diff) > Constants.COLLISION_VALUE) {
       trustOdometry = false;
     }
   }
diff --git a/src/main/java/frc/robot/subsystems/gyro/ThreadedGyro.java b/src/main/java/frc/robot/subsystems/gyro/ThreadedGyro.java
index f78599c3..24b29cd6 100644
--- a/src/main/java/frc/robot/subsystems/gyro/ThreadedGyro.java
+++ b/src/main/java/frc/robot/subsystems/gyro/ThreadedGyro.java
@@ -1,6 +1,9 @@
 package frc.robot.subsystems.gyro;
 
 import com.studica.frc.AHRS;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.constants.Constants;
 import java.util.concurrent.Executors;
@@ -16,13 +19,13 @@ public class ThreadedGyro {
   private final AtomicLong lastGyro;
   private final AtomicLong gyroOffset = new AtomicLong();
   private final ScheduledExecutorService executor;
-  private double currentVelocityValue;
+  private Vector<N2> currentVelocityValue;
   private double currentVelValueX;
   private double currentVelValueY;
 
   public ThreadedGyro(AHRS gyro) {
     this.gyro = gyro;
-    this.currentVelocityValue = 0;
+    this.currentVelocityValue = VecBuilder.fill(0, 0);
     this.currentVelValueX = 0;
     this.currentVelValueY = 0;
     this.lastGyro = new AtomicLong((Double.doubleToLongBits(0)));
@@ -67,14 +70,14 @@ private void updateGyro() {
     lastGyro.set(Double.doubleToLongBits(((gyro.getAngle()) % 360) * -1));
     currentVelValueX = gyro.getVelocityX();
     currentVelValueY = gyro.getVelocityY();
-    currentVelocityValue = Math.sqrt(Math.pow(currentVelValueX, 2) + Math.pow(currentVelValueY, 2));
+    currentVelocityValue = VecBuilder.fill(currentVelValueX, currentVelValueY);
   }
 
   public double getGyroValue() {
     return Double.longBitsToDouble(lastGyro.get());
   }
 
-  public double getVelocityValue() {
+  public Vector<N2> getVelocityValue() {
     return currentVelocityValue;
   }