diff --git a/photon-lib/src/main/java/org/photonvision/PhotonUtils.java b/photon-lib/src/main/java/org/photonvision/PhotonUtils.java
index 66eff49a7..cc46a941a 100644
--- a/photon-lib/src/main/java/org/photonvision/PhotonUtils.java
+++ b/photon-lib/src/main/java/org/photonvision/PhotonUtils.java
@@ -24,10 +24,7 @@
 
 package org.photonvision;
 
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.*;
 
 public final class PhotonUtils {
     private PhotonUtils() {
@@ -170,4 +167,43 @@ public final class PhotonUtils {
         var targetToCamera = cameraToTarget.inverse();
         return fieldToTarget.transformBy(targetToCamera);
     }
+
+    /**
+     * Estimates the pose of the robot in the field coordinate system, given the pose of the fiducial
+     * tag, the robot relative to the camera, and the target relative to the camera.
+     *
+     * @param fieldRelativeTagPose Pose3D the field relative pose of the target
+     * @param cameraToRobot Transform3D of the robot relative to the camera. Origin of the robot is
+     *     defined as the center.
+     * @param cameraToTarget Transform3D of the target relative to the camera, returned by
+     *     PhotonVision
+     * @return Transform3d Robot position relative to the field
+     */
+    public static Pose3d estimateFieldToRobotAprilTag(
+            Transform3d cameraToTarget, Pose3d fieldRelativeTagPose, Transform3d cameraToRobot) {
+        return fieldRelativeTagPose.plus(cameraToTarget.inverse()).plus(cameraToRobot);
+    }
+
+    /**
+     * Returns the yaw between your robot and a target.
+     *
+     * @param robotPose Current pose of the robot
+     * @param targetPose Pose of the target on the field
+     * @return double Yaw to the target
+     */
+    public static Rotation2d getYawToPose(Pose2d robotPose, Pose2d targetPose) {
+        Translation2d relativeTrl = targetPose.relativeTo(robotPose).getTranslation();
+        return new Rotation2d(relativeTrl.getX(), relativeTrl.getY());
+    }
+
+    /**
+     * Returns the distance between two poses
+     *
+     * @param robotPose Pose of the robot
+     * @param targetPose Pose of the target
+     * @return
+     */
+    public static double getDistanceToPose(Pose2d robotPose, Pose2d targetPose) {
+        return robotPose.getTranslation().getDistance(targetPose.getTranslation());
+    }
 }
diff --git a/photon-lib/src/test/java/org/photonvision/PhotonUtilTest.java b/photon-lib/src/test/java/org/photonvision/PhotonUtilTest.java
index 03c0a714d..22bd84c42 100644
--- a/photon-lib/src/test/java/org/photonvision/PhotonUtilTest.java
+++ b/photon-lib/src/test/java/org/photonvision/PhotonUtilTest.java
@@ -24,9 +24,7 @@
 
 package org.photonvision;
 
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.*;
 import edu.wpi.first.math.util.Units;
 import org.junit.jupiter.api.Assertions;
 import org.junit.jupiter.api.Test;
@@ -81,4 +79,24 @@ class PhotonUtilTest {
         Assertions.assertEquals(0, fieldToRobot.getY(), 0.1);
         Assertions.assertEquals(0, fieldToRobot.getRotation().getDegrees(), 0.1);
     }
+
+    @Test
+    public void testAprilTagUtils() {
+        var cameraToTarget = new Transform3d(new Translation3d(1, 0, 0), new Rotation3d());
+        var tagPose = new Pose3d(5, 0, 0, new Rotation3d());
+        var cameraToRobot = new Transform3d();
+
+        var fieldToRobot =
+                PhotonUtils.estimateFieldToRobotAprilTag(cameraToTarget, tagPose, cameraToRobot);
+
+        var targetPose =
+                new Pose2d(
+                        new Translation2d(Units.inchesToMeters(324), Units.inchesToMeters(162)),
+                        new Rotation2d());
+        var currentPose = new Pose2d(0, 0, Rotation2d.fromDegrees(0));
+        Assertions.assertEquals(4.0, fieldToRobot.getX());
+        Assertions.assertEquals(
+                Math.toDegrees(Math.atan2((Units.inchesToMeters(162)), (Units.inchesToMeters(324)))),
+                PhotonUtils.getYawToPose(currentPose, targetPose).getDegrees());
+    }
 }