diff --git a/src/main/java/frc/robot/vision/SwerveVisionEstimator.java b/src/main/java/frc/robot/vision/SwerveVisionEstimator.java
new file mode 100644
index 0000000..b464193
--- /dev/null
+++ b/src/main/java/frc/robot/vision/SwerveVisionEstimator.java
@@ -0,0 +1,90 @@
+package frc.robot.vision;
+
+import static frc.robot.vision.VisionConfig.camNames;
+import static frc.robot.vision.VisionConfig.robotToCamTransforms;
+
+import java.util.Arrays;
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.drivetrain.Drivetrain;
+import frc.robot.drivetrain.DrivetrainConfig.DriveConstants;
+
+public class SwerveVisionEstimator extends SubsystemBase{
+
+    private final Vision[] visions;
+    private static SwerveVisionEstimator mInstance;
+    private final Supplier<SwerveModulePosition[]> modSupplier;
+    private final Supplier<Rotation2d> rotationSupplier;
+    private final SwerveDrivePoseEstimator poseEstimator;
+    private final Field2d field2d = new Field2d();
+
+    public SwerveVisionEstimator(
+        Supplier<Rotation2d> rotSup, Supplier<SwerveModulePosition[]> modSup){
+            this.modSupplier = modSup;
+            this.rotationSupplier = rotSup;
+
+            poseEstimator = new SwerveDrivePoseEstimator(
+                DriveConstants.swerveKinematics,
+                rotationSupplier.get(),
+                modSupplier.get(),
+                new Pose2d());
+
+            visions = new Vision[camNames.length];
+            for (int i = 0; i < camNames.length; i++){
+                visions[i] = new Vision(camNames[i], robotToCamTransforms[i]);
+            }
+        }
+    
+    public static SwerveVisionEstimator getInstance(){
+        if (mInstance == null){
+            final var drivetrain = Drivetrain.getInstance();
+            mInstance = new SwerveVisionEstimator(drivetrain::getGyroYaw, drivetrain::getModulePositions);
+        }
+        return mInstance;
+    }
+
+    public Pose2d getCurrrentPose(){
+        return poseEstimator.getEstimatedPosition();
+    }
+
+    public void estimatorUpdate(){
+        Arrays.stream(visions).forEach(vision -> {
+            var visionEst = vision.getEstimatedGlobalPose();
+            
+            visionEst.ifPresent(est -> {
+                var estStdDevs = vision.getEstimationStdDevs();
+                
+                poseEstimator.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
+            });
+        });
+    }
+
+    private String getFormattedPose(){
+        var pose = getCurrrentPose();
+        return String.format("(%.3f, %.3f) %.2f degrees",
+        pose.getX(),
+        pose.getY(),
+        pose.getRotation().getDegrees());
+    }
+
+    @Override
+    public void periodic(){
+        //Update swerve pose estimator using drivetrain
+        poseEstimator.update(rotationSupplier.get(), modSupplier.get());
+        //Add vision measurements to swerve pose estimator
+        estimatorUpdate();
+
+        //log to dashboard
+        var dashboardPose = poseEstimator.getEstimatedPosition();
+        SmartDashboard.putString("Estimated Pose", getFormattedPose());
+        SmartDashboard.putData(field2d);
+        field2d.setRobotPose(getCurrrentPose());
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java
index b64d82b..c580bb7 100644
--- a/src/main/java/frc/robot/vision/Vision.java
+++ b/src/main/java/frc/robot/vision/Vision.java
@@ -20,7 +20,6 @@
 import edu.wpi.first.math.numbers.N3;
 import frc.robot.vision.Vision;
 import frc.robot.vision.VisionConfig.*;
-import frc.robot.vision.VisionConsumer;
 
 /*
  * This code is a modified version of Photonvision's own example and 7028's vision code:
diff --git a/src/main/java/frc/robot/vision/VisionConsumer.java b/src/main/java/frc/robot/vision/VisionConsumer.java
deleted file mode 100644
index b26bbb8..0000000
--- a/src/main/java/frc/robot/vision/VisionConsumer.java
+++ /dev/null
@@ -1,25 +0,0 @@
-package frc.robot.vision;
-
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N3;
-
-@FunctionalInterface
-public interface VisionConsumer {
-    /**
-    * Adds a vision measurement.
-    * <p>
-    * Note that the vision measurement standard deviations passed into this method
-    * will continue to apply to future measurements.
-    *
-    * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
-    * @param timestampSeconds The timestamp of the vision measurement in seconds.
-    * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement in the form [x, y, theta]ᵀ, with
-    *          units in meters and radians.
-    */
-    public void addVisionMeasurement(
-        Pose2d visionRobotPoseMeters,
-        double timestampSeconds,
-        Matrix<N3, N1> visionMeasurementStdDevs);
-}
diff --git a/src/main/java/frc/robot/vision/commands/PhotonEstimatorCommand.java b/src/main/java/frc/robot/vision/commands/PhotonEstimatorCommand.java
deleted file mode 100644
index 231672e..0000000
--- a/src/main/java/frc/robot/vision/commands/PhotonEstimatorCommand.java
+++ /dev/null
@@ -1,42 +0,0 @@
-package frc.robot.vision.commands;
-
-import static frc.robot.vision.VisionConfig.camNames;
-import static frc.robot.vision.VisionConfig.robotToCamTransforms;
-
-import java.util.Arrays;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.vision.Vision;
-import frc.robot.vision.VisionConfig.*;
-import frc.robot.vision.VisionConsumer;
-
-public class PhotonEstimatorCommand extends Command{
-    private final Vision[] visions;
-    private final VisionConsumer visionConsumer;
-
-    public PhotonEstimatorCommand(VisionConsumer consumer){
-        visions = new Vision[camNames.length];
-        for (int i = 0; i < camNames.length; i++){
-            visions[i] = new Vision(camNames[i], robotToCamTransforms[i]);
-        }
-        this.visionConsumer = consumer;
-    }
-
-    @Override
-    public void execute(){
-        Arrays.stream(visions).forEach(vision -> {
-            var visionEst = vision.getEstimatedGlobalPose();
-            
-            visionEst.ifPresent(est -> {
-                var estStdDevs = vision.getEstimationStdDevs();
-                
-                visionConsumer.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
-            });
-        });
-    }
-
-    @Override
-    public boolean runsWhenDisabled(){
-        return true;
-    }
-}