diff --git a/src/main/java/frc/robot/localization/InterpolationUtil.java b/src/main/java/frc/robot/localization/InterpolationUtil.java
index db6af490..790170ca 100644
--- a/src/main/java/frc/robot/localization/InterpolationUtil.java
+++ b/src/main/java/frc/robot/localization/InterpolationUtil.java
@@ -20,11 +20,30 @@ public class InterpolationUtil {
       List.of(SUBWOOFER, STAGE_FRONT, STAGE_RIGHT, STAGE_MIDDLE);
 
   public static Pose2d interpolatePose(Pose2d visionInput) {
+    double distanceSum = 0;
+
+    for (var dataPoint : DATA_POINTS) {
+      var distancePoint =
+          dataPoint.measuredPose().getTranslation().getDistance(visionInput.getTranslation());
+
+      distanceSum += distancePoint;
+    }
+
+    Pose2d weightedSum = new Pose2d();
+
     for (var dataPoint : DATA_POINTS) {
-      double distancePoint =
-          dataPoint.visionPose().getTranslation().getDistance(visionInput.getTranslation());
+      var distancePoint =
+          dataPoint.measuredPose().getTranslation().getDistance(visionInput.getTranslation());
+
+      var result = dataPoint.visionPose().times(distanceSum - distancePoint);
+
+      weightedSum =
+          new Pose2d(
+              weightedSum.getX() + result.getX(),
+              weightedSum.getY() + result.getY(),
+              weightedSum.getRotation().plus(result.getRotation()));
     }
 
-    return new Pose2d();
+    return weightedSum.div(distanceSum);
   }
 }