From a5dc9ec0d6a5565326958f3fc50daa6630062426 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Sun, 11 Jan 2026 13:58:53 -0500
Subject: [PATCH] Open up pose estimator strategy methods (#2252)

---
 .../photonlib/robot-pose-estimator.md         | 102 +-
 photon-lib/py/photonlibpy/__init__.py         |   3 +-
 .../py/photonlibpy/estimatedRobotPose.py      |   7 -
 .../py/photonlibpy/photonPoseEstimator.py     | 264 +----
 .../py/test/photonPoseEstimator_test.py       | 111 +--
 .../org/photonvision/EstimatedRobotPose.java  |  14 +
 .../org/photonvision/PhotonPoseEstimator.java | 476 ++++++---
 .../native/cpp/photon/PhotonPoseEstimator.cpp | 325 +++---
 .../include/photon/PhotonPoseEstimator.h      | 252 +++--
 .../LegacyPhotonPoseEstimatorTest.java        | 936 ++++++++++++++++++
 .../photonvision/PhotonPoseEstimatorTest.java | 183 +---
 .../cpp/LegacyPhotonPoseEstimatorTest.cpp     | 661 +++++++++++++
 .../native/cpp/PhotonPoseEstimatorTest.cpp    | 169 +---
 .../estimation/VisionEstimation.java          |   2 +-
 .../poseest/src/main/include/Vision.h         |  14 +-
 .../src/main/java/frc/robot/Vision.java       |  15 +-
 photonlib-python-examples/poseest/robot.py    |  11 +-
 17 files changed, 2493 insertions(+), 1052 deletions(-)
 create mode 100644 photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java
 create mode 100644 photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp

diff --git a/docs/source/docs/programming/photonlib/robot-pose-estimator.md b/docs/source/docs/programming/photonlib/robot-pose-estimator.md
index 88857da7b..daa4446e0 100644
--- a/docs/source/docs/programming/photonlib/robot-pose-estimator.md
+++ b/docs/source/docs/programming/photonlib/robot-pose-estimator.md
@@ -48,91 +48,88 @@ Another necessary argument for creating a `PhotonPoseEstimator` is the `Transfor
 
 ## Creating a `PhotonPoseEstimator`
 
-The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (see above), `PoseStrategy`, `PhotonCamera`, and `Transform3d`. `PoseStrategy` has nine possible values:
-
-- MULTI_TAG_PNP_ON_COPROCESSOR
-  - Calculates a new robot position estimate by combining all visible tag corners. Recommended for all teams as it will be the most accurate.
-  - Must configure the AprilTagFieldLayout properly in the UI, please see {ref}`here <docs/apriltag-pipelines/multitag:multitag localization>` for more information.
-- LOWEST_AMBIGUITY
-  - Choose the Pose with the lowest ambiguity.
-- CLOSEST_TO_CAMERA_HEIGHT
-  - Choose the Pose which is closest to the camera height.
-- CLOSEST_TO_REFERENCE_POSE
-  - Choose the Pose which is closest to the pose from setReferencePose().
-- CLOSEST_TO_LAST_POSE
-  - Choose the Pose which is closest to the last pose calculated.
-- AVERAGE_BEST_TARGETS
-  - Choose the Pose which is the average of all the poses from each tag.
-- MULTI_TAG_PNP_ON_RIO
-  - A slower, older version of MULTI_TAG_PNP_ON_COPROCESSOR, not recommended for use.
-- PNP_DISTANCE_TRIG_SOLVE
-  - Use distance data from best visible tag to compute a Pose. This runs on the RoboRIO in order
-    to access the robot's yaw heading, and MUST have addHeadingData called every frame so heading
-    data is up-to-date. Based on a reference implementation by [FRC Team 6328 Mechanical Advantage](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98).
-- CONSTRAINED_SOLVEPNP
-  - Solve a constrained version of the Perspective-n-Point problem with the robot's drivebase
-    flat on the floor. This computation takes place on the RoboRIO, and should not take more than 2ms.
-    This also requires addHeadingData to be called every frame so heading data is up to date.
-    If Multi-Tag PNP is enabled on the coprocessor, it will be used to provide an initial seed to
-    the optimization algorithm -- otherwise, the multi-tag fallback strategy will be used as the
-    seed.
+The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (see above) and `Transform3d`.
 
 ```{eval-rst}
 .. tab-set-code::
    .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java
     :language: java
-    :lines: 65-66
+    :lines: 63
 
    .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Vision.h
     :language: c++
-    :lines: 150-153
+    :lines: 149-150
 
    .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
     :language: python
-    :lines: 45-50
+    :lines: 45-48
 ```
 
-:::{note}
-Python still takes a `PhotonCamera` in the constructor, so you must create the camera as shown in the next section and then return and use it to create the `PhotonPoseEstimator`.
-:::
-
 ## Using a `PhotonPoseEstimator`
 
-The final prerequisite to using your `PhotonPoseEstimator` is creating a `PhotonCamera`. To do this, you must set the name of your camera in Photon Client. From there you can define the camera in code.
+To use your `PhotonPoseEstimator`, you must create a `PhotonCamera` and feed the results into your `PhotonPoseEstimator`. To do this, you must first set the name of your camera in Photon Client. From there you can define the camera in code.
 
 ```{eval-rst}
 .. tab-set-code::
     .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java
      :language: java
-     :lines: 63
+     :lines: 62
 
-    .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h
+    .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Vision.h
      :language: c++
-     :lines: 55
+     :lines: 151
 
     .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
      :language: python
      :lines: 44
 ```
 
-Calling `update()` on your `PhotonPoseEstimator` will return an `EstimatedRobotPose`, which includes a `Pose3d` of the latest estimated pose (using the selected strategy) along with a `double` of the timestamp when the robot pose was estimated.
+When taking in a result from a `PhotonCamera`, PhotonPoseEstimator offers nine possible "strategies" for calculating a pose from a pipeline result in the form of methods that you can call, following the pattern `estimate<strategy name>Pose`:
+
+- Coprocessor MultiTag (`estimateCoprocMultiTagPose`)
+  - Calculates a new robot position estimate by combining all visible tag corners. Recommended for all teams as it will be the most accurate.
+  - Must configure the AprilTagFieldLayout properly in the UI, please see {ref}`here <docs/apriltag-pipelines/multitag:multitag localization>` for more information.
+- Lowest Ambiguity (`estimateLowestAmbiguityPose`)
+  - Choose the Pose with the lowest ambiguity.
+- Closest to Camera Height (`estimateClosestToCameraHeightPose`)
+  - Choose the Pose which is closest to the camera height.
+- Closest to Reference Pose (`estimateClosestToReferencePose`)
+  - Choose the Pose which is closest to the pose that is passed into the function.
+- Average Best Targets (`estimateAverageBestTargetsPose`)
+  - Choose the Pose which is the average of all the poses from each tag.
+- roboRio MultiTag (`estimateRioMultiTagPose`)
+  - A slower, older version of Coprocessor MultiTag, not recommended for use.
+- PnP Distance Trig Solve (`estimatePnpDistanceTrigSolvePose`)
+  - Use distance data from best visible tag to compute a Pose. This runs on the RoboRIO in order
+    to access the robot's yaw heading, and MUST have addHeadingData called every frame so heading
+    data is up-to-date. Based on a reference implementation by [FRC Team 6328 Mechanical Advantage](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98).
+- Constrained SolvePnP (`estimateConstrainedSolvepnpPose`)
+  - Solve a constrained version of the Perspective-n-Point problem with the robot's drivebase
+    flat on the floor. This computation takes place on the RoboRIO, and should not take more than 2ms.
+    This also requires addHeadingData to be called every frame so heading data is up to date.
+
+Calling one of the `estimate<strategy>Pose()` methods on your `PhotonPoseEstimator` will return an `Optional<EstimatedRobotPose>`, which includes a `Pose3d` of the latest estimated pose (using the selected strategy) along with a `double` of the timestamp when the robot pose was estimated. The recommended way to use the estimatePose methods is to
+1. do estimation with one of MultiTag methods, check if the result is empty, then
+2. fallback to single tag estimation using a method like `estimateLowestAmbiguityPose`.
 
 ```{eval-rst}
 .. tab-set-code::
    .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java
     :language: java
-    :lines: 93-116
+    :lines: 91-94
 
    .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Vision.h
     :language: c++
-    :lines: 80-100
+    :lines: 79-82
 
    .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
      :language: python
-     :lines: 53
+     :lines: 52-54
 ```
 
-You should be updating your [drivetrain pose estimator](https://docs.wpilib.org/en/latest/docs/software/advanced-controls/state-space/state-space-pose-estimators.html) with the result from the `PhotonPoseEstimator` every loop using `addVisionMeasurement()`.
+For Constrained SolvePnP, it's recommended to do the previously mentioned steps, and then feed the pose (if it exists) into `estimateConstrainedSolvepnpPose`, and if the Constrained SolvePnP result is empty, simply feed the seed pose into your drivetrain pose estimator.
+
+Once you have the `Optional<EstimatedRobotPose>`, you can check to see if there's an actual pose inside, and act accordingly. You should be updating your [drivetrain pose estimator](https://docs.wpilib.org/en/latest/docs/software/advanced-controls/state-space/state-space-pose-estimators.html) with the result from the `PhotonPoseEstimator` every loop using `addVisionMeasurement()`. For Java and C++, the examples pass a method from the drivetrain to a `Vision` object, with the parameter being called `estConsumer`. Python calls the drivetrain directly.
 
 ```{eval-rst}
 .. tab-set-code::
@@ -146,7 +143,22 @@ You should be updating your [drivetrain pose estimator](https://docs.wpilib.org/
 
    .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
      :language: python
-     :lines: 54-57
+     :lines: 56-58
+```
+
+```{eval-rst}
+.. tab-set-code::
+   .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java
+    :language: java
+    :lines: 89-115
+
+   .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Vision.h
+    :language: c++
+    :lines: 77-100
+
+   .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
+     :language: python
+     :lines: 51-54
 ```
 
 ## Complete Examples
diff --git a/photon-lib/py/photonlibpy/__init__.py b/photon-lib/py/photonlibpy/__init__.py
index 240086d2c..f11462d51 100644
--- a/photon-lib/py/photonlibpy/__init__.py
+++ b/photon-lib/py/photonlibpy/__init__.py
@@ -25,12 +25,11 @@
 from .estimatedRobotPose import EstimatedRobotPose
 from .packet import Packet
 from .photonCamera import PhotonCamera
-from .photonPoseEstimator import PhotonPoseEstimator, PoseStrategy
+from .photonPoseEstimator import PhotonPoseEstimator
 
 __all__ = (
     "EstimatedRobotPose",
     "Packet",
     "PhotonCamera",
     "PhotonPoseEstimator",
-    "PoseStrategy",
 )
diff --git a/photon-lib/py/photonlibpy/estimatedRobotPose.py b/photon-lib/py/photonlibpy/estimatedRobotPose.py
index 491f71c83..b964bf423 100644
--- a/photon-lib/py/photonlibpy/estimatedRobotPose.py
+++ b/photon-lib/py/photonlibpy/estimatedRobotPose.py
@@ -16,15 +16,11 @@
 ###############################################################################
 
 from dataclasses import dataclass
-from typing import TYPE_CHECKING
 
 from wpimath.geometry import Pose3d
 
 from .targeting.photonTrackedTarget import PhotonTrackedTarget
 
-if TYPE_CHECKING:
-    from .photonPoseEstimator import PoseStrategy
-
 
 @dataclass
 class EstimatedRobotPose:
@@ -38,6 +34,3 @@ class EstimatedRobotPose:
 
     targetsUsed: list[PhotonTrackedTarget]
     """A list of the targets used to compute this pose"""
-
-    strategy: "PoseStrategy"
-    """The strategy actually used to produce this pose"""
diff --git a/photon-lib/py/photonlibpy/photonPoseEstimator.py b/photon-lib/py/photonlibpy/photonPoseEstimator.py
index 00cb0fbd2..e47251360 100644
--- a/photon-lib/py/photonlibpy/photonPoseEstimator.py
+++ b/photon-lib/py/photonlibpy/photonPoseEstimator.py
@@ -15,7 +15,6 @@
 ## along with this program.  If not, see <https://www.gnu.org/licenses/>.
 ###############################################################################
 
-import enum
 from typing import Optional
 
 import hal
@@ -34,54 +33,9 @@ from wpimath.geometry import (
 from wpimath.interpolation import TimeInterpolatableRotation2dBuffer
 
 from .estimatedRobotPose import EstimatedRobotPose
-from .photonCamera import PhotonCamera
 from .targeting.photonPipelineResult import PhotonPipelineResult
 
 
-class PoseStrategy(enum.Enum):
-    """
-    Position estimation strategies that can be used by the PhotonPoseEstimator class.
-    """
-
-    LOWEST_AMBIGUITY = enum.auto()
-    """Choose the Pose with the lowest ambiguity."""
-
-    CLOSEST_TO_CAMERA_HEIGHT = enum.auto()
-    """Choose the Pose which is closest to the camera height."""
-
-    CLOSEST_TO_REFERENCE_POSE = enum.auto()
-    """Choose the Pose which is closest to a set Reference position."""
-
-    CLOSEST_TO_LAST_POSE = enum.auto()
-    """Choose the Pose which is closest to the last pose calculated."""
-
-    AVERAGE_BEST_TARGETS = enum.auto()
-    """Return the average of the best target poses using ambiguity as weight."""
-
-    MULTI_TAG_PNP_ON_COPROCESSOR = enum.auto()
-    """
-    Use all visible tags to compute a single pose estimate on coprocessor.
-    This option needs to be enabled on the PhotonVision web UI as well.
-    """
-
-    MULTI_TAG_PNP_ON_RIO = enum.auto()
-    """
-    Use all visible tags to compute a single pose estimate.
-    This runs on the RoboRIO, and can take a lot of time.
-    """
-
-    PNP_DISTANCE_TRIG_SOLVE = enum.auto()
-    """
-    Use distance data from best visible tag to compute a Pose. This runs on
-    the RoboRIO in order to access the robot's yaw heading, and MUST have
-    addHeadingData called every frame so heading data is up-to-date.
-
-    Produces a Pose2d in estimatedRobotPose (0 for z, roll, pitch).
-
-    See https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
-    """
-
-
 class PhotonPoseEstimator:
     instance_count = 1
 
@@ -94,8 +48,6 @@ class PhotonPoseEstimator:
     def __init__(
         self,
         fieldTags: AprilTagFieldLayout,
-        strategy: PoseStrategy,
-        camera: PhotonCamera,
         robotToCamera: Transform3d,
     ):
         """Create a new PhotonPoseEstimator.
@@ -104,21 +56,13 @@ class PhotonPoseEstimator:
                            with respect to the FIRST field using the Field Coordinate System.
                            Note that setting the origin of this layout object will affect the
                            results from this class.
-        :param strategy: The strategy it should use to determine the best pose.
-        :param camera: PhotonCamera
         :param robotToCamera: Transform3d from the center of the robot to the camera mount position (i.e.,
                                 robot ➔ camera) in the Robot Coordinate System.
         """
         self._fieldTags = fieldTags
-        self._primaryStrategy = strategy
-        self._camera = camera
         self.robotToCamera = robotToCamera
 
-        self._multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY
         self._reportedErrors: set[int] = set()
-        self._poseCacheTimestampSeconds = -1.0
-        self._lastPose: Optional[Pose3d] = None
-        self._referencePose: Optional[Pose3d] = None
         self._headingBuffer = TimeInterpolatableRotation2dBuffer(1)
 
         # Usage reporting
@@ -146,92 +90,8 @@ class PhotonPoseEstimator:
 
         :param fieldTags: the AprilTagFieldLayout
         """
-        self._checkUpdate(self._fieldTags, fieldTags)
         self._fieldTags = fieldTags
 
-    @property
-    def primaryStrategy(self) -> PoseStrategy:
-        """Get the Position Estimation Strategy being used by the Position Estimator.
-
-        :returns: the strategy
-        """
-        return self._primaryStrategy
-
-    @primaryStrategy.setter
-    def primaryStrategy(self, strategy: PoseStrategy):
-        """Set the Position Estimation Strategy used by the Position Estimator.
-
-        :param strategy: the strategy to set
-        """
-        self._checkUpdate(self._primaryStrategy, strategy)
-        self._primaryStrategy = strategy
-
-    @property
-    def multiTagFallbackStrategy(self) -> PoseStrategy:
-        return self._multiTagFallbackStrategy
-
-    @multiTagFallbackStrategy.setter
-    def multiTagFallbackStrategy(self, strategy: PoseStrategy):
-        """Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must
-        NOT be MULTI_TAG_PNP
-
-        :param strategy: the strategy to set
-        """
-        self._checkUpdate(self._multiTagFallbackStrategy, strategy)
-        if (
-            strategy is PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR
-            or strategy is PoseStrategy.MULTI_TAG_PNP_ON_RIO
-        ):
-            wpilib.reportWarning(
-                "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity",
-                False,
-            )
-            strategy = PoseStrategy.LOWEST_AMBIGUITY
-        self._multiTagFallbackStrategy = strategy
-
-    @property
-    def referencePose(self) -> Optional[Pose3d]:
-        """Return the reference position that is being used by the estimator.
-
-        :returns: the referencePose
-        """
-        return self._referencePose
-
-    @referencePose.setter
-    def referencePose(self, referencePose: Pose3d | Pose2d):
-        """Update the stored reference pose for use when using the **CLOSEST_TO_REFERENCE_POSE**
-        strategy.
-
-        :param referencePose: the referencePose to set
-        """
-        if isinstance(referencePose, Pose2d):
-            referencePose = Pose3d(referencePose)
-        self._checkUpdate(self._referencePose, referencePose)
-        self._referencePose = referencePose
-
-    @property
-    def lastPose(self) -> Optional[Pose3d]:
-        return self._lastPose
-
-    @lastPose.setter
-    def lastPose(self, lastPose: Pose3d | Pose2d):
-        """Update the stored last pose. Useful for setting the initial estimate when using the
-        **CLOSEST_TO_LAST_POSE** strategy.
-
-        :param lastPose: the lastPose to set
-        """
-        if isinstance(lastPose, Pose2d):
-            lastPose = Pose3d(lastPose)
-        self._checkUpdate(self._lastPose, lastPose)
-        self._lastPose = lastPose
-
-    def _invalidatePoseCache(self) -> None:
-        self._poseCacheTimestampSeconds = -1.0
-
-    def _checkUpdate(self, oldObj, newObj) -> None:
-        if oldObj != newObj:
-            self._invalidatePoseCache()
-
     def addHeadingData(
         self, timestampSeconds: wpimath.units.seconds, heading: Rotation2d | Rotation3d
     ) -> None:
@@ -258,76 +118,40 @@ class PhotonPoseEstimator:
         self._headingBuffer.clear()
         self.addHeadingData(timestampSeconds, heading)
 
-    def update(
-        self, cameraResult: Optional[PhotonPipelineResult] = None
-    ) -> Optional[EstimatedRobotPose]:
+    def _shouldEstimate(self, cameraResult: PhotonPipelineResult) -> bool:
         """
-        Updates the estimated position of the robot. Returns empty if:
+        :param cameraResult: A pipeline result from the camera.
 
-         - The timestamp of the provided pipeline result is the same as in the previous call to
-         ``update()``.
-
-         - No targets were found in the pipeline results.
-
-        :param cameraResult: The latest pipeline result from the camera
-
-        :returns: an :class:`EstimatedRobotPose` with an estimated pose, timestamp, and targets used to
-                   create the estimate.
+        :returns: Whether or not estimation should be done.
         """
-        if not cameraResult:
-            if not self._camera:
-                wpilib.reportError("[PhotonPoseEstimator] Missing camera!", False)
-                return None
-            cameraResult = self._camera.getLatestResult()
-
         if cameraResult.getTimestampSeconds() < 0:
-            return None
+            return False
 
-        # If the pose cache timestamp was set, and the result is from the same
-        # timestamp, return an
-        # empty result
-        if (
-            self._poseCacheTimestampSeconds > 0.0
-            and abs(
-                self._poseCacheTimestampSeconds - cameraResult.getTimestampSeconds()
-            )
-            < 1e-6
-        ):
-            return None
+        # If no targets seen, trivial case -- can't do estimation
+        return len(cameraResult.targets) > 0
 
-        # Remember the timestamp of the current result used
-        self._poseCacheTimestampSeconds = cameraResult.getTimestampSeconds()
-
-        # If no targets seen, trivial case -- return empty result
-        if not cameraResult.targets:
-            return None
-
-        return self._update(cameraResult, self._primaryStrategy)
-
-    def _update(
-        self, cameraResult: PhotonPipelineResult, strat: PoseStrategy
-    ) -> Optional[EstimatedRobotPose]:
-        if strat is PoseStrategy.LOWEST_AMBIGUITY:
-            estimatedPose = self._lowestAmbiguityStrategy(cameraResult)
-        elif strat is PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR:
-            estimatedPose = self._multiTagOnCoprocStrategy(cameraResult)
-        elif strat is PoseStrategy.PNP_DISTANCE_TRIG_SOLVE:
-            estimatedPose = self._pnpDistanceTrigSolveStrategy(cameraResult)
-        else:
-            wpilib.reportError(
-                "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", False
-            )
-            return None
-
-        if not estimatedPose:
-            self._lastPose = None
-
-        return estimatedPose
-
-    def _pnpDistanceTrigSolveStrategy(
+    def estimatePnpDistanceTrigSolvePose(
         self, result: PhotonPipelineResult
     ) -> Optional[EstimatedRobotPose]:
-        if (bestTarget := result.getBestTarget()) is None:
+        """
+
+        Return the estimated position of the robot by using distance data from best visible tag to
+        compute a Pose. This runs on the RoboRIO in order to access the robot's yaw heading, and MUST
+        have addHeadingData called every frame so heading data is up-to-date.
+
+        Yields a Pose2d in estimatedRobotPose (0 for z, roll, pitch)
+
+        https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
+
+
+        :param result: A pipeline result from the camera.
+        :returns: An :class:`EstimatedRobotPose` with an estimated pose, timestamp, and targets used
+          to create the estimate.
+        """
+        if (
+            not self._shouldEstimate(result)
+            or (bestTarget := result.getBestTarget()) is None
+        ):
             return None
 
         if (
@@ -364,16 +188,22 @@ class PhotonPoseEstimator:
         )
 
         return EstimatedRobotPose(
-            Pose3d(robotPose),
-            result.getTimestampSeconds(),
-            result.getTargets(),
-            PoseStrategy.PNP_DISTANCE_TRIG_SOLVE,
+            Pose3d(robotPose), result.getTimestampSeconds(), result.getTargets()
         )
 
-    def _multiTagOnCoprocStrategy(
+    def estimateCoprocMultiTagPose(
         self, result: PhotonPipelineResult
     ) -> Optional[EstimatedRobotPose]:
-        if result.multitagResult is not None:
+        """
+        Return the estimated position of the robot by using all visible tags to compute a single
+        pose estimate on coprocessor. This option needs to be enabled on the PhotonVision web UI as
+        well.
+
+        :param result: A pipeline result from the camera.
+        :returns: An :class:`EstimatedRobotPose` with an estimated pose, timestamp, and targets used
+          to create the estimate.
+        """
+        if result.multitagResult is not None and self._shouldEstimate(result):
             best_tf = result.multitagResult.estimatedPose.best
             best = (
                 Pose3d()
@@ -385,23 +215,22 @@ class PhotonPoseEstimator:
                 best,
                 result.getTimestampSeconds(),
                 result.targets,
-                PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
             )
         else:
-            return self._update(result, self._multiTagFallbackStrategy)
+            return None
 
-    def _lowestAmbiguityStrategy(
+    def estimateLowestAmbiguityPose(
         self, result: PhotonPipelineResult
     ) -> Optional[EstimatedRobotPose]:
         """
-        Return the estimated position of the robot with the lowest position ambiguity from a List of
-        pipeline results.
+        Return the estimated position of the robot with the lowest position ambiguity from a pipeline results.
 
-        :param result: pipeline result
-
-        :returns: the estimated position of the robot in the FCS and the estimated timestamp of this
-                  estimation.
+        :param result: A pipeline result from the camera.
+        :returns: An :class:`EstimatedRobotPose` with an estimated pose, timestamp, and targets used
+          to create the estimate.
         """
+        if not self._shouldEstimate(result):
+            return None
         lowestAmbiguityTarget = None
 
         lowestAmbiguityScore = 10.0
@@ -432,7 +261,6 @@ class PhotonPoseEstimator:
             ).transformBy(self.robotToCamera.inverse()),
             result.getTimestampSeconds(),
             result.targets,
-            PoseStrategy.LOWEST_AMBIGUITY,
         )
 
     def _reportFiducialPoseError(self, fiducialId: int) -> None:
diff --git a/photon-lib/py/test/photonPoseEstimator_test.py b/photon-lib/py/test/photonPoseEstimator_test.py
index 4937b626a..ad9693664 100644
--- a/photon-lib/py/test/photonPoseEstimator_test.py
+++ b/photon-lib/py/test/photonPoseEstimator_test.py
@@ -15,10 +15,9 @@
 ## along with this program.  If not, see <https://www.gnu.org/licenses/>.
 ###############################################################################
 
-from test import testUtil
 
 import wpimath.units
-from photonlibpy import PhotonCamera, PhotonPoseEstimator, PoseStrategy
+from photonlibpy import PhotonCamera, PhotonPoseEstimator
 from photonlibpy.estimation import TargetModel
 from photonlibpy.simulation import PhotonCameraSim, SimCameraProperties, VisionTargetSim
 from photonlibpy.targeting import (
@@ -138,10 +137,11 @@ def test_lowestAmbiguityStrategy():
     )
 
     estimator = PhotonPoseEstimator(
-        aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
+        aprilTags,
+        Transform3d(),
     )
 
-    estimatedPose = estimator.update()
+    estimatedPose = estimator.estimateLowestAmbiguityPose(cameraOne.result)
 
     assert estimatedPose is not None
 
@@ -179,8 +179,6 @@ def test_pnpDistanceTrigSolve():
 
     estimator = PhotonPoseEstimator(
         aprilTags,
-        PoseStrategy.PNP_DISTANCE_TRIG_SOLVE,
-        cameraOne,
         compoundTestTransform,
     )
 
@@ -198,7 +196,7 @@ def test_pnpDistanceTrigSolve():
     estimator.addHeadingData(
         result.getTimestampSeconds(), realPose.rotation().toRotation2d()
     )
-    estimatedRobotPose = estimator.update(result)
+    estimatedRobotPose = estimator.estimatePnpDistanceTrigSolvePose(result)
 
     assert estimatedRobotPose is not None
     pose = estimatedRobotPose.estimatedPose
@@ -224,7 +222,7 @@ def test_pnpDistanceTrigSolve():
     estimator.addHeadingData(
         result.getTimestampSeconds(), realPose.rotation().toRotation2d()
     )
-    estimatedRobotPose = estimator.update(result)
+    estimatedRobotPose = estimator.estimatePnpDistanceTrigSolvePose(result)
 
     assert estimatedRobotPose is not None
     pose = estimatedRobotPose.estimatedPose
@@ -271,13 +269,10 @@ def test_multiTagOnCoprocStrategy():
 
     estimator = PhotonPoseEstimator(
         AprilTagFieldLayout(),
-        PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
-        cameraOne,
         Transform3d(),
     )
 
-    estimatedPose = estimator.update()
-
+    estimatedPose = estimator.estimateCoprocMultiTagPose(cameraOne.result)
     assert estimatedPose is not None
 
     pose = estimatedPose.estimatedPose
@@ -288,97 +283,5 @@ def test_multiTagOnCoprocStrategy():
     assertEquals(2, pose.z, 0.01)
 
 
-def test_cacheIsInvalidated():
-    aprilTags = fakeAprilTagFieldLayout()
-    cameraOne = PhotonCameraInjector()
-
-    estimator = PhotonPoseEstimator(
-        aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
-    )
-
-    # Initial state, expect no timestamp.
-    assertEquals(-1, estimator._poseCacheTimestampSeconds)
-
-    # First result is 17s after epoch start.
-    timestamps = testUtil.PipelineTimestamps(captureTimestampMicros=17_000_000)
-    latencySecs = timestamps.pipelineLatencySecs()
-
-    # No targets, expect empty result
-    cameraOne.result = PhotonPipelineResult(
-        timestamps.receiveTimestampMicros(),
-        metadata=timestamps.toPhotonPipelineMetadata(),
-    )
-    estimatedPose = estimator.update()
-
-    assert estimatedPose is None
-    assertEquals(
-        timestamps.receiveTimestampMicros() * 1e-6 - latencySecs,
-        estimator._poseCacheTimestampSeconds,
-        1e-3,
-    )
-
-    # Set actual result
-    timestamps.incrementTimeMicros(2_500_000)
-    result = PhotonPipelineResult(
-        timestamps.receiveTimestampMicros(),
-        [
-            PhotonTrackedTarget(
-                3.0,
-                -4.0,
-                9.0,
-                4.0,
-                0,
-                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-                [
-                    TargetCorner(1, 2),
-                    TargetCorner(3, 4),
-                    TargetCorner(5, 6),
-                    TargetCorner(7, 8),
-                ],
-                [
-                    TargetCorner(1, 2),
-                    TargetCorner(3, 4),
-                    TargetCorner(5, 6),
-                    TargetCorner(7, 8),
-                ],
-                0.7,
-            )
-        ],
-        metadata=timestamps.toPhotonPipelineMetadata(),
-    )
-    cameraOne.result = result
-    estimatedPose = estimator.update()
-    assert estimatedPose is not None
-    expectedTimestamp = timestamps.receiveTimestampMicros() * 1e-6 - latencySecs
-    assertEquals(expectedTimestamp, estimatedPose.timestampSeconds, 1e-3)
-    assertEquals(expectedTimestamp, estimator._poseCacheTimestampSeconds, 1e-3)
-
-    # And again -- pose cache should mean this is empty
-    cameraOne.result = result
-    estimatedPose = estimator.update()
-    assert estimatedPose is None
-    # Expect the old timestamp to still be here
-    assertEquals(expectedTimestamp, estimator._poseCacheTimestampSeconds, 1e-3)
-
-    # Set new field layout -- right after, the pose cache timestamp should be -1
-    estimator.fieldTags = AprilTagFieldLayout([AprilTag()], 0, 0)
-    assertEquals(-1, estimator._poseCacheTimestampSeconds)
-    # Update should cache the current timestamp (20) again
-    cameraOne.result = result
-    estimatedPose = estimator.update()
-
-    assert estimatedPose is not None
-
-    assertEquals(expectedTimestamp, estimatedPose.timestampSeconds, 1e-3)
-    assertEquals(expectedTimestamp, estimator._poseCacheTimestampSeconds, 1e-3)
-
-    # Setting a value from None to a non-None should invalidate the cache.
-    assert estimator.referencePose is None
-    estimator.referencePose = Pose3d(3, 3, 3, Rotation3d())
-
-    assertEquals(-1, estimator._poseCacheTimestampSeconds)
-
-
 def assertEquals(expected, actual, epsilon=0.0):
     assert abs(expected - actual) <= epsilon
diff --git a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
index a355ab00f..b3da87c59 100644
--- a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
+++ b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
@@ -43,6 +43,20 @@ public class EstimatedRobotPose {
     /** The strategy actually used to produce this pose */
     public final PoseStrategy strategy;
 
+    /**
+     * Constructs an EstimatedRobotPose
+     *
+     * @param estimatedPose estimated pose
+     * @param timestampSeconds timestamp of the estimate
+     */
+    public EstimatedRobotPose(
+            Pose3d estimatedPose, double timestampSeconds, List<PhotonTrackedTarget> targetsUsed) {
+        this.estimatedPose = estimatedPose;
+        this.timestampSeconds = timestampSeconds;
+        this.targetsUsed = targetsUsed;
+        this.strategy = null;
+    }
+
     /**
      * Constructs an EstimatedRobotPose
      *
diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
index a9330f610..b820cdb44 100644
--- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
+++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
@@ -117,7 +117,7 @@ public class PhotonPoseEstimator {
      * @param headingFree If true, heading is completely free to vary. If false, heading excursions
      *     from the provided heading measurement will be penalized
      * @param headingScaleFactor If headingFree is false, this weights the cost of changing our robot
-     *     heading estimate against the tag corner reprojection error const.
+     *     heading estimate against the tag corner reprojection error cost.
      */
     public static final record ConstrainedSolvepnpParams(
             boolean headingFree, double headingScaleFactor) {}
@@ -144,12 +144,35 @@ public class PhotonPoseEstimator {
      *     "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
      *     Coordinate System</a>. Note that setting the origin of this layout object will affect the
      *     results from this class.
-     * @param strategy The strategy it should use to determine the best pose.
      * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
      *     robot ➔ camera) in the <a href=
      *     "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
      *     Coordinate System</a>.
      */
+    public PhotonPoseEstimator(AprilTagFieldLayout fieldTags, Transform3d robotToCamera) {
+        this.fieldTags = fieldTags;
+        this.robotToCamera = robotToCamera;
+
+        HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount);
+        InstanceCount++;
+    }
+
+    /**
+     * Create a new PhotonPoseEstimator.
+     *
+     * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects
+     *     with respect to the FIRST field using the <a href=
+     *     "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
+     *     Coordinate System</a>. Note that setting the origin of this layout object will affect the
+     *     results from this class.
+     * @param strategy The strategy it should use to determine the best pose.
+     * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
+     *     robot ➔ camera) in the <a href=
+     *     "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
+     *     Coordinate System</a>.
+     * @deprecated Use individual estimation methods with the 2 argument constructor instead.
+     */
+    @Deprecated(forRemoval = true, since = "2026")
     public PhotonPoseEstimator(
             AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
         this.fieldTags = fieldTags;
@@ -221,7 +244,9 @@ public class PhotonPoseEstimator {
      * Get the Position Estimation Strategy being used by the Position Estimator.
      *
      * @return the strategy
+     * @deprecated Use individual estimation methods instead.
      */
+    @Deprecated(forRemoval = true, since = "2026")
     public PoseStrategy getPrimaryStrategy() {
         return primaryStrategy;
     }
@@ -230,7 +255,9 @@ public class PhotonPoseEstimator {
      * Set the Position Estimation Strategy used by the Position Estimator.
      *
      * @param strategy the strategy to set
+     * @deprecated Use individual estimation methods instead.
      */
+    @Deprecated(forRemoval = true, since = "2026")
     public void setPrimaryStrategy(PoseStrategy strategy) {
         checkUpdate(this.primaryStrategy, strategy);
 
@@ -246,7 +273,9 @@ public class PhotonPoseEstimator {
      * NOT be MULTI_TAG_PNP
      *
      * @param strategy the strategy to set
+     * @deprecated Use individual estimation methods instead.
      */
+    @Deprecated(forRemoval = true, since = "2026")
     public void setMultiTagFallbackStrategy(PoseStrategy strategy) {
         checkUpdate(this.multiTagFallbackStrategy, strategy);
         if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR
@@ -262,7 +291,9 @@ public class PhotonPoseEstimator {
      * Return the reference position that is being used by the estimator.
      *
      * @return the referencePose
+     * @deprecated Use individual estimation methods instead.
      */
+    @Deprecated(forRemoval = true, since = "2026")
     public Pose3d getReferencePose() {
         return referencePose;
     }
@@ -272,7 +303,9 @@ public class PhotonPoseEstimator {
      * strategy.
      *
      * @param referencePose the referencePose to set
+     * @deprecated Use individual estimation methods instead.
      */
+    @Deprecated(forRemoval = true, since = "2026")
     public void setReferencePose(Pose3d referencePose) {
         checkUpdate(this.referencePose, referencePose);
         this.referencePose = referencePose;
@@ -283,7 +316,9 @@ public class PhotonPoseEstimator {
      * strategy.
      *
      * @param referencePose the referencePose to set
+     * @deprecated Use individual estimation methods instead.
      */
+    @Deprecated(forRemoval = true, since = "2026")
     public void setReferencePose(Pose2d referencePose) {
         setReferencePose(new Pose3d(referencePose));
     }
@@ -293,7 +328,9 @@ public class PhotonPoseEstimator {
      * <b>CLOSEST_TO_LAST_POSE</b> strategy.
      *
      * @param lastPose the lastPose to set
+     * @deprecated Use individual estimation methods instead.
      */
+    @Deprecated(forRemoval = true, since = "2026")
     public void setLastPose(Pose3d lastPose) {
         this.lastPose = lastPose;
     }
@@ -303,7 +340,9 @@ public class PhotonPoseEstimator {
      * <b>CLOSEST_TO_LAST_POSE</b> strategy.
      *
      * @param lastPose the lastPose to set
+     * @deprecated Use individual estimation methods instead.
      */
+    @Deprecated(forRemoval = true, since = "2026")
     public void setLastPose(Pose2d lastPose) {
         setLastPose(new Pose3d(lastPose));
     }
@@ -389,9 +428,11 @@ public class PhotonPoseEstimator {
      * provided in this overload.
      *
      * @param cameraResult The latest pipeline result from the camera
-     * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
      *     create the estimate.
+     * @deprecated Use individual estimation methods instead.
      */
+    @Deprecated(forRemoval = true, since = "2026")
     public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
         return update(cameraResult, Optional.empty(), Optional.empty());
     }
@@ -411,9 +452,11 @@ public class PhotonPoseEstimator {
      *     otherwise
      * @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
      *     otherwise
-     * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
      *     create the estimate.
+     * @deprecated Use individual estimation methods instead.
      */
+    @Deprecated(forRemoval = true, since = "2026")
     public Optional<EstimatedRobotPose> update(
             PhotonPipelineResult cameraResult,
             Optional<Matrix<N3, N3>> cameraMatrix,
@@ -436,9 +479,11 @@ public class PhotonPoseEstimator {
      * @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
      *     otherwise
      * @param constrainedPnpParams Constrained SolvePNP params, if needed.
-     * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
      *     create the estimate.
+     * @deprecated Use individual estimation methods instead.
      */
+    @Deprecated(forRemoval = true, since = "2026")
     public Optional<EstimatedRobotPose> update(
             PhotonPipelineResult cameraResult,
             Optional<Matrix<N3, N3>> cameraMatrix,
@@ -475,7 +520,7 @@ public class PhotonPoseEstimator {
      *
      * @param cameraResult The latest pipeline result from the camera
      * @param strategy The pose strategy to use. Can't be CONSTRAINED_SOLVEPNP.
-     * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
      *     create the estimate.
      */
     private Optional<EstimatedRobotPose> update(
@@ -491,21 +536,122 @@ public class PhotonPoseEstimator {
             PoseStrategy strategy) {
         Optional<EstimatedRobotPose> estimatedPose =
                 switch (strategy) {
-                    case LOWEST_AMBIGUITY -> lowestAmbiguityStrategy(cameraResult);
-                    case CLOSEST_TO_CAMERA_HEIGHT -> closestToCameraHeightStrategy(cameraResult);
+                    case LOWEST_AMBIGUITY -> estimateLowestAmbiguityPose(cameraResult);
+                    case CLOSEST_TO_CAMERA_HEIGHT -> estimateClosestToCameraHeightPose(cameraResult);
                     case CLOSEST_TO_REFERENCE_POSE ->
-                            closestToReferencePoseStrategy(cameraResult, referencePose);
+                            estimateClosestToReferencePose(cameraResult, referencePose);
                     case CLOSEST_TO_LAST_POSE -> {
                         setReferencePose(lastPose);
-                        yield closestToReferencePoseStrategy(cameraResult, referencePose);
+                        yield estimateClosestToReferencePose(cameraResult, referencePose);
+                    }
+                    case AVERAGE_BEST_TARGETS -> estimateAverageBestTargetsPose(cameraResult);
+                    case MULTI_TAG_PNP_ON_RIO -> {
+                        if (cameraMatrix.isEmpty() || distCoeffs.isEmpty()) {
+                            DriverStation.reportWarning(
+                                    "No camera calibration data provided for multi-tag-on-rio",
+                                    Thread.currentThread().getStackTrace());
+                            yield update(cameraResult, this.multiTagFallbackStrategy);
+                        }
+                        var res = estimateRioMultiTagPose(cameraResult, cameraMatrix.get(), distCoeffs.get());
+                        if (res.isEmpty()) {
+                            yield update(
+                                    cameraResult,
+                                    cameraMatrix,
+                                    distCoeffs,
+                                    constrainedPnpParams,
+                                    this.multiTagFallbackStrategy);
+                        }
+                        yield res;
+                    }
+                    case MULTI_TAG_PNP_ON_COPROCESSOR -> {
+                        if (cameraResult.getMultiTagResult().isEmpty()) {
+                            yield update(cameraResult, this.multiTagFallbackStrategy);
+                        }
+                        yield estimateCoprocMultiTagPose(cameraResult);
+                    }
+                    case PNP_DISTANCE_TRIG_SOLVE -> estimatePnpDistanceTrigSolvePose(cameraResult);
+                    case CONSTRAINED_SOLVEPNP -> {
+                        boolean hasCalibData = cameraMatrix.isPresent() && distCoeffs.isPresent();
+                        // cannot run multitagPNP, use fallback strategy
+                        if (!hasCalibData) {
+                            yield update(
+                                    cameraResult,
+                                    cameraMatrix,
+                                    distCoeffs,
+                                    Optional.empty(),
+                                    this.multiTagFallbackStrategy);
+                        }
+
+                        if (constrainedPnpParams.isEmpty()) {
+                            yield Optional.empty();
+                        }
+
+                        // Need heading if heading fixed
+                        if (!constrainedPnpParams.get().headingFree
+                                && headingBuffer.getSample(cameraResult.getTimestampSeconds()).isEmpty()) {
+                            yield update(
+                                    cameraResult,
+                                    cameraMatrix,
+                                    distCoeffs,
+                                    Optional.empty(),
+                                    this.multiTagFallbackStrategy);
+                        }
+
+                        Pose3d fieldToRobotSeed;
+                        // Attempt to use multi-tag to get a pose estimate seed
+                        if (cameraResult.getMultiTagResult().isPresent()) {
+                            fieldToRobotSeed =
+                                    Pose3d.kZero.plus(
+                                            cameraResult
+                                                    .getMultiTagResult()
+                                                    .get()
+                                                    .estimatedPose
+                                                    .best
+                                                    .plus(robotToCamera.inverse()));
+                        } else {
+                            // HACK - use fallback strategy to gimme a seed pose
+                            // TODO - make sure nested update doesn't break state
+                            var nestedUpdate =
+                                    update(
+                                            cameraResult,
+                                            cameraMatrix,
+                                            distCoeffs,
+                                            Optional.empty(),
+                                            this.multiTagFallbackStrategy);
+                            if (nestedUpdate.isEmpty()) {
+                                // best i can do is bail
+                                yield Optional.empty();
+                            }
+                            fieldToRobotSeed = nestedUpdate.get().estimatedPose;
+                        }
+
+                        if (!constrainedPnpParams.get().headingFree) {
+                            // If heading fixed, force rotation component
+                            fieldToRobotSeed =
+                                    new Pose3d(
+                                            fieldToRobotSeed.getTranslation(),
+                                            new Rotation3d(
+                                                    headingBuffer.getSample(cameraResult.getTimestampSeconds()).get()));
+                        }
+
+                        var pnpResult =
+                                estimateConstrainedSolvepnpPose(
+                                        cameraResult,
+                                        cameraMatrix.get(),
+                                        distCoeffs.get(),
+                                        fieldToRobotSeed,
+                                        constrainedPnpParams.get().headingFree,
+                                        constrainedPnpParams.get().headingScaleFactor);
+                        if (!pnpResult.isPresent()) {
+                            yield update(
+                                    cameraResult,
+                                    cameraMatrix,
+                                    distCoeffs,
+                                    Optional.empty(),
+                                    this.multiTagFallbackStrategy);
+                        }
+                        yield pnpResult;
                     }
-                    case AVERAGE_BEST_TARGETS -> averageBestTargetsStrategy(cameraResult);
-                    case MULTI_TAG_PNP_ON_RIO ->
-                            multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
-                    case MULTI_TAG_PNP_ON_COPROCESSOR -> multiTagOnCoprocStrategy(cameraResult);
-                    case PNP_DISTANCE_TRIG_SOLVE -> pnpDistanceTrigSolveStrategy(cameraResult);
-                    case CONSTRAINED_SOLVEPNP ->
-                            constrainedPnpStrategy(cameraResult, cameraMatrix, distCoeffs, constrainedPnpParams);
                 };
 
         if (estimatedPose.isPresent()) {
@@ -515,12 +661,43 @@ public class PhotonPoseEstimator {
         return estimatedPose;
     }
 
-    private Optional<EstimatedRobotPose> pnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
-        PhotonTrackedTarget bestTarget = result.getBestTarget();
+    /**
+     * @param cameraResult A pipeline result from the camera.
+     * @return Whether or not pose estimation should be performed.
+     */
+    private boolean shouldEstimate(PhotonPipelineResult cameraResult) {
+        // Time in the past -- give up, since the following if expects times > 0
+        if (cameraResult.getTimestampSeconds() < 0) {
+            return false;
+        }
+
+        // If no targets seen, trivial case -- can't do estimation
+        return cameraResult.hasTargets();
+    }
+
+    /**
+     * Return the estimated position of the robot by using distance data from best visible tag to
+     * compute a Pose. This runs on the RoboRIO in order to access the robot's yaw heading, and MUST
+     * have addHeadingData called every frame so heading data is up-to-date.
+     *
+     * <p>Yields a Pose2d in estimatedRobotPose (0 for z, roll, pitch)
+     *
+     * <p>https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
+     *
+     * @param cameraResult A pipeline result from the camera.
+     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     *     create the estimate.
+     */
+    public Optional<EstimatedRobotPose> estimatePnpDistanceTrigSolvePose(
+            PhotonPipelineResult cameraResult) {
+        if (!shouldEstimate(cameraResult)) {
+            return Optional.empty();
+        }
+        PhotonTrackedTarget bestTarget = cameraResult.getBestTarget();
 
         if (bestTarget == null) return Optional.empty();
 
-        var headingSampleOpt = headingBuffer.getSample(result.getTimestampSeconds());
+        var headingSampleOpt = headingBuffer.getSample(cameraResult.getTimestampSeconds());
         if (headingSampleOpt.isEmpty()) {
             return Optional.empty();
         }
@@ -555,98 +732,82 @@ public class PhotonPoseEstimator {
         return Optional.of(
                 new EstimatedRobotPose(
                         new Pose3d(robotPose),
-                        result.getTimestampSeconds(),
-                        result.getTargets(),
+                        cameraResult.getTimestampSeconds(),
+                        cameraResult.getTargets(),
                         PoseStrategy.PNP_DISTANCE_TRIG_SOLVE));
     }
 
-    private Optional<EstimatedRobotPose> constrainedPnpStrategy(
-            PhotonPipelineResult result,
-            Optional<Matrix<N3, N3>> cameraMatrixOpt,
-            Optional<Matrix<N8, N1>> distCoeffsOpt,
-            Optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
-        boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
-        // cannot run multitagPNP, use fallback strategy
-        if (!hasCalibData) {
-            return update(
-                    result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
-        }
-
-        if (constrainedPnpParams.isEmpty()) {
+    /**
+     * Return the estimated position of the robot by solving a constrained version of the
+     * Perspective-n-Point problem with the robot's drivebase flat on the floor. This computation
+     * takes place on the RoboRIO, and typically takes not more than 2ms. See {@link
+     * org.photonvision.jni.ConstrainedSolvepnpJni} for tuning handles this strategy exposes.
+     * Internally, the cost function is a sum-squared of pixel reprojection error + (optionally)
+     * heading error * heading scale factor. This strategy needs addHeadingData called every frame so
+     * heading data is up-to-date.
+     *
+     * @param cameraResult A pipeline result from the camera.
+     * @param cameraMatrix Camera intrinsics from camera calibration data.
+     * @param distCoeffs Distortion coefficients from camera calibration data.
+     * @param seedPose An initial guess at robot pose, refined via optimization. Better guesses will
+     *     converge faster. Can come from any pose source, but some battle-tested sources are {@link
+     *     #estimateCoprocMultiTagPose(PhotonPipelineResult)}, or {@link
+     *     #estimateLowestAmbiguityPose(PhotonPipelineResult)} if MultiTag results aren't available.
+     * @param headingFree If true, heading is completely free to vary. If false, heading excursions
+     *     from the provided heading measurement will be penalized
+     * @param headingScaleFactor If headingFree is false, this weights the cost of changing our robot
+     *     heading estimate against the tag corner reprojection error cont.
+     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     *     create the estimate.
+     */
+    public Optional<EstimatedRobotPose> estimateConstrainedSolvepnpPose(
+            PhotonPipelineResult cameraResult,
+            Matrix<N3, N3> cameraMatrix,
+            Matrix<N8, N1> distCoeffs,
+            Pose3d seedPose,
+            boolean headingFree,
+            double headingScaleFactor) {
+        if (!shouldEstimate(cameraResult)) {
             return Optional.empty();
         }
-
-        // Need heading if heading fixed
-        if (!constrainedPnpParams.get().headingFree
-                && headingBuffer.getSample(result.getTimestampSeconds()).isEmpty()) {
-            return update(
-                    result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
-        }
-
-        Pose3d fieldToRobotSeed;
-
-        // Attempt to use multi-tag to get a pose estimate seed
-        if (result.getMultiTagResult().isPresent()) {
-            fieldToRobotSeed =
-                    Pose3d.kZero.plus(
-                            result.getMultiTagResult().get().estimatedPose.best.plus(robotToCamera.inverse()));
-        } else {
-            // HACK - use fallback strategy to gimme a seed pose
-            // TODO - make sure nested update doesn't break state
-            var nestedUpdate =
-                    update(
-                            result,
-                            cameraMatrixOpt,
-                            distCoeffsOpt,
-                            Optional.empty(),
-                            this.multiTagFallbackStrategy);
-            if (nestedUpdate.isEmpty()) {
-                // best i can do is bail
-                return Optional.empty();
-            }
-            fieldToRobotSeed = nestedUpdate.get().estimatedPose;
-        }
-
-        if (!constrainedPnpParams.get().headingFree) {
-            // If heading fixed, force rotation component
-            fieldToRobotSeed =
-                    new Pose3d(
-                            fieldToRobotSeed.getTranslation(),
-                            new Rotation3d(headingBuffer.getSample(result.getTimestampSeconds()).get()));
-        }
-
         var pnpResult =
                 VisionEstimation.estimateRobotPoseConstrainedSolvepnp(
-                        cameraMatrixOpt.get(),
-                        distCoeffsOpt.get(),
-                        result.getTargets(),
+                        cameraMatrix,
+                        distCoeffs,
+                        cameraResult.getTargets(),
                         robotToCamera,
-                        fieldToRobotSeed,
+                        seedPose,
                         fieldTags,
                         tagModel,
-                        constrainedPnpParams.get().headingFree,
-                        headingBuffer.getSample(result.getTimestampSeconds()).get(),
-                        constrainedPnpParams.get().headingScaleFactor);
-        // try fallback strategy if solvePNP fails for some reason
-        if (!pnpResult.isPresent())
-            return update(
-                    result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
+                        headingFree,
+                        headingBuffer.getSample(cameraResult.getTimestampSeconds()).get(),
+                        headingScaleFactor);
+        if (!pnpResult.isPresent()) return Optional.empty();
         var best = Pose3d.kZero.plus(pnpResult.get().best); // field-to-robot
 
         return Optional.of(
                 new EstimatedRobotPose(
                         best,
-                        result.getTimestampSeconds(),
-                        result.getTargets(),
+                        cameraResult.getTimestampSeconds(),
+                        cameraResult.getTargets(),
                         PoseStrategy.CONSTRAINED_SOLVEPNP));
     }
 
-    private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
-        if (result.getMultiTagResult().isEmpty()) {
-            return update(result, this.multiTagFallbackStrategy);
+    /**
+     * Return the estimated position of the robot by using all visible tags to compute a single pose
+     * estimate on coprocessor. This option needs to be enabled on the PhotonVision web UI as well.
+     *
+     * @param cameraResult A pipeline result from the camera.
+     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     *     create the estimate.
+     */
+    public Optional<EstimatedRobotPose> estimateCoprocMultiTagPose(
+            PhotonPipelineResult cameraResult) {
+        if (cameraResult.getMultiTagResult().isEmpty() || !shouldEstimate(cameraResult)) {
+            return Optional.empty();
         }
 
-        var best_tf = result.getMultiTagResult().get().estimatedPose.best;
+        var best_tf = cameraResult.getMultiTagResult().get().estimatedPose.best;
         var best =
                 Pose3d.kZero
                         .plus(best_tf) // field-to-camera
@@ -655,33 +816,31 @@ public class PhotonPoseEstimator {
         return Optional.of(
                 new EstimatedRobotPose(
                         best,
-                        result.getTimestampSeconds(),
-                        result.getTargets(),
+                        cameraResult.getTimestampSeconds(),
+                        cameraResult.getTargets(),
                         PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
     }
 
-    private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
-            PhotonPipelineResult result,
-            Optional<Matrix<N3, N3>> cameraMatrixOpt,
-            Optional<Matrix<N8, N1>> distCoeffsOpt) {
-        if (cameraMatrixOpt.isEmpty() || distCoeffsOpt.isEmpty()) {
-            DriverStation.reportWarning(
-                    "No camera calibration data provided for multi-tag-on-rio",
-                    Thread.currentThread().getStackTrace());
-            return update(result, this.multiTagFallbackStrategy);
-        }
-
-        if (result.getTargets().size() < 2) {
-            return update(result, this.multiTagFallbackStrategy);
+    /**
+     * Return the estimated position of the robot by using all visible tags to compute a single pose
+     * estimate on the RoboRIO. This can take a lot of time due to the RIO's weak computing power.
+     *
+     * @param cameraResult A pipeline result from the camera.
+     * @param cameraMatrix Camera intrinsics from camera calibration data
+     * @param distCoeffs Distortion coefficients from camera calibration data.
+     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     *     create the estimate.
+     */
+    public Optional<EstimatedRobotPose> estimateRioMultiTagPose(
+            PhotonPipelineResult cameraResult, Matrix<N3, N3> cameraMatrix, Matrix<N8, N1> distCoeffs) {
+        if (cameraResult.getTargets().size() < 2 || !shouldEstimate(cameraResult)) {
+            return Optional.empty();
         }
 
         var pnpResult =
                 VisionEstimation.estimateCamPosePNP(
-                        cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
-        // try fallback strategy if solvePNP fails for some reason
-        if (!pnpResult.isPresent())
-            return update(
-                    result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
+                        cameraMatrix, distCoeffs, cameraResult.getTargets(), fieldTags, tagModel);
+        if (!pnpResult.isPresent()) return Optional.empty();
 
         var best =
                 Pose3d.kZero
@@ -691,25 +850,29 @@ public class PhotonPoseEstimator {
         return Optional.of(
                 new EstimatedRobotPose(
                         best,
-                        result.getTimestampSeconds(),
-                        result.getTargets(),
+                        cameraResult.getTimestampSeconds(),
+                        cameraResult.getTargets(),
                         PoseStrategy.MULTI_TAG_PNP_ON_RIO));
     }
 
     /**
-     * Return the estimated position of the robot with the lowest position ambiguity from a List of
-     * pipeline results.
+     * Return the estimated position of the robot with the lowest position ambiguity from a pipeline
+     * result.
      *
-     * @param result pipeline result
-     * @return the estimated position of the robot in the FCS and the estimated timestamp of this
-     *     estimation.
+     * @param cameraResult A pipeline result from the camera.
+     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     *     create the estimate.
      */
-    private Optional<EstimatedRobotPose> lowestAmbiguityStrategy(PhotonPipelineResult result) {
+    public Optional<EstimatedRobotPose> estimateLowestAmbiguityPose(
+            PhotonPipelineResult cameraResult) {
+        if (!shouldEstimate(cameraResult)) {
+            return Optional.empty();
+        }
         PhotonTrackedTarget lowestAmbiguityTarget = null;
 
         double lowestAmbiguityScore = 10;
 
-        for (PhotonTrackedTarget target : result.targets) {
+        for (PhotonTrackedTarget target : cameraResult.targets) {
             double targetPoseAmbiguity = target.getPoseAmbiguity();
             // Make sure the target is a Fiducial target.
             if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) {
@@ -737,8 +900,8 @@ public class PhotonPoseEstimator {
                                 .get()
                                 .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse())
                                 .transformBy(robotToCamera.inverse()),
-                        result.getTimestampSeconds(),
-                        result.getTargets(),
+                        cameraResult.getTimestampSeconds(),
+                        cameraResult.getTargets(),
                         PoseStrategy.LOWEST_AMBIGUITY));
     }
 
@@ -746,15 +909,19 @@ public class PhotonPoseEstimator {
      * Return the estimated position of the robot using the target with the lowest delta height
      * difference between the estimated and actual height of the camera.
      *
-     * @param result pipeline result
-     * @return the estimated position of the robot in the FCS and the estimated timestamp of this
-     *     estimation.
+     * @param cameraResult A pipeline result from the camera.
+     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     *     create the estimate.
      */
-    private Optional<EstimatedRobotPose> closestToCameraHeightStrategy(PhotonPipelineResult result) {
+    public Optional<EstimatedRobotPose> estimateClosestToCameraHeightPose(
+            PhotonPipelineResult cameraResult) {
+        if (!shouldEstimate(cameraResult)) {
+            return Optional.empty();
+        }
         double smallestHeightDifference = 10e9;
         EstimatedRobotPose closestHeightTarget = null;
 
-        for (PhotonTrackedTarget target : result.targets) {
+        for (PhotonTrackedTarget target : cameraResult.targets) {
             int targetFiducialId = target.getFiducialId();
 
             // Don't report errors for non-fiducial targets. This could also be resolved by
@@ -792,8 +959,8 @@ public class PhotonPoseEstimator {
                                         .get()
                                         .transformBy(target.getAlternateCameraToTarget().inverse())
                                         .transformBy(robotToCamera.inverse()),
-                                result.getTimestampSeconds(),
-                                result.getTargets(),
+                                cameraResult.getTimestampSeconds(),
+                                cameraResult.getTargets(),
                                 PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
             }
 
@@ -805,8 +972,8 @@ public class PhotonPoseEstimator {
                                         .get()
                                         .transformBy(target.getBestCameraToTarget().inverse())
                                         .transformBy(robotToCamera.inverse()),
-                                result.getTimestampSeconds(),
-                                result.getTargets(),
+                                cameraResult.getTimestampSeconds(),
+                                cameraResult.getTargets(),
                                 PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
             }
         }
@@ -819,13 +986,16 @@ public class PhotonPoseEstimator {
      * Return the estimated position of the robot using the target with the lowest delta in the vector
      * magnitude between it and the reference pose.
      *
-     * @param result pipeline result
+     * @param cameraResult A pipeline result from the camera.
      * @param referencePose reference pose to check vector magnitude difference against.
-     * @return the estimated position of the robot in the FCS and the estimated timestamp of this
-     *     estimation.
+     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     *     create the estimate.
      */
-    private Optional<EstimatedRobotPose> closestToReferencePoseStrategy(
-            PhotonPipelineResult result, Pose3d referencePose) {
+    public Optional<EstimatedRobotPose> estimateClosestToReferencePose(
+            PhotonPipelineResult cameraResult, Pose3d referencePose) {
+        if (!shouldEstimate(cameraResult)) {
+            return Optional.empty();
+        }
         if (referencePose == null) {
             DriverStation.reportError(
                     "[PhotonPoseEstimator] Tried to use reference pose strategy without setting the reference!",
@@ -836,7 +1006,7 @@ public class PhotonPoseEstimator {
         double smallestPoseDelta = 10e9;
         EstimatedRobotPose lowestDeltaPose = null;
 
-        for (PhotonTrackedTarget target : result.targets) {
+        for (PhotonTrackedTarget target : cameraResult.targets) {
             int targetFiducialId = target.getFiducialId();
 
             // Don't report errors for non-fiducial targets. This could also be resolved by
@@ -870,8 +1040,8 @@ public class PhotonPoseEstimator {
                 lowestDeltaPose =
                         new EstimatedRobotPose(
                                 altTransformPosition,
-                                result.getTimestampSeconds(),
-                                result.getTargets(),
+                                cameraResult.getTimestampSeconds(),
+                                cameraResult.getTargets(),
                                 PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
             }
             if (bestDifference < smallestPoseDelta) {
@@ -879,8 +1049,8 @@ public class PhotonPoseEstimator {
                 lowestDeltaPose =
                         new EstimatedRobotPose(
                                 bestTransformPosition,
-                                result.getTimestampSeconds(),
-                                result.getTargets(),
+                                cameraResult.getTimestampSeconds(),
+                                cameraResult.getTargets(),
                                 PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
             }
         }
@@ -890,15 +1060,19 @@ public class PhotonPoseEstimator {
     /**
      * Return the average of the best target poses using ambiguity as weight.
      *
-     * @param result pipeline result
-     * @return the estimated position of the robot in the FCS and the estimated timestamp of this
-     *     estimation.
+     * @param cameraResult A pipeline result from the camera.
+     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     *     create the estimate.
      */
-    private Optional<EstimatedRobotPose> averageBestTargetsStrategy(PhotonPipelineResult result) {
+    public Optional<EstimatedRobotPose> estimateAverageBestTargetsPose(
+            PhotonPipelineResult cameraResult) {
+        if (!shouldEstimate(cameraResult)) {
+            return Optional.empty();
+        }
         List<Pair<PhotonTrackedTarget, Pose3d>> estimatedRobotPoses = new ArrayList<>();
         double totalAmbiguity = 0;
 
-        for (PhotonTrackedTarget target : result.targets) {
+        for (PhotonTrackedTarget target : cameraResult.targets) {
             int targetFiducialId = target.getFiducialId();
 
             // Don't report errors for non-fiducial targets. This could also be resolved by
@@ -923,8 +1097,8 @@ public class PhotonPoseEstimator {
                                         .get()
                                         .transformBy(target.getBestCameraToTarget().inverse())
                                         .transformBy(robotToCamera.inverse()),
-                                result.getTimestampSeconds(),
-                                result.getTargets(),
+                                cameraResult.getTimestampSeconds(),
+                                cameraResult.getTargets(),
                                 PoseStrategy.AVERAGE_BEST_TARGETS));
             }
 
@@ -958,8 +1132,8 @@ public class PhotonPoseEstimator {
         return Optional.of(
                 new EstimatedRobotPose(
                         new Pose3d(transform, rotation),
-                        result.getTimestampSeconds(),
-                        result.getTargets(),
+                        cameraResult.getTimestampSeconds(),
+                        cameraResult.getTargets(),
                         PoseStrategy.AVERAGE_BEST_TARGETS));
     }
 
diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
index 285c64b6b..fd9c1ea17 100644
--- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
+++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
@@ -24,13 +24,9 @@
 
 #include "photon/PhotonPoseEstimator.h"
 
-#include <cmath>
-#include <iostream>
 #include <limits>
-#include <map>
-#include <memory>
+#include <optional>
 #include <span>
-#include <string>
 #include <utility>
 #include <vector>
 
@@ -46,6 +42,7 @@
 #include <units/angle.h>
 #include <units/math.h>
 #include <units/time.h>
+#include <wpi/deprecated.h>
 
 #include "photon/PhotonCamera.h"
 #include "photon/estimation/TargetModel.h"
@@ -55,7 +52,7 @@
 
 #define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT
 #include <opencv2/core/eigen.hpp>
-
+WPI_IGNORE_DEPRECATED
 namespace photon {
 
 namespace detail {
@@ -67,6 +64,19 @@ cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX,
                                    units::meter_t cornerY, frc::Pose3d tagPose);
 }  // namespace detail
 
+PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
+                                         frc::Transform3d robotToCamera)
+    : aprilTags(tags),
+      m_robotToCamera(robotToCamera),
+      lastPose(frc::Pose3d()),
+      referencePose(frc::Pose3d()),
+      poseCacheTimestamp(-1_s),
+      headingBuffer(frc::TimeInterpolatableBuffer<frc::Rotation2d>(1_s)) {
+  HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator,
+             InstanceCount);
+  InstanceCount++;
+}
+
 PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
                                          PoseStrategy strat,
                                          frc::Transform3d robotToCamera)
@@ -138,39 +148,94 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
 
   switch (strategy) {
     case LOWEST_AMBIGUITY:
-      ret = LowestAmbiguityStrategy(result);
+      ret = EstimateLowestAmbiguityPose(result);
       break;
     case CLOSEST_TO_CAMERA_HEIGHT:
-      ret = ClosestToCameraHeightStrategy(result);
+      ret = EstimateClosestToCameraHeightPose(result);
       break;
     case CLOSEST_TO_REFERENCE_POSE:
-      ret = ClosestToReferencePoseStrategy(result);
+      ret = EstimateClosestToReferencePose(result, this->referencePose);
       break;
     case CLOSEST_TO_LAST_POSE:
       SetReferencePose(lastPose);
-      ret = ClosestToReferencePoseStrategy(result);
+      ret = EstimateClosestToReferencePose(result, this->referencePose);
       break;
     case AVERAGE_BEST_TARGETS:
-      ret = AverageBestTargetsStrategy(result);
+      ret = EstimateAverageBestTargetsPose(result);
       break;
     case MULTI_TAG_PNP_ON_COPROCESSOR:
-      ret = MultiTagOnCoprocStrategy(result);
+      if (!result.MultiTagResult()) {
+        ret = Update(result, this->multiTagFallbackStrategy);
+      } else {
+        ret = EstimateCoprocMultiTagPose(result);
+      }
       break;
     case MULTI_TAG_PNP_ON_RIO:
-      if (cameraMatrixData && cameraDistCoeffs) {
-        ret = MultiTagOnRioStrategy(result, cameraMatrixData, cameraDistCoeffs);
-      } else {
+      if (!cameraMatrixData && !cameraDistCoeffs) {
         FRC_ReportError(frc::warn::Warning,
                         "No camera calibration provided to multi-tag-on-rio!",
                         "");
+        ret = Update(result, this->multiTagFallbackStrategy);
+      }
+      ret =
+          EstimateRioMultiTagPose(result, *cameraMatrixData, *cameraDistCoeffs);
+      if (!ret) {
+        ret = Update(result, this->multiTagFallbackStrategy);
       }
       break;
-    case CONSTRAINED_SOLVEPNP:
-      ret = ConstrainedPnpStrategy(result, cameraMatrixData, cameraDistCoeffs,
-                                   constrainedPnpParams);
+    case CONSTRAINED_SOLVEPNP: {
+      using namespace frc;
+
+      if (!cameraMatrixData || !cameraDistCoeffs) {
+        FRC_ReportError(
+            frc::warn::Warning,
+            "No camera calibration data provided for Constrained SolvePnP!");
+        ret = Update(result, this->multiTagFallbackStrategy);
+        break;
+      }
+
+      if (!constrainedPnpParams) {
+        return {};
+      }
+
+      if (!constrainedPnpParams->headingFree &&
+          !headingBuffer.Sample(result.GetTimestamp()).has_value()) {
+        ret = Update(result, cameraMatrixData, cameraDistCoeffs, {},
+                     this->multiTagFallbackStrategy);
+        break;
+      }
+
+      frc::Pose3d fieldToRobotSeed;
+
+      if (result.MultiTagResult().has_value()) {
+        fieldToRobotSeed =
+            frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
+                             m_robotToCamera.Inverse());
+      } else {
+        std::optional<EstimatedRobotPose> nestedUpdate =
+            Update(result, cameraMatrixData, cameraDistCoeffs, {},
+                   this->multiTagFallbackStrategy);
+
+        if (!nestedUpdate.has_value()) {
+          return {};
+        }
+
+        fieldToRobotSeed = nestedUpdate->estimatedPose;
+      }
+
+      ret = EstimateConstrainedSolvepnpPose(
+          result, *cameraMatrixData, *cameraDistCoeffs, fieldToRobotSeed,
+          constrainedPnpParams->headingFree,
+          constrainedPnpParams->headingScalingFactor);
+
+      if (!ret) {
+        ret = Update(result, cameraMatrixData, cameraDistCoeffs, {},
+                     this->multiTagFallbackStrategy);
+      }
       break;
+    }
     case PNP_DISTANCE_TRIG_SOLVE:
-      ret = PnpDistanceTrigSolveStrategy(result);
+      ret = EstimatePnpDistanceTrigSolvePose(result);
       break;
     default:
       FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
@@ -184,10 +249,26 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
   return ret;
 }
 
-std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
-    PhotonPipelineResult result) {
+bool ShouldEstimate(const PhotonPipelineResult& result) {
+  // Time in the past -- give up, since the following if expects times > 0
+  if (result.GetTimestamp() < 0_s) {
+    FRC_ReportError(frc::warn::Warning,
+                    "Result timestamp was reported in the past!");
+    return false;
+  }
+
+  // If no targets seen, trivial case -- can't do estimation
+  return result.HasTargets();
+}
+
+std::optional<EstimatedRobotPose>
+PhotonPoseEstimator::EstimateLowestAmbiguityPose(
+    PhotonPipelineResult cameraResult) {
+  if (!ShouldEstimate(cameraResult)) {
+    return std::nullopt;
+  }
   double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
-  auto targets = result.GetTargets();
+  auto targets = cameraResult.GetTargets();
   auto foundIt = targets.end();
   for (auto it = targets.begin(); it != targets.end(); ++it) {
     if (it->GetPoseAmbiguity() < lowestAmbiguityScore) {
@@ -214,18 +295,21 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
   return EstimatedRobotPose{
       fiducialPose->TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
           .TransformBy(m_robotToCamera.Inverse()),
-      result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY};
+      cameraResult.GetTimestamp(), cameraResult.GetTargets(), LOWEST_AMBIGUITY};
 }
 
 std::optional<EstimatedRobotPose>
-PhotonPoseEstimator::ClosestToCameraHeightStrategy(
-    PhotonPipelineResult result) {
+PhotonPoseEstimator::EstimateClosestToCameraHeightPose(
+    PhotonPipelineResult cameraResult) {
+  if (!ShouldEstimate(cameraResult)) {
+    return std::nullopt;
+  }
   units::meter_t smallestHeightDifference =
       units::meter_t(std::numeric_limits<double>::infinity());
 
   std::optional<EstimatedRobotPose> pose = std::nullopt;
 
-  for (auto& target : result.GetTargets()) {
+  for (auto& target : cameraResult.GetTargets()) {
     std::optional<frc::Pose3d> fiducialPose =
         aprilTags.GetTagPose(target.GetFiducialId());
     if (!fiducialPose) {
@@ -250,14 +334,16 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
       pose = EstimatedRobotPose{
           targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
               .TransformBy(m_robotToCamera.Inverse()),
-          result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
+          cameraResult.GetTimestamp(), cameraResult.GetTargets(),
+          CLOSEST_TO_CAMERA_HEIGHT};
     }
     if (bestDifference < smallestHeightDifference) {
       smallestHeightDifference = bestDifference;
       pose = EstimatedRobotPose{
           targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
               .TransformBy(m_robotToCamera.Inverse()),
-          result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
+          cameraResult.GetTimestamp(), cameraResult.GetTargets(),
+          CLOSEST_TO_CAMERA_HEIGHT};
     }
   }
 
@@ -265,14 +351,17 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
 }
 
 std::optional<EstimatedRobotPose>
-PhotonPoseEstimator::ClosestToReferencePoseStrategy(
-    PhotonPipelineResult result) {
+PhotonPoseEstimator::EstimateClosestToReferencePose(
+    PhotonPipelineResult cameraResult, frc::Pose3d referencePose) {
+  if (!ShouldEstimate(cameraResult)) {
+    return std::nullopt;
+  }
   units::meter_t smallestDifference =
       units::meter_t(std::numeric_limits<double>::infinity());
   units::second_t stateTimestamp = units::second_t(0);
   frc::Pose3d pose = lastPose;
 
-  auto targets = result.GetTargets();
+  auto targets = cameraResult.GetTargets();
   for (auto& target : targets) {
     std::optional<frc::Pose3d> fiducialPose =
         aprilTags.GetTagPose(target.GetFiducialId());
@@ -298,17 +387,17 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
     if (alternativeDifference < smallestDifference) {
       smallestDifference = alternativeDifference;
       pose = altPose;
-      stateTimestamp = result.GetTimestamp();
+      stateTimestamp = cameraResult.GetTimestamp();
     }
 
     if (bestDifference < smallestDifference) {
       smallestDifference = bestDifference;
       pose = bestPose;
-      stateTimestamp = result.GetTimestamp();
+      stateTimestamp = cameraResult.GetTimestamp();
     }
   }
 
-  return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets(),
+  return EstimatedRobotPose{pose, stateTimestamp, cameraResult.GetTargets(),
                             CLOSEST_TO_REFERENCE_POSE};
 }
 
@@ -361,41 +450,31 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
                 Rotation3d(rv));
 }
 
-std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
-    PhotonPipelineResult result) {
-  if (!result.MultiTagResult()) {
-    return Update(result, this->multiTagFallbackStrategy);
+std::optional<EstimatedRobotPose>
+PhotonPoseEstimator::EstimateCoprocMultiTagPose(
+    PhotonPipelineResult cameraResult) {
+  if (!cameraResult.MultiTagResult() || !ShouldEstimate(cameraResult)) {
+    return std::nullopt;
   }
 
-  const auto field2camera = result.MultiTagResult()->estimatedPose.best;
+  const auto field2camera = cameraResult.MultiTagResult()->estimatedPose.best;
 
   const auto fieldToRobot =
       frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
-  return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
-                                    result.GetTargets(),
+  return photon::EstimatedRobotPose(fieldToRobot, cameraResult.GetTimestamp(),
+                                    cameraResult.GetTargets(),
                                     MULTI_TAG_PNP_ON_COPROCESSOR);
 }
 
-std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
-    PhotonPipelineResult result,
-    std::optional<PhotonCamera::CameraMatrix> camMat,
-    std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
-  using namespace frc;
-
-  if (!camMat || !distCoeffs) {
-    FRC_ReportError(frc::warn::Warning,
-                    "No camera calibration data provided to "
-                    "PhotonPoseEstimator::MultiTagOnRioStrategy!",
-                    "");
-    return Update(result, this->multiTagFallbackStrategy);
-  }
-
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::EstimateRioMultiTagPose(
+    PhotonPipelineResult cameraResult, PhotonCamera::CameraMatrix cameraMatrix,
+    PhotonCamera::DistortionMatrix distCoeffs) {
   // Need at least 2 targets
-  if (!result.HasTargets() || result.GetTargets().size() < 2) {
-    return Update(result, this->multiTagFallbackStrategy);
+  if (cameraResult.GetTargets().size() < 2 || !ShouldEstimate(cameraResult)) {
+    return std::nullopt;
   }
 
-  auto const targets = result.GetTargets();
+  const auto targets = cameraResult.GetTargets();
 
   // List of corners mapped from 3d space (meters) to the 2d camera screen
   // (pixels).
@@ -418,7 +497,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
 
   // We should only do multi-tag if at least 2 tags (* 4 corners/tag)
   if (imagePoints.size() < 8) {
-    return Update(result, this->multiTagFallbackStrategy);
+    return std::nullopt;
   }
 
   // Output mats for results
@@ -426,27 +505,31 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
   cv::Mat const tvec(3, 1, cv::DataType<double>::type);
 
   {
-    cv::Mat cameraMatCV(camMat->rows(), camMat->cols(), CV_64F);
-    cv::eigen2cv(*camMat, cameraMatCV);
-    cv::Mat distCoeffsMatCV(distCoeffs->rows(), distCoeffs->cols(), CV_64F);
-    cv::eigen2cv(*distCoeffs, distCoeffsMatCV);
+    cv::Mat cameraMatCV(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F);
+    cv::eigen2cv(cameraMatrix, cameraMatCV);
+    cv::Mat distCoeffsMatCV(distCoeffs.rows(), distCoeffs.cols(), CV_64F);
+    cv::eigen2cv(distCoeffs, distCoeffsMatCV);
 
     cv::solvePnP(objectPoints, imagePoints, cameraMatCV, distCoeffsMatCV, rvec,
                  tvec, false, cv::SOLVEPNP_SQPNP);
   }
 
-  const Pose3d pose = detail::ToPose3d(tvec, rvec);
+  const frc::Pose3d pose = detail::ToPose3d(tvec, rvec);
 
-  return photon::EstimatedRobotPose(pose.TransformBy(m_robotToCamera.Inverse()),
-                                    result.GetTimestamp(), result.GetTargets(),
-                                    MULTI_TAG_PNP_ON_RIO);
+  return photon::EstimatedRobotPose(
+      pose.TransformBy(m_robotToCamera.Inverse()), cameraResult.GetTimestamp(),
+      cameraResult.GetTargets(), MULTI_TAG_PNP_ON_RIO);
 }
 
 std::optional<EstimatedRobotPose>
-PhotonPoseEstimator::PnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
-  PhotonTrackedTarget bestTarget = result.GetBestTarget();
+PhotonPoseEstimator::EstimatePnpDistanceTrigSolvePose(
+    PhotonPipelineResult cameraResult) {
+  if (!ShouldEstimate(cameraResult)) {
+    return std::nullopt;
+  }
+  PhotonTrackedTarget bestTarget = cameraResult.GetBestTarget();
   std::optional<frc::Rotation2d> headingSampleOpt =
-      headingBuffer.Sample(result.GetTimestamp());
+      headingBuffer.Sample(cameraResult.GetTimestamp());
   if (!headingSampleOpt) {
     FRC_ReportError(frc::warn::Warning,
                     "There was no heading data! Use AddHeadingData to add it!");
@@ -485,17 +568,21 @@ PhotonPoseEstimator::PnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
   frc::Pose2d robotPose = frc::Pose2d(
       fieldToCameraTranslation + camToRobotTranslation, headingSample);
 
-  return EstimatedRobotPose{frc::Pose3d(robotPose), result.GetTimestamp(),
-                            result.GetTargets(), PNP_DISTANCE_TRIG_SOLVE};
+  return EstimatedRobotPose{frc::Pose3d(robotPose), cameraResult.GetTimestamp(),
+                            cameraResult.GetTargets(), PNP_DISTANCE_TRIG_SOLVE};
 }
 
 std::optional<EstimatedRobotPose>
-PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
+PhotonPoseEstimator::EstimateAverageBestTargetsPose(
+    PhotonPipelineResult cameraResult) {
+  if (!ShouldEstimate(cameraResult)) {
+    return std::nullopt;
+  }
   std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
       tempPoses;
   double totalAmbiguity = 0;
 
-  auto targets = result.GetTargets();
+  auto targets = cameraResult.GetTargets();
   for (auto& target : targets) {
     std::optional<frc::Pose3d> fiducialPose =
         aprilTags.GetTagPose(target.GetFiducialId());
@@ -512,13 +599,15 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
       return EstimatedRobotPose{
           targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
               .TransformBy(m_robotToCamera.Inverse()),
-          result.GetTimestamp(), result.GetTargets(), AVERAGE_BEST_TARGETS};
+          cameraResult.GetTimestamp(), cameraResult.GetTargets(),
+          AVERAGE_BEST_TARGETS};
     }
     totalAmbiguity += 1. / target.GetPoseAmbiguity();
 
     tempPoses.push_back(std::make_pair(
         targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
-        std::make_pair(target.GetPoseAmbiguity(), result.GetTimestamp())));
+        std::make_pair(target.GetPoseAmbiguity(),
+                       cameraResult.GetTimestamp())));
   }
 
   frc::Translation3d transform = frc::Translation3d();
@@ -532,77 +621,45 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
   }
 
   return EstimatedRobotPose{frc::Pose3d(transform, rotation),
-                            result.GetTimestamp(), result.GetTargets(),
-                            AVERAGE_BEST_TARGETS};
+                            cameraResult.GetTimestamp(),
+                            cameraResult.GetTargets(), AVERAGE_BEST_TARGETS};
 }
 
-std::optional<EstimatedRobotPose> PhotonPoseEstimator::ConstrainedPnpStrategy(
-    photon::PhotonPipelineResult result,
-    std::optional<photon::PhotonCamera::CameraMatrix> camMat,
-    std::optional<photon::PhotonCamera::DistortionMatrix> distCoeffs,
-    std::optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
-  using namespace frc;
-
-  if (!camMat || !distCoeffs) {
-    FRC_ReportError(frc::warn::Warning,
-                    "No camera calibration data provided to "
-                    "StrPoseEstimator::MultiTagOnRioStrategy!",
-                    "");
-    return Update(result, this->multiTagFallbackStrategy);
+std::optional<EstimatedRobotPose>
+PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
+    photon::PhotonPipelineResult cameraResult,
+    photon::PhotonCamera::CameraMatrix cameraMatrix,
+    photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose,
+    bool headingFree, double headingScaleFactor) {
+  if (!ShouldEstimate(cameraResult)) {
+    return std::nullopt;
+  }
+  if (!headingFree) {
+    seedPose = frc::Pose3d{
+        seedPose.Translation(),
+        frc::Rotation3d{
+            headingBuffer.Sample(cameraResult.GetTimestamp()).value()}};
   }
 
-  if (!constrainedPnpParams) {
-    return {};
-  }
-
-  if (!constrainedPnpParams->headingFree &&
-      !headingBuffer.Sample(result.GetTimestamp()).has_value()) {
-    return Update(result, camMat, distCoeffs, {},
-                  this->multiTagFallbackStrategy);
-  }
-
-  frc::Pose3d fieldToRobotSeed;
-
-  if (result.MultiTagResult().has_value()) {
-    fieldToRobotSeed =
-        frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
-                         m_robotToCamera.Inverse());
-  } else {
-    std::optional<EstimatedRobotPose> nestedUpdate =
-        Update(result, camMat, distCoeffs, {}, this->multiTagFallbackStrategy);
-
-    if (!nestedUpdate.has_value()) {
-      return {};
-    }
-
-    fieldToRobotSeed = nestedUpdate->estimatedPose;
-  }
-
-  if (!constrainedPnpParams.value().headingFree) {
-    fieldToRobotSeed = frc::Pose3d{
-        fieldToRobotSeed.Translation(),
-        frc::Rotation3d{headingBuffer.Sample(result.GetTimestamp()).value()}};
-  }
-
-  std::vector<photon::PhotonTrackedTarget> targets{result.GetTargets().begin(),
-                                                   result.GetTargets().end()};
+  std::vector<photon::PhotonTrackedTarget> targets{
+      cameraResult.GetTargets().begin(), cameraResult.GetTargets().end()};
 
   std::optional<photon::PnpResult> pnpResult =
       VisionEstimation::EstimateRobotPoseConstrainedSolvePNP(
-          camMat.value(), distCoeffs.value(), targets, m_robotToCamera,
-          fieldToRobotSeed, aprilTags, photon::kAprilTag36h11,
-          constrainedPnpParams->headingFree,
-          frc::Rotation2d{headingBuffer.Sample(result.GetTimestamp()).value()},
-          constrainedPnpParams->headingScalingFactor);
+          cameraMatrix, distCoeffs, targets, m_robotToCamera, seedPose,
+          aprilTags, photon::kAprilTag36h11, headingFree,
+          frc::Rotation2d{
+              headingBuffer.Sample(cameraResult.GetTimestamp()).value()},
+          headingScaleFactor);
 
   if (!pnpResult) {
-    return Update(result, camMat, distCoeffs, {},
-                  this->multiTagFallbackStrategy);
+    return std::nullopt;
   }
 
   frc::Pose3d best = frc::Pose3d{} + pnpResult->best;
 
-  return EstimatedRobotPose{best, result.GetTimestamp(), result.GetTargets(),
+  return EstimatedRobotPose{best, cameraResult.GetTimestamp(),
+                            cameraResult.GetTargets(),
                             PoseStrategy::CONSTRAINED_SOLVEPNP};
 }
 }  // namespace photon
diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
index f14865a15..d5c2501ec 100644
--- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
+++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
@@ -24,8 +24,6 @@
 
 #pragma once
 
-#include <memory>
-
 #include <frc/apriltag/AprilTagFieldLayout.h>
 #include <frc/geometry/Pose3d.h>
 #include <frc/geometry/Rotation3d.h>
@@ -34,11 +32,8 @@
 #include <opencv2/core/mat.hpp>
 
 #include "photon/PhotonCamera.h"
-#include "photon/dataflow/structures/Packet.h"
-#include "photon/targeting/MultiTargetPNPResult.h"
 #include "photon/targeting/PhotonPipelineResult.h"
 #include "photon/targeting/PhotonTrackedTarget.h"
-#include "photon/targeting/PnpResult.h"
 
 namespace photon {
 enum PoseStrategy {
@@ -93,10 +88,26 @@ class PhotonPoseEstimator {
    *
    * @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
    * respect to the FIRST field.
-   * @param strategy The strategy it should use to determine the best pose.
    * @param robotToCamera Transform3d from the center of the robot to the camera
    * mount positions (ie, robot ➔ camera).
    */
+  explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
+                               frc::Transform3d robotToCamera);
+
+  /**
+   * Create a new PhotonPoseEstimator.
+   *
+   * @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
+   * respect to the FIRST field.
+   * @param strategy The strategy it should use to determine the best pose.
+   * @param robotToCamera Transform3d from the center of the robot to the camera
+   * mount positions (ie, robot ➔ camera).
+   * @deprecated Use individual estimation methods with the 2 argument
+   * constructor instead.
+   */
+  [[deprecated(
+      "Use individual estimation methods with the 2 argument constructor "
+      "instead.")]]
   explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
                                PoseStrategy strategy,
                                frc::Transform3d robotToCamera);
@@ -112,14 +123,20 @@ class PhotonPoseEstimator {
    * Get the Position Estimation Strategy being used by the Position Estimator.
    *
    * @return the strategy
+   * @deprecated Use individual estimation methods instead.
    */
-  PoseStrategy GetPoseStrategy() const { return strategy; }
+  [[deprecated("Use individual estimation methods instead.")]]
+  PoseStrategy GetPoseStrategy() const {
+    return strategy;
+  }
 
   /**
    * Set the Position Estimation Strategy used by the Position Estimator.
    *
    * @param strategy the strategy to set
+   * @deprecated Use individual estimation methods instead.
    */
+  [[deprecated("Use individual estimation methods instead.")]]
   inline void SetPoseStrategy(PoseStrategy strat) {
     if (strategy != strat) {
       InvalidatePoseCache();
@@ -132,22 +149,30 @@ class PhotonPoseEstimator {
    * only one tag can be seen. Must NOT be MULTI_TAG_PNP
    *
    * @param strategy the strategy to set
+   * @deprecated Use individual estimation methods instead.
    */
+  [[deprecated("Use individual estimation methods instead.")]]
   void SetMultiTagFallbackStrategy(PoseStrategy strategy);
 
   /**
    * Return the reference position that is being used by the estimator.
    *
    * @return the referencePose
+   * @deprecated Use individual estimation methods instead.
    */
-  frc::Pose3d GetReferencePose() const { return referencePose; }
+  [[deprecated("Use individual estimation methods instead.")]]
+  frc::Pose3d GetReferencePose() const {
+    return referencePose;
+  }
 
   /**
    * Update the stored reference pose for use when using the
    * CLOSEST_TO_REFERENCE_POSE strategy.
    *
    * @param referencePose the referencePose to set
+   * @deprecated Use individual estimation methods instead.
    */
+  [[deprecated("Use individual estimation methods instead.")]]
   inline void SetReferencePose(frc::Pose3d referencePose) {
     if (this->referencePose != referencePose) {
       InvalidatePoseCache();
@@ -178,8 +203,12 @@ class PhotonPoseEstimator {
    * using the CLOSEST_TO_LAST_POSE strategy.
    *
    * @param lastPose the lastPose to set
+   * @deprecated Use individual estimation methods instead.
    */
-  inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
+  [[deprecated("Use individual estimation methods instead.")]]
+  inline void SetLastPose(frc::Pose3d lastPose) {
+    this->lastPose = lastPose;
+  }
 
   /**
    * Add robot heading data to the buffer. Must be called periodically for the
@@ -247,7 +276,9 @@ class PhotonPoseEstimator {
    * @param distCoeffsData The camera calibration distortion coefficients. Only
    * required if doing multitag-on-rio, and may be nullopt otherwise.
    * @param constrainedPnpParams Constrained SolvePNP params, if needed.
+   * @deprecated Use individual estimation methods instead.
    */
+  [[deprecated("Use individual estimation methods instead.")]]
   std::optional<EstimatedRobotPose> Update(
       const photon::PhotonPipelineResult& result,
       std::optional<photon::PhotonCamera::CameraMatrix> cameraMatrixData =
@@ -257,6 +288,127 @@ class PhotonPoseEstimator {
       std::optional<ConstrainedSolvepnpParams> constrainedPnpParams =
           std::nullopt);
 
+  /**
+   * Return the estimated position of the robot with the lowest position
+   * ambiguity from a List of pipeline results.
+   *
+   * @param cameraResult A pipeline result from the camera.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimateLowestAmbiguityPose(
+      PhotonPipelineResult cameraResult);
+
+  /**
+   * Return the estimated position of the robot using the target with the lowest
+   * delta height difference between the estimated and actual height of the
+   * camera.
+   *
+   * @param cameraResult A pipeline result from the camera.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimateClosestToCameraHeightPose(
+      PhotonPipelineResult cameraResult);
+
+  /**
+   * Return the estimated position of the robot using the target with the lowest
+   * delta in the vector magnitude between it and the reference pose.
+   *
+   * @param cameraResult A pipeline result from the camera.
+   * @param referencePose reference pose to check vector magnitude difference
+   * against.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimateClosestToReferencePose(
+      PhotonPipelineResult cameraResult, frc::Pose3d referencePose);
+
+  /**
+   * Return the estimated position of the robot by using all visible tags to
+   * compute a single pose estimate on coprocessor. This option needs to be
+   * enabled on the PhotonVision web UI as well.
+   *
+   * @param cameraResult A pipeline result from the camera.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimateCoprocMultiTagPose(
+      PhotonPipelineResult cameraResult);
+
+  /**
+   * Return the estimated position of the robot by using all visible tags to
+   * compute a single pose estimate on the RoboRIO. This can take a lot of time
+   * due to the RIO's weak computing power.
+   *
+   * @param cameraResult A pipeline result from the camera.
+   * @param cameraMatrix Camera intrinsics from camera calibration data.
+   * @param distCoeffs Distortion coefficients from camera calibration data.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimateRioMultiTagPose(
+      PhotonPipelineResult cameraResult, PhotonCamera::CameraMatrix camMat,
+      PhotonCamera::DistortionMatrix distCoeffs);
+
+  /**
+   * Return the estimated position of the robot by using distance data from best
+   * visible tag to compute a Pose. This runs on the RoboRIO in order to access
+   * the robot's yaw heading, and MUST have AddHeadingData called every frame so
+   * heading data is up-to-date.
+   *
+   * <p>Yields a Pose2d in estimatedRobotPose (0 for z, roll, pitch)
+   *
+   * <p>https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
+   *
+   * @param cameraResult A pipeline result from the camera.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimatePnpDistanceTrigSolvePose(
+      PhotonPipelineResult cameraResult);
+
+  /**
+   * Return the average of the best target poses using ambiguity as weight.
+   * @param cameraResult A pipeline result from the camera.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimateAverageBestTargetsPose(
+      PhotonPipelineResult cameraResult);
+
+  /**
+   * Return the estimated position of the robot by solving a constrained version
+   * of the Perspective-n-Point problem with the robot's drivebase flat on the
+   * floor. This computation takes place on the RoboRIO, and typically takes not
+   * more than 2ms. See
+   * photon::VisionEstimation::EstimateRobotPoseConstrainedSolvePNP for tuning
+   * handles this strategy exposes. Internally, the cost function is a
+   * sum-squared of pixel reprojection error + (optionally) heading error *
+   * heading scale factor. This strategy needs addHeadingData called every frame
+   * so heading data is up-to-date.
+   *
+   * @param cameraResult A pipeline result from the camera.
+   * @param cameraMatrix Camera intrinsics from camera calibration data.
+   * @param distCoeffs Distortion coefficients from camera calibration data.
+   * @param seedPose An initial guess at robot pose, refined via optimization.
+   * Better guesses will converge faster. Can come from any pose source, but
+   * some battle-tested sources are estimateCoprocMultiTagPose, or
+   * estimateLowestAmbiguityPose if MultiTag results aren't available.
+   * @param headingFree If true, heading is completely free to vary. If false,
+   * heading excursions from the provided heading measurement will be penalized
+   * @param headingScaleFactor If headingFree is false, this weights the cost of
+   * changing our robot heading estimate against the tag corner reprojection
+   * error cost.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimateConstrainedSolvepnpPose(
+      photon::PhotonPipelineResult cameraResult,
+      photon::PhotonCamera::CameraMatrix cameraMatrix,
+      photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose,
+      bool headingFree, double headingScaleFactor);
+
  private:
   frc::AprilTagFieldLayout aprilTags;
   PoseStrategy strategy;
@@ -280,9 +432,9 @@ class PhotonPoseEstimator {
    * This should only be called after timestamp checks have been done by another
    * update() overload.
    *
-   * @param cameraResult The latest pipeline result from the camera
+   * @param cameraResult A pipeline result from the camera.
    * @param strategy The pose strategy to use
-   * @return an EstimatedRobotPose with an estimated pose, timestamp, and
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
    * targets used to create the estimate.
    */
   std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result,
@@ -296,84 +448,6 @@ class PhotonPoseEstimator {
       std::optional<PhotonCamera::DistortionMatrix> coeffsData,
       std::optional<ConstrainedSolvepnpParams> constrainedPnpParams,
       PoseStrategy strategy);
-
-  /**
-   * Return the estimated position of the robot with the lowest position
-   * ambiguity from a List of pipeline results.
-   *
-   * @return the estimated position of the robot in the FCS and the estimated
-   * timestamp of this estimation.
-   */
-  std::optional<EstimatedRobotPose> LowestAmbiguityStrategy(
-      PhotonPipelineResult result);
-
-  /**
-   * Return the estimated position of the robot using the target with the lowest
-   * delta height difference between the estimated and actual height of the
-   * camera.
-   *
-   * @return the estimated position of the robot in the FCS and the estimated
-   * timestamp of this estimation.
-   */
-  std::optional<EstimatedRobotPose> ClosestToCameraHeightStrategy(
-      PhotonPipelineResult result);
-
-  /**
-   * Return the estimated position of the robot using the target with the lowest
-   * delta in the vector magnitude between it and the reference pose.
-   *
-   * @param referencePose reference pose to check vector magnitude difference
-   * against.
-   * @return the estimated position of the robot in the FCS and the estimated
-   * timestamp of this estimation.
-   */
-  std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
-      PhotonPipelineResult result);
-
-  /**
-   * Return the pose calculated by combining all tags into one on coprocessor
-   *
-   * @return the estimated position of the robot in the FCS
-   */
-  std::optional<EstimatedRobotPose> MultiTagOnCoprocStrategy(
-      PhotonPipelineResult result);
-
-  /**
-   * Return the pose calculation using all targets in view in the same PNP()
-   calculation
-   *
-   * @return the estimated position of the robot in the FCS and the estimated
-   timestamp of this estimation.
-   */
-  std::optional<EstimatedRobotPose> MultiTagOnRioStrategy(
-      PhotonPipelineResult result,
-      std::optional<PhotonCamera::CameraMatrix> camMat,
-      std::optional<PhotonCamera::DistortionMatrix> distCoeffs);
-
-  /**
-   * Return the pose calculation using the best visible tag and the robot's
-   * heading
-   *
-   * @return the estimated position of the robot in the FCS and the estimated
-   * timestamp of this estimation
-   */
-  std::optional<EstimatedRobotPose> PnpDistanceTrigSolveStrategy(
-      PhotonPipelineResult result);
-
-  /**
-   * Return the average of the best target poses using ambiguity as weight.
-
-   * @return the estimated position of the robot in the FCS and the estimated
-   timestamp of this estimation.
-   */
-  std::optional<EstimatedRobotPose> AverageBestTargetsStrategy(
-      PhotonPipelineResult result);
-
-  std::optional<EstimatedRobotPose> ConstrainedPnpStrategy(
-      photon::PhotonPipelineResult result,
-      std::optional<photon::PhotonCamera::CameraMatrix> camMat,
-      std::optional<photon::PhotonCamera::DistortionMatrix> distCoeffs,
-      std::optional<ConstrainedSolvepnpParams> constrainedPnpParams);
 };
 
 }  // namespace photon
diff --git a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java
new file mode 100644
index 000000000..9e8f33746
--- /dev/null
+++ b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java
@@ -0,0 +1,936 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.photonvision;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+import static org.junit.jupiter.api.Assertions.assertNull;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.fail;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.RuntimeLoader;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+import org.junit.jupiter.api.AfterAll;
+import org.junit.jupiter.api.AutoClose;
+import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.Test;
+import org.photonvision.PhotonPoseEstimator.ConstrainedSolvepnpParams;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.estimation.TargetModel;
+import org.photonvision.jni.LibraryLoader;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.SimCameraProperties;
+import org.photonvision.simulation.VisionTargetSim;
+import org.photonvision.targeting.MultiTargetPNPResult;
+import org.photonvision.targeting.PhotonPipelineMetadata;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.PnpResult;
+import org.photonvision.targeting.TargetCorner;
+
+class LegacyPhotonPoseEstimatorTest {
+    static AprilTagFieldLayout aprilTags;
+    @AutoClose final PhotonCameraInjector cameraOne = new PhotonCameraInjector();
+
+    @BeforeAll
+    public static void init() throws IOException {
+        if (!LibraryLoader.loadWpiLibraries()) {
+            fail();
+        }
+        RuntimeLoader.loadLibrary("photontargetingJNI");
+
+        HAL.initialize(1000, 0);
+
+        List<AprilTag> tagList = new ArrayList<>(2);
+        tagList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
+        tagList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
+        double fieldLength = Units.feetToMeters(54.0);
+        double fieldWidth = Units.feetToMeters(27.0);
+        aprilTags = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth);
+    }
+
+    @AfterAll
+    public static void teardown() {
+        HAL.shutdown();
+    }
+
+    @Test
+    void testLowestAmbiguityStrategy() {
+        cameraOne.result =
+                new PhotonPipelineResult(
+                        0,
+                        11 * 1000000,
+                        1100000,
+                        1024,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        0,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
+                                        new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.1,
+                                        6.7,
+                                        1,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)),
+                                        new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
+                                        0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        9.0,
+                                        -2.0,
+                                        19.0,
+                                        3.0,
+                                        0,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
+                                        new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
+                                        0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)))));
+
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d());
+
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
+        Pose3d pose = estimatedPose.get().estimatedPose;
+
+        assertEquals(11, estimatedPose.get().timestampSeconds);
+        assertEquals(1, pose.getX(), .01);
+        assertEquals(3, pose.getY(), .01);
+        assertEquals(2, pose.getZ(), .01);
+    }
+
+    @Test
+    void testClosestToCameraHeightStrategy() {
+        cameraOne.result =
+                new PhotonPipelineResult(
+                        0,
+                        4000000,
+                        1100000,
+                        1024,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        1,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
+                                        new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.1,
+                                        6.7,
+                                        1,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
+                                        0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        9.0,
+                                        -2.0,
+                                        19.0,
+                                        3.0,
+                                        0,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(4, 4, 4), new Rotation3d()),
+                                        new Transform3d(new Translation3d(5, 5, 5), new Rotation3d()),
+                                        0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)))));
+
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags,
+                        PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT,
+                        new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
+
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
+        Pose3d pose = estimatedPose.get().estimatedPose;
+
+        assertEquals(4, estimatedPose.get().timestampSeconds);
+        assertEquals(4, pose.getX(), .01);
+        assertEquals(4, pose.getY(), .01);
+        assertEquals(0, pose.getZ(), .01);
+    }
+
+    @Test
+    void closestToReferencePoseStrategy() {
+        cameraOne.result =
+                new PhotonPipelineResult(
+                        0,
+                        17000000,
+                        1100000,
+                        1024,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        1,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
+                                        new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.1,
+                                        6.7,
+                                        1,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
+                                        0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        9.0,
+                                        -2.0,
+                                        19.0,
+                                        3.0,
+                                        0,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
+                                        0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)))));
+
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags,
+                        PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
+                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
+        estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
+
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
+        Pose3d pose = estimatedPose.get().estimatedPose;
+
+        assertEquals(17, estimatedPose.get().timestampSeconds);
+        assertEquals(1, pose.getX(), .01);
+        assertEquals(1.1, pose.getY(), .01);
+        assertEquals(.9, pose.getZ(), .01);
+    }
+
+    @Test
+    void closestToLastPose() {
+        cameraOne.result =
+                new PhotonPipelineResult(
+                        0,
+                        1000000,
+                        1100000,
+                        1024,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        1,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
+                                        new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.1,
+                                        6.7,
+                                        1,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
+                                        0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        9.0,
+                                        -2.0,
+                                        19.0,
+                                        3.0,
+                                        0,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
+                                        0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)))));
+
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags,
+                        PoseStrategy.CLOSEST_TO_LAST_POSE,
+                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
+
+        estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
+
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
+        Pose3d pose = estimatedPose.get().estimatedPose;
+
+        cameraOne.result =
+                new PhotonPipelineResult(
+                        0,
+                        7000000,
+                        1100000,
+                        1024,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        1,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
+                                        new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.1,
+                                        6.7,
+                                        0,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(2.1, 1.9, 2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
+                                        0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        9.0,
+                                        -2.0,
+                                        19.0,
+                                        3.0,
+                                        0,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(2.4, 2.4, 2.2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(2, 1, 2), new Rotation3d()),
+                                        0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)))));
+
+        estimatedPose = estimator.update(cameraOne.result);
+        pose = estimatedPose.get().estimatedPose;
+
+        assertEquals(7, estimatedPose.get().timestampSeconds);
+        assertEquals(.9, pose.getX(), .01);
+        assertEquals(1.1, pose.getY(), .01);
+        assertEquals(1, pose.getZ(), .01);
+    }
+
+    @Test
+    void pnpDistanceTrigSolve() {
+        List<VisionTargetSim> simTargets =
+                aprilTags.getTags().stream()
+                        .map((AprilTag x) -> new VisionTargetSim(x.pose, TargetModel.kAprilTag36h11, x.ID))
+                        .toList();
+        try (PhotonCameraSim cameraOneSim =
+                new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG())) {
+            /* Compound Rolled + Pitched + Yaw */
+            Transform3d compoundTestTransform =
+                    new Transform3d(
+                            -Units.inchesToMeters(12),
+                            -Units.inchesToMeters(11),
+                            3,
+                            new Rotation3d(
+                                    Units.degreesToRadians(37),
+                                    Units.degreesToRadians(6),
+                                    Units.degreesToRadians(60)));
+
+            var estimator =
+                    new PhotonPoseEstimator(
+                            aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
+
+            /* this is the real pose of the robot base we test against */
+            var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197));
+            PhotonPipelineResult result =
+                    cameraOneSim.process(
+                            1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
+            var bestTarget = result.getBestTarget();
+            assertNotNull(bestTarget);
+            assertEquals(0, bestTarget.fiducialId);
+
+            estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
+            var estimatedPose = estimator.update(result);
+
+            var pose = estimatedPose.get().estimatedPose;
+            assertEquals(realPose.getX(), pose.getX(), .01);
+            assertEquals(realPose.getY(), pose.getY(), .01);
+            assertEquals(0.0, pose.getZ(), .01);
+
+            /* Straight on */
+            Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, Rotation3d.kZero);
+
+            estimator.setRobotToCameraTransform(straightOnTestTransform);
+
+            /* Pose to compare with */
+            realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818));
+            result =
+                    cameraOneSim.process(
+                            1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
+
+            estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
+            estimatedPose = estimator.update(result);
+
+            pose = estimatedPose.get().estimatedPose;
+            assertEquals(realPose.getX(), pose.getX(), .01);
+            assertEquals(realPose.getY(), pose.getY(), .01);
+            assertEquals(0.0, pose.getZ(), .01);
+        }
+    }
+
+    @Test
+    void cacheIsInvalidated() {
+        var result =
+                new PhotonPipelineResult(
+                        0,
+                        20_000_000,
+                        1_100_000,
+                        1024,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        0,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)))));
+
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags,
+                        PoseStrategy.AVERAGE_BEST_TARGETS,
+                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
+
+        // Initial state, expect no timestamp
+        assertEquals(-1, estimator.poseCacheTimestampSeconds);
+
+        // Empty result, expect empty result
+        cameraOne.result = new PhotonPipelineResult();
+        cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6);
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
+        assertFalse(estimatedPose.isPresent());
+
+        // Set actual result
+        cameraOne.result = result;
+        estimatedPose = estimator.update(cameraOne.result);
+        assertTrue(estimatedPose.isPresent());
+        assertEquals(20, estimatedPose.get().timestampSeconds, .01);
+        assertEquals(20, estimator.poseCacheTimestampSeconds);
+
+        // And again -- pose cache should mean this is empty
+        cameraOne.result = result;
+        estimatedPose = estimator.update(cameraOne.result);
+        assertFalse(estimatedPose.isPresent());
+        // Expect the old timestamp to still be here
+        assertEquals(20, estimator.poseCacheTimestampSeconds);
+
+        // Set new field layout -- right after, the pose cache timestamp should be -1
+        estimator.setFieldTags(new AprilTagFieldLayout(List.of(new AprilTag(0, new Pose3d())), 0, 0));
+        assertEquals(-1, estimator.poseCacheTimestampSeconds);
+        // Update should cache the current timestamp (20) again
+        cameraOne.result = result;
+        estimatedPose = estimator.update(cameraOne.result);
+        assertEquals(20, estimatedPose.get().timestampSeconds, .01);
+        assertEquals(20, estimator.poseCacheTimestampSeconds);
+
+        // Setting a value from None to a non-None should invalidate the cache
+        assertNull(estimator.getReferencePose());
+        assertEquals(20, estimator.poseCacheTimestampSeconds);
+        estimator.setReferencePose(new Pose2d(new Translation2d(1, 2), Rotation2d.kZero));
+        assertEquals(-1, estimator.poseCacheTimestampSeconds, "wtf");
+    }
+
+    @Test
+    void averageBestPoses() {
+        cameraOne.result =
+                new PhotonPipelineResult(
+                        0,
+                        20 * 1000000,
+                        1100000,
+                        1024,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        0,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))), // 1 1 1 ambig: .7
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.1,
+                                        6.7,
+                                        1,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
+                                        new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
+                                        0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))), // 2 2 2 ambig .3
+                                new PhotonTrackedTarget(
+                                        9.0,
+                                        -2.0,
+                                        19.0,
+                                        3.0,
+                                        0,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
+                                        new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
+                                        0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))))); // 3 3 3 ambig .4
+
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags,
+                        PoseStrategy.AVERAGE_BEST_TARGETS,
+                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
+
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
+        Pose3d pose = estimatedPose.get().estimatedPose;
+
+        assertEquals(20, estimatedPose.get().timestampSeconds, .01);
+        assertEquals(2.15, pose.getX(), .01);
+        assertEquals(2.15, pose.getY(), .01);
+        assertEquals(2.15, pose.getZ(), .01);
+    }
+
+    @Test
+    void testMultiTagOnRioFallback() {
+        cameraOne.result =
+                new PhotonPipelineResult(
+                        0,
+                        11 * 1_000_000,
+                        1_100_000,
+                        1024,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        0,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
+                                        new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.1,
+                                        6.7,
+                                        1,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)),
+                                        new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
+                                        0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)))));
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_RIO, Transform3d.kZero);
+        estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
+        Pose3d pose = estimatedPose.get().estimatedPose;
+        // Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
+        assertAll(
+                () -> assertEquals(11, estimatedPose.get().timestampSeconds),
+                () -> assertEquals(1, pose.getX(), 1e-9),
+                () -> assertEquals(3, pose.getY(), 1e-9),
+                () -> assertEquals(2, pose.getZ(), 1e-9));
+    }
+
+    @Test
+    public void testConstrainedPnpOneTag() {
+        var distortion = VecBuilder.fill(0, 0, 0, 0, 0, 0, 0, 0);
+        var cameraMat =
+                MatBuilder.fill(
+                        Nat.N3(),
+                        Nat.N3(),
+                        399.37500000000006,
+                        0,
+                        319.5,
+                        0,
+                        399.16666666666674,
+                        239.5,
+                        0,
+                        0,
+                        1);
+
+        /*
+         * Ground truth:
+         * 29.989279,NT:/photonvision/YOUR CAMERA
+         * NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/w,0.
+         * 31064262190452635
+         * 29.989279,NT:/photonvision/YOUR CAMERA
+         * NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/x,0.
+         * 24478552235412665
+         * 29.989279,NT:/photonvision/YOUR CAMERA
+         * NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/y,-0.
+         * 0836470779150917
+         * 29.989279,NT:/photonvision/YOUR CAMERA
+         * NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/z,0.
+         * 914649865171567
+         * 29.989279,NT:/photonvision/YOUR CAMERA
+         * NAME/rawBytes/multitagResult/estimatedPose/best/translation/x,3.
+         * 191446451763934
+         * 29.989279,NT:/photonvision/YOUR CAMERA
+         * NAME/rawBytes/multitagResult/estimatedPose/best/translation/y,4.
+         * 44396966389316
+         * 29.989279,NT:/photonvision/YOUR CAMERA
+         * NAME/rawBytes/multitagResult/estimatedPose/best/translation/z,0.
+         * 4995793771070878
+         */
+        List<TargetCorner> corners8 =
+                List.of(
+                        new TargetCorner(98.09875447066685, 331.0093220119495),
+                        new TargetCorner(122.20226758624413, 335.50083894738486),
+                        new TargetCorner(127.17118732489361, 313.81406314178633),
+                        new TargetCorner(104.28543773760417, 309.6516557438994));
+
+        var result =
+                new PhotonPipelineResult(
+                        new PhotonPipelineMetadata(10000, 2000, 1, 100),
+                        List.of(
+                                new PhotonTrackedTarget(0, 0, 0, 0, 8, 0, 0, null, null, 0, corners8, corners8)),
+                        Optional.of(
+                                new MultiTargetPNPResult(
+                                        new PnpResult(
+                                                new Transform3d(
+                                                        // From ground truth
+                                                        new Translation3d(
+                                                                3.1665557336121353, 4.430673446050584, 0.48678786477534686),
+                                                        new Rotation3d(
+                                                                new Quaternion(
+                                                                        0.3132532247418243,
+                                                                        0.24722671090692333,
+                                                                        -0.08413452932300695,
+                                                                        0.9130568172784148))),
+                                                0.1),
+                                        new ArrayList<Short>(8))));
+
+        final double camPitch = Units.degreesToRadians(30.0);
+        final Transform3d kRobotToCam =
+                new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0));
+
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
+                        PoseStrategy.CONSTRAINED_SOLVEPNP,
+                        kRobotToCam);
+
+        estimator.addHeadingData(result.getTimestampSeconds(), Rotation2d.kZero);
+
+        Optional<EstimatedRobotPose> estimatedPose =
+                estimator.update(
+                        result,
+                        Optional.of(cameraMat),
+                        Optional.of(distortion),
+                        Optional.of(new ConstrainedSolvepnpParams(true, 0)));
+        Pose3d pose = estimatedPose.get().estimatedPose;
+        System.out.println(pose);
+    }
+
+    @Test
+    void testConstrainedPnpEmptyCase() {
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
+                        PoseStrategy.CONSTRAINED_SOLVEPNP,
+                        Transform3d.kZero);
+        PhotonPipelineResult result = new PhotonPipelineResult();
+        var estimate = estimator.update(result);
+        assertEquals(estimate, Optional.empty());
+    }
+
+    private static class PhotonCameraInjector extends PhotonCamera {
+        public PhotonCameraInjector() {
+            super("Test");
+        }
+
+        PhotonPipelineResult result;
+
+        @Override
+        public List<PhotonPipelineResult> getAllUnreadResults() {
+            return List.of(result);
+        }
+
+        @Override
+        public PhotonPipelineResult getLatestResult() {
+            return result;
+        }
+    }
+}
diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
index 0449d7e15..bfc169aa8 100644
--- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
+++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
@@ -24,11 +24,8 @@
 
 package org.photonvision;
 
-import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertNotNull;
-import static org.junit.jupiter.api.Assertions.assertNull;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.junit.jupiter.api.Assertions.fail;
 
@@ -39,13 +36,11 @@ import edu.wpi.first.hal.HAL;
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Quaternion;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.util.RuntimeLoader;
@@ -57,8 +52,6 @@ import org.junit.jupiter.api.AfterAll;
 import org.junit.jupiter.api.AutoClose;
 import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.Test;
-import org.photonvision.PhotonPoseEstimator.ConstrainedSolvepnpParams;
-import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.estimation.TargetModel;
 import org.photonvision.jni.LibraryLoader;
 import org.photonvision.simulation.PhotonCameraSim;
@@ -170,10 +163,10 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
 
-        PhotonPoseEstimator estimator =
-                new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d());
+        PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags, new Transform3d());
 
-        Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
+        Optional<EstimatedRobotPose> estimatedPose =
+                estimator.estimateLowestAmbiguityPose(cameraOne.result);
         Pose3d pose = estimatedPose.get().estimatedPose;
 
         assertEquals(11, estimatedPose.get().timestampSeconds);
@@ -257,11 +250,10 @@ class PhotonPoseEstimatorTest {
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(
-                        aprilTags,
-                        PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT,
-                        new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
+                        aprilTags, new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
 
-        Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
+        Optional<EstimatedRobotPose> estimatedPose =
+                estimator.estimateClosestToCameraHeightPose(cameraOne.result);
         Pose3d pose = estimatedPose.get().estimatedPose;
 
         assertEquals(4, estimatedPose.get().timestampSeconds);
@@ -345,12 +337,11 @@ class PhotonPoseEstimatorTest {
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(
-                        aprilTags,
-                        PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
-                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
-        estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
+                        aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
 
-        Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
+        Optional<EstimatedRobotPose> estimatedPose =
+                estimator.estimateClosestToReferencePose(
+                        cameraOne.result, new Pose3d(1, 1, 1, new Rotation3d()));
         Pose3d pose = estimatedPose.get().estimatedPose;
 
         assertEquals(17, estimatedPose.get().timestampSeconds);
@@ -434,13 +425,11 @@ class PhotonPoseEstimatorTest {
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(
-                        aprilTags,
-                        PoseStrategy.CLOSEST_TO_LAST_POSE,
-                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
+                        aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
 
-        estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
-
-        Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
+        Optional<EstimatedRobotPose> estimatedPose =
+                estimator.estimateClosestToReferencePose(
+                        cameraOne.result, new Pose3d(1, 1, 1, new Rotation3d()));
         Pose3d pose = estimatedPose.get().estimatedPose;
 
         cameraOne.result =
@@ -514,7 +503,7 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
 
-        estimatedPose = estimator.update(cameraOne.result);
+        estimatedPose = estimator.estimateClosestToReferencePose(cameraOne.result, pose);
         pose = estimatedPose.get().estimatedPose;
 
         assertEquals(7, estimatedPose.get().timestampSeconds);
@@ -542,9 +531,7 @@ class PhotonPoseEstimatorTest {
                                     Units.degreesToRadians(6),
                                     Units.degreesToRadians(60)));
 
-            var estimator =
-                    new PhotonPoseEstimator(
-                            aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
+            var estimator = new PhotonPoseEstimator(aprilTags, compoundTestTransform);
 
             /* this is the real pose of the robot base we test against */
             var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197));
@@ -556,7 +543,7 @@ class PhotonPoseEstimatorTest {
             assertEquals(0, bestTarget.fiducialId);
 
             estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
-            var estimatedPose = estimator.update(result);
+            var estimatedPose = estimator.estimatePnpDistanceTrigSolvePose(result);
 
             var pose = estimatedPose.get().estimatedPose;
             assertEquals(realPose.getX(), pose.getX(), .01);
@@ -575,7 +562,7 @@ class PhotonPoseEstimatorTest {
                             1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
 
             estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
-            estimatedPose = estimator.update(result);
+            estimatedPose = estimator.estimatePnpDistanceTrigSolvePose(result);
 
             pose = estimatedPose.get().estimatedPose;
             assertEquals(realPose.getX(), pose.getX(), .01);
@@ -584,82 +571,6 @@ class PhotonPoseEstimatorTest {
         }
     }
 
-    @Test
-    void cacheIsInvalidated() {
-        var result =
-                new PhotonPipelineResult(
-                        0,
-                        20_000_000,
-                        1_100_000,
-                        1024,
-                        List.of(
-                                new PhotonTrackedTarget(
-                                        3.0,
-                                        -4.0,
-                                        9.0,
-                                        4.0,
-                                        0,
-                                        -1,
-                                        -1,
-                                        new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
-                                        new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
-                                        0.7,
-                                        List.of(
-                                                new TargetCorner(1, 2),
-                                                new TargetCorner(3, 4),
-                                                new TargetCorner(5, 6),
-                                                new TargetCorner(7, 8)),
-                                        List.of(
-                                                new TargetCorner(1, 2),
-                                                new TargetCorner(3, 4),
-                                                new TargetCorner(5, 6),
-                                                new TargetCorner(7, 8)))));
-
-        PhotonPoseEstimator estimator =
-                new PhotonPoseEstimator(
-                        aprilTags,
-                        PoseStrategy.AVERAGE_BEST_TARGETS,
-                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
-
-        // Initial state, expect no timestamp
-        assertEquals(-1, estimator.poseCacheTimestampSeconds);
-
-        // Empty result, expect empty result
-        cameraOne.result = new PhotonPipelineResult();
-        cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6);
-        Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
-        assertFalse(estimatedPose.isPresent());
-
-        // Set actual result
-        cameraOne.result = result;
-        estimatedPose = estimator.update(cameraOne.result);
-        assertTrue(estimatedPose.isPresent());
-        assertEquals(20, estimatedPose.get().timestampSeconds, .01);
-        assertEquals(20, estimator.poseCacheTimestampSeconds);
-
-        // And again -- pose cache should mean this is empty
-        cameraOne.result = result;
-        estimatedPose = estimator.update(cameraOne.result);
-        assertFalse(estimatedPose.isPresent());
-        // Expect the old timestamp to still be here
-        assertEquals(20, estimator.poseCacheTimestampSeconds);
-
-        // Set new field layout -- right after, the pose cache timestamp should be -1
-        estimator.setFieldTags(new AprilTagFieldLayout(List.of(new AprilTag(0, new Pose3d())), 0, 0));
-        assertEquals(-1, estimator.poseCacheTimestampSeconds);
-        // Update should cache the current timestamp (20) again
-        cameraOne.result = result;
-        estimatedPose = estimator.update(cameraOne.result);
-        assertEquals(20, estimatedPose.get().timestampSeconds, .01);
-        assertEquals(20, estimator.poseCacheTimestampSeconds);
-
-        // Setting a value from None to a non-None should invalidate the cache
-        assertNull(estimator.getReferencePose());
-        assertEquals(20, estimator.poseCacheTimestampSeconds);
-        estimator.setReferencePose(new Pose2d(new Translation2d(1, 2), Rotation2d.kZero));
-        assertEquals(-1, estimator.poseCacheTimestampSeconds, "wtf");
-    }
-
     @Test
     void averageBestPoses() {
         cameraOne.result =
@@ -735,11 +646,10 @@ class PhotonPoseEstimatorTest {
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(
-                        aprilTags,
-                        PoseStrategy.AVERAGE_BEST_TARGETS,
-                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
+                        aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
 
-        Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
+        Optional<EstimatedRobotPose> estimatedPose =
+                estimator.estimateAverageBestTargetsPose(cameraOne.result);
         Pose3d pose = estimatedPose.get().estimatedPose;
 
         assertEquals(20, estimatedPose.get().timestampSeconds, .01);
@@ -749,8 +659,9 @@ class PhotonPoseEstimatorTest {
     }
 
     @Test
-    void testMultiTagOnRioFallback() {
-        cameraOne.result =
+    void testMultiTagOnCoprocFallback() {
+        PhotonCameraInjector camera = new PhotonCameraInjector();
+        camera.result =
                 new PhotonPipelineResult(
                         0,
                         11 * 1_000_000,
@@ -799,18 +710,21 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
-        PhotonPoseEstimator estimator =
-                new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_RIO, Transform3d.kZero);
-        estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+        PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags, Transform3d.kZero);
+
+        Optional<EstimatedRobotPose> estimatedPose =
+                estimator.estimateCoprocMultiTagPose(camera.result);
+        assertTrue(estimatedPose.isEmpty());
+
+        estimatedPose = estimator.estimateLowestAmbiguityPose(camera.result);
+        assertTrue(estimatedPose.isPresent());
 
-        Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
         Pose3d pose = estimatedPose.get().estimatedPose;
         // Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
-        assertAll(
-                () -> assertEquals(11, estimatedPose.get().timestampSeconds),
-                () -> assertEquals(1, pose.getX(), 1e-9),
-                () -> assertEquals(3, pose.getY(), 1e-9),
-                () -> assertEquals(2, pose.getZ(), 1e-9));
+        assertEquals(11, estimatedPose.get().timestampSeconds);
+        assertEquals(1, pose.getX(), 1e-9);
+        assertEquals(3, pose.getY(), 1e-9);
+        assertEquals(2, pose.getZ(), 1e-9);
     }
 
     @Test
@@ -888,34 +802,17 @@ class PhotonPoseEstimatorTest {
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(
-                        AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
-                        PoseStrategy.CONSTRAINED_SOLVEPNP,
-                        kRobotToCam);
+                        AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), kRobotToCam);
 
+        var multiTagEstimate = estimator.estimateCoprocMultiTagPose(result);
         estimator.addHeadingData(result.getTimestampSeconds(), Rotation2d.kZero);
-
         Optional<EstimatedRobotPose> estimatedPose =
-                estimator.update(
-                        result,
-                        Optional.of(cameraMat),
-                        Optional.of(distortion),
-                        Optional.of(new ConstrainedSolvepnpParams(true, 0)));
+                estimator.estimateConstrainedSolvepnpPose(
+                        result, cameraMat, distortion, multiTagEstimate.get().estimatedPose, true, 0);
         Pose3d pose = estimatedPose.get().estimatedPose;
         System.out.println(pose);
     }
 
-    @Test
-    void testConstrainedPnpEmptyCase() {
-        PhotonPoseEstimator estimator =
-                new PhotonPoseEstimator(
-                        AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
-                        PoseStrategy.CONSTRAINED_SOLVEPNP,
-                        Transform3d.kZero);
-        PhotonPipelineResult result = new PhotonPipelineResult();
-        var estimate = estimator.update(result);
-        assertEquals(estimate, Optional.empty());
-    }
-
     private static class PhotonCameraInjector extends PhotonCamera {
         public PhotonCameraInjector() {
             super("Test");
diff --git a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp
new file mode 100644
index 000000000..0445be6b1
--- /dev/null
+++ b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp
@@ -0,0 +1,661 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <map>
+#include <utility>
+#include <vector>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Rotation3d.h>
+#include <frc/geometry/Transform3d.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <wpi/SmallVector.h>
+#include <wpi/deprecated.h>
+
+#include "gtest/gtest.h"
+#include "photon/PhotonCamera.h"
+#include "photon/PhotonPoseEstimator.h"
+#include "photon/dataflow/structures/Packet.h"
+#include "photon/simulation/PhotonCameraSim.h"
+#include "photon/simulation/SimCameraProperties.h"
+#include "photon/simulation/VisionTargetSim.h"
+#include "photon/targeting/MultiTargetPNPResult.h"
+#include "photon/targeting/PhotonPipelineResult.h"
+#include "photon/targeting/PhotonTrackedTarget.h"
+#include "photon/targeting/PnpResult.h"
+WPI_IGNORE_DEPRECATED
+
+static std::vector<frc::AprilTag> tags = {
+    {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
+                    frc::Rotation3d())},
+    {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
+                    frc::Rotation3d())}};
+
+static frc::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft};
+
+static std::vector<photon::TargetCorner> corners{
+    photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.},
+    photon::TargetCorner{5., 6.}, photon::TargetCorner{7., 8.}};
+static std::vector<photon::TargetCorner> detectedCorners{
+    photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.},
+    photon::TargetCorner{5., 6.}, photon::TargetCorner{7., 8.}};
+
+TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
+  photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
+
+  std::vector<photon::PhotonTrackedTarget> targets{
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          0.7, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {photon::PhotonPipelineResult{
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
+  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
+
+  photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
+                                        frc::Transform3d{});
+
+  std::optional<photon::EstimatedRobotPose> estimatedPose;
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+  ASSERT_TRUE(estimatedPose);
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .02);
+  EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
+}
+
+TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
+  std::vector<frc::AprilTag> tags = {
+      {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
+                      frc::Rotation3d())},
+      {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
+                      frc::Rotation3d())},
+  };
+  auto aprilTags = frc::AprilTagFieldLayout(tags, 54_ft, 27_ft);
+
+  std::vector<std::pair<photon::PhotonCamera, frc::Transform3d>> cameras;
+
+  photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
+
+  // ID 0 at 3,3,3
+  // ID 1 at 5,5,5
+
+  std::vector<photon::PhotonTrackedTarget> targets{
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {photon::PhotonPipelineResult{
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
+  cameraOne.testResult[0].SetReceiveTimestamp(17_s);
+
+  photon::PhotonPoseEstimator estimator(
+      aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}});
+
+  std::optional<photon::EstimatedRobotPose> estimatedPose;
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+  ASSERT_TRUE(estimatedPose);
+
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .02);
+  EXPECT_NEAR(4, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(4, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(0, units::unit_cast<double>(pose.Z()), .01);
+}
+
+TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
+  photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
+
+  std::vector<photon::PhotonTrackedTarget> targets{
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {photon::PhotonPipelineResult{
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
+  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
+
+  photon::PhotonPoseEstimator estimator(aprilTags,
+                                        photon::CLOSEST_TO_REFERENCE_POSE, {});
+  estimator.SetReferencePose(
+      frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
+
+  std::optional<photon::EstimatedRobotPose> estimatedPose;
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  ASSERT_TRUE(estimatedPose);
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .01);
+  EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(.9, units::unit_cast<double>(pose.Z()), .01);
+}
+
+TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) {
+  photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
+
+  std::vector<photon::PhotonTrackedTarget> targets{
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {photon::PhotonPipelineResult{
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
+  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
+
+  photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
+                                        {});
+  estimator.SetLastPose(
+      frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
+
+  std::optional<photon::EstimatedRobotPose> estimatedPose;
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  ASSERT_TRUE(estimatedPose);
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  std::vector<photon::PhotonTrackedTarget> targetsThree{
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 0, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.testResult = {photon::PhotonPipelineResult{
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree,
+      std::nullopt}};
+  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21));
+
+  // std::optional<photon::EstimatedRobotPose> estimatedPose;
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  ASSERT_TRUE(estimatedPose);
+  pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(21.0, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .01);
+  EXPECT_NEAR(.9, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(1, units::unit_cast<double>(pose.Z()), .01);
+}
+
+TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
+  photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
+  cameraOne.test = true;
+
+  std::vector<photon::VisionTargetSim> targets;
+  targets.reserve(tags.size());
+  for (const auto& tag : tags) {
+    targets.push_back(
+        photon::VisionTargetSim(tag.pose, photon::kAprilTag36h11, tag.ID));
+  }
+  photon::PhotonCameraSim cameraOneSim = photon::PhotonCameraSim(
+      &cameraOne, photon::SimCameraProperties::PERFECT_90DEG());
+
+  /* Compound Rolled + Pitched + Yaw */
+  frc::Transform3d compoundTestTransform = frc::Transform3d(
+      -12_in, -11_in, 3_m, frc::Rotation3d(37_deg, 6_deg, 60_deg));
+
+  photon::PhotonPoseEstimator estimator(
+      aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
+
+  /* real pose of the robot base to test against */
+  frc::Pose3d realPose =
+      frc::Pose3d(7.3_m, 4.42_m, 0_m, frc::Rotation3d(0_rad, 0_rad, 2.197_rad));
+
+  photon::PhotonPipelineResult result = cameraOneSim.Process(
+      1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
+      targets);
+  cameraOne.testResult = {result};
+  cameraOne.testResult[0].SetReceiveTimestamp(17_s);
+
+  estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
+
+  std::optional<photon::EstimatedRobotPose> estimatedPose;
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  ASSERT_TRUE(estimatedPose);
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(units::unit_cast<double>(realPose.X()),
+              units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(units::unit_cast<double>(realPose.Y()),
+              units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(units::unit_cast<double>(realPose.Z()),
+              units::unit_cast<double>(pose.Z()), .01);
+
+  /* Straight on */
+  frc::Transform3d straightOnTestTransform =
+      frc::Transform3d(0_m, 0_m, 3_m, frc::Rotation3d(0_rad, 0_rad, 0_rad));
+
+  estimator.SetRobotToCameraTransform(straightOnTestTransform);
+  realPose = frc::Pose3d(4.81_m, 2.38_m, 0_m,
+                         frc::Rotation3d(0_rad, 0_rad, 2.818_rad));
+  result = cameraOneSim.Process(
+      1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
+      targets);
+  cameraOne.testResult = {result};
+  cameraOne.testResult[0].SetReceiveTimestamp(18_s);
+
+  estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
+
+  estimatedPose = std::nullopt;
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  ASSERT_TRUE(estimatedPose);
+  pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(units::unit_cast<double>(realPose.X()),
+              units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(units::unit_cast<double>(realPose.Y()),
+              units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(units::unit_cast<double>(realPose.Z()),
+              units::unit_cast<double>(pose.Z()), .01);
+}
+
+TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) {
+  photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
+
+  std::vector<photon::PhotonTrackedTarget> targets{
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {photon::PhotonPipelineResult{
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
+  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
+
+  photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
+                                        {});
+
+  std::optional<photon::EstimatedRobotPose> estimatedPose;
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  ASSERT_TRUE(estimatedPose);
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .01);
+  EXPECT_NEAR(2.15, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
+}
+
+TEST(LegacyPhotonPoseEstimatorTest, PoseCache) {
+  photon::PhotonCamera cameraOne = photon::PhotonCamera("test2");
+
+  std::vector<photon::PhotonTrackedTarget> targets{
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+
+  photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
+                                        {});
+
+  // empty input, expect empty out
+  cameraOne.testResult = {photon::PhotonPipelineResult{
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000},
+      std::vector<photon::PhotonTrackedTarget>{}, std::nullopt}};
+  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1));
+
+  std::optional<photon::EstimatedRobotPose> estimatedPose;
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  EXPECT_FALSE(estimatedPose);
+
+  // Set result, and update -- expect present and timestamp to be 15
+  cameraOne.testResult = {photon::PhotonPipelineResult{
+      photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt}};
+  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
+
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  ASSERT_TRUE(estimatedPose);
+  EXPECT_NEAR((15_s - 3_ms).to<double>(),
+              estimatedPose.value().timestamp.to<double>(), 1e-6);
+
+  // And again -- pose cache should result in returning std::nullopt
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  EXPECT_FALSE(estimatedPose);
+
+  // If the camera produces a result that is > 1 micro second later,
+  // the pose cache should not be hit.
+  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(16));
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  EXPECT_NEAR((16_s - 3_ms).to<double>(),
+              estimatedPose.value().timestamp.to<double>(), 1e-6);
+
+  // And again -- pose cache should result in returning std::nullopt
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  EXPECT_FALSE(estimatedPose);
+
+  // Setting ReferencePose should also clear the cache
+  estimator.SetReferencePose(frc::Pose3d(units::meter_t(1), units::meter_t(2),
+                                         units::meter_t(3), frc::Rotation3d()));
+
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  ASSERT_TRUE(estimatedPose);
+  EXPECT_NEAR((16_s - 3_ms).to<double>(),
+              estimatedPose.value().timestamp.to<double>(), 1e-6);
+}
+
+TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) {
+  photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
+
+  std::vector<photon::PhotonTrackedTarget> targets{
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          0.7, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {photon::PhotonPipelineResult{
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
+  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
+
+  photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
+                                        frc::Transform3d{});
+
+  std::optional<photon::EstimatedRobotPose> estimatedPose;
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+  ASSERT_TRUE(estimatedPose);
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  // Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
+  EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .02);
+  EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
+}
+
+TEST(LegacyPhotonPoseEstimatorTest, CopyResult) {
+  std::vector<photon::PhotonTrackedTarget> targets{};
+
+  auto testResult = photon::PhotonPipelineResult{
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt};
+  testResult.SetReceiveTimestamp(units::second_t(11));
+
+  auto test2 = testResult;
+
+  EXPECT_NEAR(testResult.GetTimestamp().to<double>(),
+              test2.GetTimestamp().to<double>(), 0.001);
+}
+
+TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) {
+  photon::PhotonPoseEstimator estimator(
+      frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
+      photon::CONSTRAINED_SOLVEPNP, frc::Transform3d());
+
+  photon::PhotonPipelineResult result;
+  auto estimate = estimator.Update(result);
+  EXPECT_FALSE(estimate.has_value());
+}
+
+TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
+  photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
+  auto distortion = Eigen::VectorXd::Zero(8);
+  auto cameraMat = Eigen::Matrix3d{{399.37500000000006, 0, 319.5},
+                                   {0, 399.16666666666674, 239.5},
+                                   {0, 0, 1}};
+
+  // Create corners data matching the Java test
+  std::vector<photon::TargetCorner> corners8{
+      photon::TargetCorner{98.09875447066685, 331.0093220119495},
+      photon::TargetCorner{122.20226758624413, 335.50083894738486},
+      photon::TargetCorner{127.17118732489361, 313.81406314178633},
+      photon::TargetCorner{104.28543773760417, 309.6516557438994}};
+
+  frc::Transform3d poseTransform(
+      frc::Translation3d(3.1665557336121353_m, 4.430673446050584_m,
+                         0.48678786477534686_m),
+      frc::Rotation3d(frc::Quaternion(0.3132532247418243, 0.24722671090692333,
+                                      -0.08413452932300695,
+                                      0.9130568172784148)));
+
+  std::vector<photon::PhotonTrackedTarget> targets{
+      photon::PhotonTrackedTarget{0.0, 0.0, 0.0, 0.0, 8, 0, 0.0f, poseTransform,
+                                  poseTransform, 0.0, corners8, corners8}};
+
+  auto multiTagResult = std::make_optional<photon::MultiTargetPNPResult>(
+      photon::PnpResult{poseTransform, poseTransform, 0.1, 0.1, 0.0},
+      std::vector<int16_t>{8});
+
+  photon::PhotonPipelineResult result{
+      photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets,
+      multiTagResult};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {result};
+  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
+
+  const units::radian_t camPitch = 30_deg;
+  const frc::Transform3d kRobotToCam{frc::Translation3d(0.5_m, 0.0_m, 0.5_m),
+                                     frc::Rotation3d(0_rad, -camPitch, 0_rad)};
+
+  photon::PhotonPoseEstimator estimator(
+      frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
+      photon::CONSTRAINED_SOLVEPNP, kRobotToCam);
+
+  estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(),
+                           frc::Rotation2d());
+
+  auto estimatedPose =
+      estimator.Update(cameraOne.testResult[0], cameraMat, distortion,
+                       photon::ConstrainedSolvepnpParams{true, 0});
+
+  ASSERT_TRUE(estimatedPose.has_value());
+
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(3.58, units::unit_cast<double>(pose.X()), 0.01);
+  EXPECT_NEAR(4.13, units::unit_cast<double>(pose.Y()), 0.01);
+  EXPECT_NEAR(0.0, units::unit_cast<double>(pose.Z()), 0.01);
+
+  EXPECT_EQ(photon::CONSTRAINED_SOLVEPNP, estimatedPose.value().strategy);
+}
diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
index b58f6bab9..5d5caeade 100644
--- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
+++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
@@ -93,12 +93,11 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
       photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
   cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
 
-  photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
-                                        frc::Transform3d{});
+  photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{});
 
   std::optional<photon::EstimatedRobotPose> estimatedPose;
   for (const auto& result : cameraOne.GetAllUnreadResults()) {
-    estimatedPose = estimator.Update(result);
+    estimatedPose = estimator.EstimateLowestAmbiguityPose(result);
   }
   ASSERT_TRUE(estimatedPose);
   frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -154,12 +153,11 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
       photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
   cameraOne.testResult[0].SetReceiveTimestamp(17_s);
 
-  photon::PhotonPoseEstimator estimator(
-      aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}});
+  photon::PhotonPoseEstimator estimator(aprilTags, {{0_m, 0_m, 4_m}, {}});
 
   std::optional<photon::EstimatedRobotPose> estimatedPose;
   for (const auto& result : cameraOne.GetAllUnreadResults()) {
-    estimatedPose = estimator.Update(result);
+    estimatedPose = estimator.EstimateClosestToCameraHeightPose(result);
   }
   ASSERT_TRUE(estimatedPose);
 
@@ -203,14 +201,13 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
       photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
   cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
 
-  photon::PhotonPoseEstimator estimator(aprilTags,
-                                        photon::CLOSEST_TO_REFERENCE_POSE, {});
-  estimator.SetReferencePose(
-      frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
+  photon::PhotonPoseEstimator estimator(aprilTags, {});
 
   std::optional<photon::EstimatedRobotPose> estimatedPose;
   for (const auto& result : cameraOne.GetAllUnreadResults()) {
-    estimatedPose = estimator.Update(result);
+    estimatedPose = estimator.EstimateClosestToReferencePose(
+        result,
+        frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
   }
 
   ASSERT_TRUE(estimatedPose);
@@ -254,14 +251,13 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
       photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
   cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
 
-  photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
-                                        {});
-  estimator.SetLastPose(
-      frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
+  photon::PhotonPoseEstimator estimator(aprilTags, {});
 
   std::optional<photon::EstimatedRobotPose> estimatedPose;
   for (const auto& result : cameraOne.GetAllUnreadResults()) {
-    estimatedPose = estimator.Update(result);
+    estimatedPose = estimator.EstimateClosestToReferencePose(
+        result,
+        frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
   }
 
   ASSERT_TRUE(estimatedPose);
@@ -295,9 +291,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
       std::nullopt}};
   cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21));
 
-  // std::optional<photon::EstimatedRobotPose> estimatedPose;
   for (const auto& result : cameraOne.GetAllUnreadResults()) {
-    estimatedPose = estimator.Update(result);
+    estimatedPose = estimator.EstimateClosestToReferencePose(result, pose);
   }
 
   ASSERT_TRUE(estimatedPose);
@@ -327,8 +322,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
   frc::Transform3d compoundTestTransform = frc::Transform3d(
       -12_in, -11_in, 3_m, frc::Rotation3d(37_deg, 6_deg, 60_deg));
 
-  photon::PhotonPoseEstimator estimator(
-      aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
+  photon::PhotonPoseEstimator estimator(aprilTags, compoundTestTransform);
 
   /* real pose of the robot base to test against */
   frc::Pose3d realPose =
@@ -344,7 +338,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
 
   std::optional<photon::EstimatedRobotPose> estimatedPose;
   for (const auto& result : cameraOne.GetAllUnreadResults()) {
-    estimatedPose = estimator.Update(result);
+    estimatedPose = estimator.EstimatePnpDistanceTrigSolvePose(result);
   }
 
   ASSERT_TRUE(estimatedPose);
@@ -374,7 +368,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
 
   estimatedPose = std::nullopt;
   for (const auto& result : cameraOne.GetAllUnreadResults()) {
-    estimatedPose = estimator.Update(result);
+    estimatedPose = estimator.EstimatePnpDistanceTrigSolvePose(result);
   }
 
   ASSERT_TRUE(estimatedPose);
@@ -419,12 +413,11 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
       photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
   cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
 
-  photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
-                                        {});
+  photon::PhotonPoseEstimator estimator(aprilTags, {});
 
   std::optional<photon::EstimatedRobotPose> estimatedPose;
   for (const auto& result : cameraOne.GetAllUnreadResults()) {
-    estimatedPose = estimator.Update(result);
+    estimatedPose = estimator.EstimateAverageBestTargetsPose(result);
   }
 
   ASSERT_TRUE(estimatedPose);
@@ -437,101 +430,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
   EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
 }
 
-TEST(PhotonPoseEstimatorTest, PoseCache) {
-  photon::PhotonCamera cameraOne = photon::PhotonCamera("test2");
-
-  std::vector<photon::PhotonTrackedTarget> targets{
-      photon::PhotonTrackedTarget{
-          3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
-          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7, corners, detectedCorners},
-      photon::PhotonTrackedTarget{
-          3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
-          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3, corners, detectedCorners},
-      photon::PhotonTrackedTarget{
-          9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
-          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4, corners, detectedCorners}};
-
-  cameraOne.test = true;
-
-  photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
-                                        {});
-
-  // empty input, expect empty out
-  cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000, 1000},
-      std::vector<photon::PhotonTrackedTarget>{}, std::nullopt}};
-  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1));
-
-  std::optional<photon::EstimatedRobotPose> estimatedPose;
-  for (const auto& result : cameraOne.GetAllUnreadResults()) {
-    estimatedPose = estimator.Update(result);
-  }
-
-  EXPECT_FALSE(estimatedPose);
-
-  // Set result, and update -- expect present and timestamp to be 15
-  cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt}};
-  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
-
-  for (const auto& result : cameraOne.GetAllUnreadResults()) {
-    estimatedPose = estimator.Update(result);
-  }
-
-  ASSERT_TRUE(estimatedPose);
-  EXPECT_NEAR((15_s - 3_ms).to<double>(),
-              estimatedPose.value().timestamp.to<double>(), 1e-6);
-
-  // And again -- pose cache should result in returning std::nullopt
-  for (const auto& result : cameraOne.GetAllUnreadResults()) {
-    estimatedPose = estimator.Update(result);
-  }
-
-  EXPECT_FALSE(estimatedPose);
-
-  // If the camera produces a result that is > 1 micro second later,
-  // the pose cache should not be hit.
-  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(16));
-  for (const auto& result : cameraOne.GetAllUnreadResults()) {
-    estimatedPose = estimator.Update(result);
-  }
-
-  EXPECT_NEAR((16_s - 3_ms).to<double>(),
-              estimatedPose.value().timestamp.to<double>(), 1e-6);
-
-  // And again -- pose cache should result in returning std::nullopt
-  for (const auto& result : cameraOne.GetAllUnreadResults()) {
-    estimatedPose = estimator.Update(result);
-  }
-
-  EXPECT_FALSE(estimatedPose);
-
-  // Setting ReferencePose should also clear the cache
-  estimator.SetReferencePose(frc::Pose3d(units::meter_t(1), units::meter_t(2),
-                                         units::meter_t(3), frc::Rotation3d()));
-
-  for (const auto& result : cameraOne.GetAllUnreadResults()) {
-    estimatedPose = estimator.Update(result);
-  }
-
-  ASSERT_TRUE(estimatedPose);
-  EXPECT_NEAR((16_s - 3_ms).to<double>(),
-              estimatedPose.value().timestamp.to<double>(), 1e-6);
-}
-
-TEST(PhotonPoseEstimatorTest, MultiTagOnRioFallback) {
+TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) {
   photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
 
   std::vector<photon::PhotonTrackedTarget> targets{
@@ -555,12 +454,15 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnRioFallback) {
       photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
   cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
 
-  photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
-                                        frc::Transform3d{});
+  photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{});
 
   std::optional<photon::EstimatedRobotPose> estimatedPose;
   for (const auto& result : cameraOne.GetAllUnreadResults()) {
-    estimatedPose = estimator.Update(result);
+    estimatedPose = estimator.EstimateCoprocMultiTagPose(result);
+  }
+  ASSERT_FALSE(estimatedPose);
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.EstimateLowestAmbiguityPose(result);
   }
   ASSERT_TRUE(estimatedPose);
   frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -586,16 +488,6 @@ TEST(PhotonPoseEstimatorTest, CopyResult) {
               test2.GetTimestamp().to<double>(), 0.001);
 }
 
-TEST(PhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) {
-  photon::PhotonPoseEstimator estimator(
-      frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
-      photon::CONSTRAINED_SOLVEPNP, frc::Transform3d());
-
-  photon::PhotonPipelineResult result;
-  auto estimate = estimator.Update(result);
-  EXPECT_FALSE(estimate.has_value());
-}
-
 TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
   photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
   auto distortion = Eigen::VectorXd::Zero(8);
@@ -639,14 +531,17 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
 
   photon::PhotonPoseEstimator estimator(
       frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
-      photon::CONSTRAINED_SOLVEPNP, kRobotToCam);
+      kRobotToCam);
+
+  auto estimatedMultiTagPose =
+      estimator.EstimateCoprocMultiTagPose(cameraOne.testResult[0]);
 
   estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(),
                            frc::Rotation2d());
 
-  auto estimatedPose =
-      estimator.Update(cameraOne.testResult[0], cameraMat, distortion,
-                       photon::ConstrainedSolvepnpParams{true, 0});
+  auto estimatedPose = estimator.EstimateConstrainedSolvepnpPose(
+      cameraOne.testResult[0], cameraMat, distortion,
+      estimatedMultiTagPose->estimatedPose, true, 0);
 
   ASSERT_TRUE(estimatedPose.has_value());
 
diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
index afd3f4c3a..fee42fa18 100644
--- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
+++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
@@ -156,7 +156,7 @@ public class VisionEstimation {
      * @param visTags The visible tags reported by PV. Non-tag targets are automatically excluded.
      * @param robot2camera The {@link Transform3d} from the robot odometry frame to the camera optical
      *     frame
-     * @param robotPoseSeed An initial guess at robot pose, refined via optimizaiton. Better guesses
+     * @param robotPoseSeed An initial guess at robot pose, refined via optimization. Better guesses
      *     will converge faster.
      * @param tagLayout The known tag layout on the field
      * @param tagModel The physical size of the AprilTags
diff --git a/photonlib-cpp-examples/poseest/src/main/include/Vision.h b/photonlib-cpp-examples/poseest/src/main/include/Vision.h
index 429aca74f..b3dd50589 100644
--- a/photonlib-cpp-examples/poseest/src/main/include/Vision.h
+++ b/photonlib-cpp-examples/poseest/src/main/include/Vision.h
@@ -49,9 +49,6 @@ class Vision {
                             Eigen::Matrix<double, 3, 1>)>
              estConsumer)
       : estConsumer{estConsumer} {
-    photonEstimator.SetMultiTagFallbackStrategy(
-        photon::PoseStrategy::LOWEST_AMBIGUITY);
-
     if (frc::RobotBase::IsSimulation()) {
       visionSim = std::make_unique<photon::VisionSystemSim>("main");
 
@@ -79,7 +76,10 @@ class Vision {
     // Run each new pipeline result through our pose estimator
     for (const auto& result : camera.GetAllUnreadResults()) {
       // cache result and update pose estimator
-      auto visionEst = photonEstimator.Update(result);
+      auto visionEst = photonEstimator.EstimateCoprocMultiTagPose(result);
+      if (!visionEst) {
+        visionEst = photonEstimator.EstimateLowestAmbiguityPose(result);
+      }
       m_latestResult = result;
 
       // In sim only, add our vision estimate to the sim debug field
@@ -146,10 +146,8 @@ class Vision {
   frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); }
 
  private:
-  photon::PhotonPoseEstimator photonEstimator{
-      constants::Vision::kTagLayout,
-      photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR,
-      constants::Vision::kRobotToCam};
+  photon::PhotonPoseEstimator photonEstimator{constants::Vision::kTagLayout,
+                                              constants::Vision::kRobotToCam};
   photon::PhotonCamera camera{constants::Vision::kCameraName};
   std::unique_ptr<photon::VisionSystemSim> visionSim;
   std::unique_ptr<photon::SimCameraProperties> cameraProp;
diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java
index d7fce3947..4fe409ab6 100644
--- a/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java
+++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java
@@ -38,7 +38,6 @@ import java.util.Optional;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator;
-import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.simulation.PhotonCameraSim;
 import org.photonvision.simulation.SimCameraProperties;
 import org.photonvision.simulation.VisionSystemSim;
@@ -61,10 +60,7 @@ public class Vision {
     public Vision(EstimateConsumer estConsumer) {
         this.estConsumer = estConsumer;
         camera = new PhotonCamera(kCameraName);
-
-        photonEstimator =
-                new PhotonPoseEstimator(kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, kRobotToCam);
-        photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+        photonEstimator = new PhotonPoseEstimator(kTagLayout, kRobotToCam);
 
         // ----- Simulation
         if (Robot.isSimulation()) {
@@ -91,9 +87,12 @@ public class Vision {
 
     public void periodic() {
         Optional<EstimatedRobotPose> visionEst = Optional.empty();
-        for (var change : camera.getAllUnreadResults()) {
-            visionEst = photonEstimator.update(change);
-            updateEstimationStdDevs(visionEst, change.getTargets());
+        for (var result : camera.getAllUnreadResults()) {
+            visionEst = photonEstimator.estimateCoprocMultiTagPose(result);
+            if (visionEst.isEmpty()) {
+                visionEst = photonEstimator.estimateLowestAmbiguityPose(result);
+            }
+            updateEstimationStdDevs(visionEst, result.getTargets());
 
             if (Robot.isSimulation()) {
                 visionEst.ifPresentOrElse(
diff --git a/photonlib-python-examples/poseest/robot.py b/photonlib-python-examples/poseest/robot.py
index 3dcb0c471..f06e98286 100644
--- a/photonlib-python-examples/poseest/robot.py
+++ b/photonlib-python-examples/poseest/robot.py
@@ -27,7 +27,7 @@
 import drivetrain
 import wpilib
 import wpimath.geometry
-from photonlibpy import PhotonCamera, PhotonPoseEstimator, PoseStrategy
+from photonlibpy import PhotonCamera, PhotonPoseEstimator
 from robotpy_apriltag import AprilTagField, AprilTagFieldLayout
 
 kRobotToCam = wpimath.geometry.Transform3d(
@@ -44,14 +44,15 @@ class MyRobot(wpilib.TimedRobot):
         self.cam = PhotonCamera("YOUR CAMERA NAME")
         self.camPoseEst = PhotonPoseEstimator(
             AprilTagFieldLayout.loadField(AprilTagField.kDefaultField),
-            PoseStrategy.LOWEST_AMBIGUITY,
-            self.cam,
             kRobotToCam,
         )
 
     def robotPeriodic(self) -> None:
-        camEstPose = self.camPoseEst.update()
-        if camEstPose:
+        for result in self.cam.getAllUnreadResults():
+            camEstPose = self.camPoseEst.estimateCoprocMultiTagPose(result)
+            if camEstPose is None:
+                camEstPose = self.camPoseEst.estimateLowestAmbiguityPose(result)
+
             self.swerve.addVisionPoseEstimate(
                 camEstPose.estimatedPose, camEstPose.timestampSeconds
             )