diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml
new file mode 100644
index 0000000000..de54a54756
--- /dev/null
+++ b/.github/workflows/python.yml
@@ -0,0 +1,62 @@
+name: Build and Distribute PhotonLibPy
+
+permissions:
+  id-token: write  # IMPORTANT: this permission is mandatory for trusted publishing
+
+on:
+  push:
+    branches: [ master ]
+    tags:
+      - 'v*'
+  pull_request:
+    branches: [ master ]
+
+jobs:
+  buildAndDeploy:
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: Checkout code
+        uses: actions/checkout@v3
+        with:
+          sparse-checkout-cone-mode: false
+          fetch-tags: true
+          fetch-depth: 99999
+
+      - name: Set up Python
+        uses: actions/setup-python@v5
+        with:
+          python-version: 3.11
+
+      - name: Install dependencies
+        run: |
+          python -m pip install --upgrade pip
+          pip install setuptools wheel pytest
+
+      - name: Build wheel
+        working-directory: ./photon-lib/py
+        run: |
+          python setup.py sdist bdist_wheel
+
+      - name: Run Unit Tests
+        working-directory: ./photon-lib/py
+        run: |
+            pip install --no-cache-dir dist/*.whl
+            pytest
+
+
+      - name: Upload artifacts
+        uses: actions/upload-artifact@master
+        with:
+          name: dist
+          path: ./photon-lib/py/dist/
+
+      - name: Publish package distributions to TestPyPI
+        # Only upload on tags
+        if: startsWith(github.ref, 'refs/tags/v')
+        uses: pypa/gh-action-pypi-publish@release/v1
+        with:
+          packages_dir: ./photon-lib/py/dist/
+
+    permissions:
+      id-token: write  # IMPORTANT: this permission is mandatory for trusted publishing
diff --git a/photon-lib/py/.gitignore b/photon-lib/py/.gitignore
new file mode 100644
index 0000000000..f0c4ced6d6
--- /dev/null
+++ b/photon-lib/py/.gitignore
@@ -0,0 +1,5 @@
+photonlibpy.egg-info/
+dist/
+build/
+.eggs/
+photonlibpy/version.py
diff --git a/photon-lib/py/buildAndTest.bat b/photon-lib/py/buildAndTest.bat
new file mode 100644
index 0000000000..b9774a54d6
--- /dev/null
+++ b/photon-lib/py/buildAndTest.bat
@@ -0,0 +1,14 @@
+:: Uninstall if it already was installed
+pip uninstall -y photonlibpy
+
+:: Build wheel
+python setup.py bdist_wheel
+
+:: Install whatever wheel was made
+for %%f in (dist/*.whl) do (
+    echo installing dist/%%f
+    pip install --no-cache-dir dist/%%f
+)
+
+:: Run the test suite
+pytest
diff --git a/photon-lib/py/photonlibpy/__init__.py b/photon-lib/py/photonlibpy/__init__.py
new file mode 100644
index 0000000000..d5b258f5e7
--- /dev/null
+++ b/photon-lib/py/photonlibpy/__init__.py
@@ -0,0 +1 @@
+# No one here but us chickens
diff --git a/photon-lib/py/photonlibpy/multiTargetPNPResult.py b/photon-lib/py/photonlibpy/multiTargetPNPResult.py
new file mode 100644
index 0000000000..63bbb34301
--- /dev/null
+++ b/photon-lib/py/photonlibpy/multiTargetPNPResult.py
@@ -0,0 +1,45 @@
+from dataclasses import dataclass, field
+from wpimath.geometry import Transform3d
+from photonlibpy.packet import Packet
+
+
+@dataclass
+class PNPResult:
+    _NUM_BYTES_IN_FLOAT = 8
+    PACK_SIZE_BYTES = 1 + (_NUM_BYTES_IN_FLOAT * 7 * 2) + (_NUM_BYTES_IN_FLOAT * 3)
+
+    isPresent: bool = False
+    best: Transform3d = field(default_factory=Transform3d)
+    alt: Transform3d = field(default_factory=Transform3d)
+    ambiguity: float = 0.0
+    bestReprojError: float = 0.0
+    altReprojError: float = 0.0
+
+    def createFromPacket(self, packet: Packet) -> Packet:
+        self.isPresent = packet.decodeBoolean()
+        self.best = packet.decodeTransform()
+        self.alt = packet.decodeTransform()
+        self.bestReprojError = packet.decodeDouble()
+        self.altReprojError = packet.decodeDouble()
+        self.ambiguity = packet.decodeDouble()
+        return packet
+
+
+@dataclass
+class MultiTargetPNPResult:
+    _MAX_IDS = 32
+    # pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally)
+    _PACK_SIZE_BYTES = PNPResult.PACK_SIZE_BYTES + (1 * _MAX_IDS)
+
+    estimatedPose: PNPResult = field(default_factory=PNPResult)
+    fiducialIDsUsed: list[int] = field(default_factory=list)
+
+    def createFromPacket(self, packet: Packet) -> Packet:
+        self.estimatedPose = PNPResult()
+        self.estimatedPose.createFromPacket(packet)
+        self.fiducialIDsUsed = []
+        for _ in range(MultiTargetPNPResult._MAX_IDS):
+            fidId = packet.decode16()
+            if fidId >= 0:
+                self.fiducialIDsUsed.append(fidId)
+        return packet
diff --git a/photon-lib/py/photonlibpy/packet.py b/photon-lib/py/photonlibpy/packet.py
new file mode 100644
index 0000000000..c3816d94f8
--- /dev/null
+++ b/photon-lib/py/photonlibpy/packet.py
@@ -0,0 +1,143 @@
+import struct
+from wpimath.geometry import Transform3d, Translation3d, Rotation3d, Quaternion
+import wpilib
+
+
+class Packet:
+    def __init__(self, data: list[int]):
+        """
+        * Constructs an empty packet.
+        *
+        * @param self.size The self.size of the packet buffer.
+        """
+        self.packetData = data
+        self.size = len(data)
+        self.readPos = 0
+        self.outOfBytes = False
+
+    def clear(self):
+        """Clears the packet and resets the read and write positions."""
+        self.packetData = [0] * self.size
+        self.readPos = 0
+        self.outOfBytes = False
+
+    def getSize(self):
+        return self.size
+
+    _NO_MORE_BYTES_MESSAGE = """
+    Photonlib - Ran out of bytes while decoding.
+    Make sure the version of photonvision on the coprocessor
+    matches the version of photonlib running in the robot code.
+    """
+
+    def _getNextByte(self) -> int:
+        retVal = 0x00
+
+        if not self.outOfBytes:
+            try:
+                retVal = 0x00FF & self.packetData[self.readPos]
+                self.readPos += 1
+            except IndexError:
+                wpilib.reportError(Packet._NO_MORE_BYTES_MESSAGE, True)
+                self.outOfBytes = True
+
+        return retVal
+
+    def getData(self) -> list[int]:
+        """
+        * Returns the packet data.
+        *
+        * @return The packet data.
+        """
+        return self.packetData
+
+    def setData(self, data: list[int]):
+        """
+        * Sets the packet data.
+        *
+        * @param data The packet data.
+        """
+        self.clear()
+        self.packetData = data
+        self.size = len(self.packetData)
+
+    def _decodeGeneric(self, unpackFormat, numBytes):
+        # Read ints in from the data buffer
+        intList = []
+        for _ in range(numBytes):
+            intList.append(self._getNextByte())
+
+        # Interpret the bytes as a floating point number
+        value = struct.unpack(unpackFormat, bytes(intList))[0]
+
+        return value
+
+    def decode8(self) -> int:
+        """
+        * Returns a single decoded byte from the packet.
+        *
+        * @return A decoded byte from the packet.
+        """
+        return self._decodeGeneric(">b", 1)
+
+    def decode16(self) -> int:
+        """
+        * Returns a single decoded byte from the packet.
+        *
+        * @return A decoded byte from the packet.
+        """
+        return self._decodeGeneric(">h", 2)
+
+    def decode32(self) -> int:
+        """
+        * Returns a decoded int (32 bytes) from the packet.
+        *
+        * @return A decoded int from the packet.
+        """
+        return self._decodeGeneric(">l", 4)
+
+    def decodeDouble(self) -> float:
+        """
+        * Returns a decoded double from the packet.
+        *
+        * @return A decoded double from the packet.
+        """
+        return self._decodeGeneric(">d", 8)
+
+    def decodeBoolean(self) -> bool:
+        """
+        * Returns a decoded boolean from the packet.
+        *
+        * @return A decoded boolean from the packet.
+        """
+        return self.decode8() == 1
+
+    def decodeDoubleArray(self, length: int) -> list[float]:
+        """
+        * Returns a decoded array of floats from the packet.
+        *
+        * @return A decoded array of floats from the packet.
+        """
+        ret = []
+        for _ in range(length):
+            ret.append(self.decodeDouble())
+        return ret
+
+    def decodeTransform(self) -> Transform3d:
+        """
+        * Returns a decoded Transform3d
+        *
+        * @return A decoded Tansform3d from the packet.
+        """
+        x = self.decodeDouble()
+        y = self.decodeDouble()
+        z = self.decodeDouble()
+        translation = Translation3d(x, y, z)
+
+        w = self.decodeDouble()
+        x = self.decodeDouble()
+        y = self.decodeDouble()
+        z = self.decodeDouble()
+        rotation = Rotation3d(Quaternion(w, x, y, z))
+
+        return Transform3d(translation, rotation)
diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py
new file mode 100644
index 0000000000..0d474a2434
--- /dev/null
+++ b/photon-lib/py/photonlibpy/photonCamera.py
@@ -0,0 +1,170 @@
+from enum import Enum
+import ntcore
+from wpilib import Timer
+import wpilib
+from photonlibpy.packet import Packet
+from photonlibpy.photonPipelineResult import PhotonPipelineResult
+from photonlibpy.version import PHOTONVISION_VERSION, PHOTONLIB_VERSION
+
+
+class VisionLEDMode(Enum):
+    kDefault = -1
+    kOff = 0
+    kOn = 1
+    kBlink = 2
+
+
+lastVersionTimeCheck = 0.0
+_VERSION_CHECK_ENABLED = True
+
+
+def setVersionCheckEnabled(enabled: bool):
+    _VERSION_CHECK_ENABLED = enabled
+
+
+class PhotonCamera:
+    def __init__(self, cameraName: str):
+        instance = ntcore.NetworkTableInstance.getDefault()
+        self.name = cameraName
+        self._tableName = "photonvision"
+        photonvision_root_table = instance.getTable(self._tableName)
+        self.cameraTable = photonvision_root_table.getSubTable(cameraName)
+        self.path = self.cameraTable.getPath()
+        self.rawBytesEntry = self.cameraTable.getRawTopic("rawBytes").subscribe(
+            "rawBytes", bytes([]), ntcore.PubSubOptions(periodic=0.01, sendAll=True)
+        )
+
+        self.driverModePublisher = self.cameraTable.getBooleanTopic(
+            "driverModeRequest"
+        ).publish()
+        self.driverModeSubscriber = self.cameraTable.getBooleanTopic(
+            "driverMode"
+        ).subscribe(False)
+        self.inputSaveImgEntry = self.cameraTable.getIntegerTopic(
+            "inputSaveImgCmd"
+        ).getEntry(0)
+        self.outputSaveImgEntry = self.cameraTable.getIntegerTopic(
+            "outputSaveImgCmd"
+        ).getEntry(0)
+        self.pipelineIndexRequest = self.cameraTable.getIntegerTopic(
+            "pipelineIndexRequest"
+        ).publish()
+        self.pipelineIndexState = self.cameraTable.getIntegerTopic(
+            "pipelineIndexState"
+        ).subscribe(0)
+        self.heartbeatEntry = self.cameraTable.getIntegerTopic("heartbeat").subscribe(
+            -1
+        )
+
+        self.ledModeRequest = photonvision_root_table.getIntegerTopic(
+            "ledModeRequest"
+        ).publish()
+        self.ledModeState = photonvision_root_table.getIntegerTopic(
+            "ledModeState"
+        ).subscribe(-1)
+        self.versionEntry = photonvision_root_table.getStringTopic("version").subscribe(
+            ""
+        )
+
+        # Existing is enough to make this multisubscriber do its thing
+        self.topicNameSubscriber = ntcore.MultiSubscriber(
+            instance, ["/photonvision/"], ntcore.PubSubOptions(topicsOnly=True)
+        )
+
+        self.prevHeartbeat = 0
+        self.prevHeartbeatChangeTime = Timer.getFPGATimestamp()
+
+    def getLatestResult(self) -> PhotonPipelineResult:
+        self._versionCheck()
+
+        retVal = PhotonPipelineResult()
+        packetWithTimestamp = self.rawBytesEntry.getAtomic()
+        byteList = packetWithTimestamp.value
+        timestamp = packetWithTimestamp.time
+
+        if len(byteList) < 1:
+            return retVal
+        else:
+            retVal.populateFromPacket(Packet(byteList))
+            # NT4 allows us to correct the timestamp based on when the message was sent
+            retVal.setTimestampSeconds(
+                timestamp / 1e-6 - retVal.getLatencyMillis() / 1e-3
+            )
+            return retVal
+
+    def getDriverMode(self) -> bool:
+        return self.driverModeSubscriber.get()
+
+    def setDriverMode(self, driverMode: bool) -> None:
+        self.driverModePublisher.set(driverMode)
+
+    def takeInputSnapshot(self) -> None:
+        self.inputSaveImgEntry.set(self.inputSaveImgEntry.get() + 1)
+
+    def takeOutputSnapshot(self) -> None:
+        self.outputSaveImgEntry.set(self.outputSaveImgEntry.get() + 1)
+
+    def getPipelineIndex(self) -> int:
+        return self.pipelineIndexState.get(0)
+
+    def setPipelineIndex(self, index: int) -> None:
+        self.pipelineIndexRequest.set(index)
+
+    def getLEDMode(self) -> VisionLEDMode:
+        mode = self.ledModeState.get()
+        return VisionLEDMode(mode)
+
+    def setLEDMode(self, led: VisionLEDMode) -> None:
+        self.ledModeRequest.set(led.value)
+
+    def getName(self) -> str:
+        return self.name
+
+    def isConnected(self) -> bool:
+        curHeartbeat = self.heartbeatEntry.get()
+        now = Timer.getFPGATimestamp()
+
+        if curHeartbeat != self.prevHeartbeat:
+            self.prevHeartbeat = curHeartbeat
+            self.prevHeartbeatChangeTime = now
+
+        return (now - self.prevHeartbeatChangeTime) < 0.5
+
+    def _versionCheck(self) -> None:
+        if not _VERSION_CHECK_ENABLED:
+            return
+
+        if (Timer.getFPGATimestamp() - lastVersionTimeCheck) < 5.0:
+            return
+
+        if not self.heartbeatEntry.exists():
+            cameraNames = (
+                self.cameraTable.getInstance().getTable(self._tableName).getSubTables()
+            )
+            if len(cameraNames) == 0:
+                wpilib.reportError(
+                    "Could not find any PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!",
+                    False,
+                )
+            else:
+                wpilib.reportError(
+                    f"PhotonVision coprocessor at path {self.path} not found in Network Tables. Double check that your camera names match! Only the following camera names were found: { ''.join(cameraNames)}",
+                    True,
+                )
+
+        elif not self.isConnected():
+            wpilib.reportWarning(
+                f"PhotonVision coprocessor at path {self.path} is not sending new data.",
+                True,
+            )
+
+        versionString = self.versionEntry.get(defaultValue="")
+        if len(versionString) > 0 and versionString != PHOTONVISION_VERSION:
+            wpilib.reportWarning(
+                "Photon version "
+                + PHOTONVISION_VERSION
+                + " does not match coprocessor version "
+                + versionString
+                + f"! Please install photonlibpy version {PHOTONLIB_VERSION}",
+                True,
+            )
diff --git a/photon-lib/py/photonlibpy/photonPipelineResult.py b/photon-lib/py/photonlibpy/photonPipelineResult.py
new file mode 100644
index 0000000000..3fe02c9fea
--- /dev/null
+++ b/photon-lib/py/photonlibpy/photonPipelineResult.py
@@ -0,0 +1,38 @@
+from dataclasses import dataclass, field
+
+from photonlibpy.multiTargetPNPResult import MultiTargetPNPResult
+from photonlibpy.packet import Packet
+from photonlibpy.photonTrackedTarget import PhotonTrackedTarget
+
+
+@dataclass
+class PhotonPipelineResult:
+    latencyMillis: float = -1.0
+    timestampSec: float = -1.0
+    targets: list[PhotonTrackedTarget] = field(default_factory=list)
+    multiTagResult: MultiTargetPNPResult = field(default_factory=MultiTargetPNPResult)
+
+    def populateFromPacket(self, packet: Packet) -> Packet:
+        self.targets = []
+        self.latencyMillis = packet.decodeDouble()
+        self.multiTagResult = MultiTargetPNPResult()
+        self.multiTagResult.createFromPacket(packet)
+        targetCount = packet.decode8()
+        for _ in range(targetCount):
+            target = PhotonTrackedTarget()
+            target.createFromPacket(packet)
+            self.targets.append(target)
+
+        return packet
+
+    def setTimestampSeconds(self, timestampSec: float) -> None:
+        self.timestampSec = timestampSec
+
+    def getLatencyMillis(self) -> float:
+        return self.latencyMillis
+
+    def getTimestamp(self) -> float:
+        return self.timestampSec
+
+    def getTargets(self) -> list[PhotonTrackedTarget]:
+        return self.targets
diff --git a/photon-lib/py/photonlibpy/photonTrackedTarget.py b/photon-lib/py/photonlibpy/photonTrackedTarget.py
new file mode 100644
index 0000000000..56c225d997
--- /dev/null
+++ b/photon-lib/py/photonlibpy/photonTrackedTarget.py
@@ -0,0 +1,82 @@
+from dataclasses import dataclass, field
+from wpimath.geometry import Transform3d
+from photonlibpy.packet import Packet
+
+
+@dataclass
+class TargetCorner:
+    x: float
+    y: float
+
+
+@dataclass
+class PhotonTrackedTarget:
+    _MAX_CORNERS = 8
+    _NUM_BYTES_IN_FLOAT = 8
+    _PACK_SIZE_BYTES = _NUM_BYTES_IN_FLOAT * (5 + 7 + 2 * 4 + 1 + 7 + 2 * _MAX_CORNERS)
+
+    yaw: float = 0.0
+    pitch: float = 0.0
+    area: float = 0.0
+    skew: float = 0.0
+    fiducialId: int = -1
+    bestCameraToTarget: Transform3d = field(default_factory=Transform3d)
+    altCameraToTarget: Transform3d = field(default_factory=Transform3d)
+    minAreaRectCorners: list[TargetCorner] | None = None
+    detectedCorners: list[TargetCorner] | None = None
+    poseAmbiguity: float = 0.0
+
+    def getYaw(self) -> float:
+        return self.yaw
+
+    def getPitch(self) -> float:
+        return self.pitch
+
+    def getArea(self) -> float:
+        return self.area
+
+    def getSkew(self) -> float:
+        return self.skew
+
+    def getFiducialId(self) -> int:
+        return self.fiducialId
+
+    def getPoseAmbiguity(self) -> float:
+        return self.poseAmbiguity
+
+    def getMinAreaRectCorners(self) -> list[TargetCorner] | None:
+        return self.minAreaRectCorners
+
+    def getDetectedCorners(self) -> list[TargetCorner] | None:
+        return self.detectedCorners
+
+    def getBestCameraToTarget(self) -> Transform3d:
+        return self.bestCameraToTarget
+
+    def getAlternateCameraToTarget(self) -> Transform3d:
+        return self.altCameraToTarget
+
+    def _decodeTargetList(self, packet: Packet, numTargets: int) -> list[TargetCorner]:
+        retList = []
+        for _ in range(numTargets):
+            cx = packet.decodeDouble()
+            cy = packet.decodeDouble()
+            retList.append(TargetCorner(cx, cy))
+        return retList
+
+    def createFromPacket(self, packet: Packet) -> Packet:
+        self.yaw = packet.decodeDouble()
+        self.pitch = packet.decodeDouble()
+        self.area = packet.decodeDouble()
+        self.skew = packet.decodeDouble()
+        self.fiducialId = packet.decode32()
+
+        self.bestCameraToTarget = packet.decodeTransform()
+        self.altCameraToTarget = packet.decodeTransform()
+
+        self.poseAmbiguity = packet.decodeDouble()
+
+        self.minAreaRectCorners = self._decodeTargetList(packet, 4)  # always four
+        numCorners = packet.decode8()
+        self.detectedCorners = self._decodeTargetList(packet, numCorners)
+        return packet
diff --git a/photon-lib/py/setup.py b/photon-lib/py/setup.py
new file mode 100644
index 0000000000..b1c3a06133
--- /dev/null
+++ b/photon-lib/py/setup.py
@@ -0,0 +1,54 @@
+from setuptools import setup, find_packages
+import subprocess, re
+
+gitDescribeResult = (
+    subprocess.check_output(["git", "describe", "--tags", "--match=v*", "--always"])
+    .decode("utf-8")
+    .strip()
+)
+
+m = re.search(
+    r"(v[0-9]{4}\.[0-9]{1}\.[0-9]{1})-?((?:beta)?(?:alpha)?)-?([0-9\.]*)",
+    gitDescribeResult,
+)
+
+# Extract the first portion of the git describe result
+# which should be PEP440 compliant
+if m:
+    versionString = m.group(0)
+    prefix = m.group(1)
+    maturity = m.group(2)
+    suffix = m.group(3).replace(".", "")
+    versionString = f"{prefix}.{maturity}.{suffix}"
+
+
+else:
+    print("Warning, no valid version found")
+    versionString = gitDescribeResult
+
+print(f"Building version {versionString}")
+
+# Put the version info into a python file for runtime access
+with open("photonlibpy/version.py", "w", encoding="utf-8") as fp:
+    fp.write(f'PHOTONLIB_VERSION="{versionString}"\n')
+    fp.write(f'PHOTONVISION_VERSION="{gitDescribeResult}"\n')
+
+
+descriptionStr = f"""
+Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors.
+Implemented with PhotonVision version {gitDescribeResult} .
+"""
+
+setup(
+    name="photonlibpy",
+    packages=find_packages(),
+    version=versionString,
+    install_requires=[
+        "wpilib<2025,>=2024.0.0b2",
+        "robotpy-wpimath<2025,>=2024.0.0b2",
+        "pyntcore<2025,>=2024.0.0b2",
+    ],
+    description=descriptionStr,
+    url="https://photonvision.org",
+    author="Photonvision Development Team",
+)
diff --git a/photon-lib/py/test/data.py b/photon-lib/py/test/data.py
new file mode 100644
index 0000000000..8c317dfe2f
--- /dev/null
+++ b/photon-lib/py/test/data.py
@@ -0,0 +1,239 @@
+# fmt: off
+rawBytes1 = [
+    64, 166, 117, 41, 225, 243, 165, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 0
+]
+rawBytes2 = [
+    64, 114, 72, 58, 227, 96, 141, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 0
+]
+rawBytes3 = [
+    64, 55, 65, 189, 215, 102, 131, 195, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 0
+]
+rawBytes4 = [
+    64, 115, 23, 245, 248, 9, 145, 121, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 1, 64, 41, 32, 212, 70, 53, 253, 38, 64, 19, 140,
+    198, 187, 206, 56, 251, 64, 38, 63, 170, 170, 170, 170, 170, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 17, 63, 226, 15, 19, 123, 229, 152, 236, 191, 191, 225,
+    27, 96, 1, 219, 30, 63, 168, 175, 103, 65, 172, 24, 8, 63, 115, 51, 123,
+    216, 202, 14, 128, 191, 155, 163, 119, 215, 217, 209, 224, 63, 212, 76, 79,
+    227, 166, 197, 80, 63, 238, 85, 211, 252, 64, 132, 136, 63, 226, 13, 126,
+    170, 219, 202, 209, 191, 190, 134, 80, 94, 25, 179, 17, 63, 168, 92, 53,
+    102, 36, 28, 64, 63, 204, 203, 52, 12, 186, 226, 51, 63, 148, 67, 104, 89,
+    131, 114, 208, 63, 211, 104, 18, 25, 149, 138, 78, 63, 237, 159, 242, 53,
+    211, 204, 51, 63, 217, 254, 169, 82, 190, 36, 22, 64, 123, 96, 0, 6, 100,
+    53, 178, 64, 112, 207, 255, 241, 198, 25, 18, 64, 132, 215, 255, 254, 189,
+    61, 109, 64, 86, 191, 255, 217, 164, 214, 161, 64, 138, 55, 94, 60, 205,
+    229, 39, 64, 115, 130, 222, 78, 57, 230, 238, 64, 131, 15, 94, 65, 66, 194,
+    147, 64, 126, 162, 222, 73, 150, 202, 88, 4, 64, 130, 248, 64, 192, 0, 0, 0,
+    64, 126, 15, 133, 64, 0, 0, 0, 64, 137, 206, 237, 128, 0, 0, 0, 64, 116, 48,
+    240, 32, 0, 0, 0, 64, 132, 218, 43, 96, 0, 0, 0, 64, 86, 210, 155, 128, 0,
+    0, 0, 64, 123, 102, 127, 192, 0, 0, 0, 64, 112, 211, 233, 96, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+]
+rawBytes5 = [
+    64, 102, 149, 235, 181, 90, 192, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 1, 64, 21, 210, 112, 148, 86, 4, 131, 64, 3, 87,
+    196, 18, 174, 105, 145, 64, 47, 80, 237, 9, 123, 66, 95, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 17, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0,
+    0, 0, 0, 0, 64, 116, 192, 0, 0, 0, 0, 0, 64, 127, 128, 0, 0, 0, 0, 0, 64,
+    116, 192, 0, 0, 0, 0, 0, 64, 100, 96, 0, 0, 0, 0, 0, 64, 133, 72, 0, 0, 0,
+    0, 0, 64, 100, 96, 0, 0, 0, 0, 0, 64, 133, 72, 0, 0, 0, 0, 0, 64, 127, 128,
+    0, 0, 0, 0, 0, 4, 64, 133, 78, 45, 224, 0, 0, 0, 64, 127, 129, 184, 160, 0,
+    0, 0, 64, 133, 78, 1, 192, 0, 0, 0, 64, 100, 100, 194, 224, 0, 0, 0, 64,
+    118, 181, 224, 64, 0, 0, 0, 64, 102, 98, 136, 0, 0, 0, 0, 64, 116, 207, 155,
+    64, 0, 0, 0, 64, 126, 121, 100, 96, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0
+]
+rawBytes6 = [
+    64, 78, 129, 235, 32, 116, 234, 142, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 8, 64, 43, 213, 73, 8, 241, 221, 240, 192, 2, 146,
+    71, 190, 201, 205, 25, 64, 37, 96, 141, 183, 156, 102, 0, 192, 9, 112, 76,
+    0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191,
+    240, 0, 0, 0, 0, 0, 0, 64, 101, 10, 36, 92, 234, 132, 108, 64, 89, 45, 24,
+    60, 197, 216, 218, 64, 114, 7, 35, 134, 17, 204, 20, 64, 87, 129, 247, 219,
+    201, 12, 226, 64, 114, 67, 177, 81, 138, 189, 202, 64, 100, 68, 236, 33,
+    157, 19, 147, 64, 101, 131, 63, 243, 220, 103, 216, 64, 101, 26, 124, 82,
+    27, 121, 143, 0, 192, 32, 131, 181, 181, 155, 145, 13, 192, 37, 92, 235, 61,
+    221, 83, 253, 63, 173, 176, 233, 61, 133, 212, 255, 192, 73, 171, 139, 128,
+    0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191,
+    240, 0, 0, 0, 0, 0, 0, 64, 92, 42, 37, 190, 203, 146, 101, 64, 102, 61, 168,
+    153, 137, 73, 186, 64, 94, 48, 100, 45, 88, 178, 51, 64, 100, 249, 193, 146,
+    50, 77, 24, 64, 94, 220, 25, 65, 52, 109, 155, 64, 101, 62, 112, 102, 118,
+    182, 70, 64, 92, 213, 218, 210, 167, 77, 205, 64, 102, 130, 87, 109, 205,
+    178, 232, 0, 192, 19, 156, 59, 67, 88, 173, 35, 192, 37, 85, 193, 76, 240,
+    22, 41, 63, 152, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 255, 255,
+    255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64,
+    96, 128, 0, 0, 0, 0, 0, 64, 101, 128, 0, 0, 0, 0, 0, 64, 97, 64, 0, 0, 0, 0,
+    0, 64, 101, 128, 0, 0, 0, 0, 0, 64, 97, 64, 0, 0, 0, 0, 0, 64, 101, 224, 0,
+    0, 0, 0, 0, 64, 96, 128, 0, 0, 0, 0, 0, 64, 101, 224, 0, 0, 0, 0, 0, 0, 64,
+    48, 164, 171, 25, 83, 4, 154, 192, 34, 178, 85, 3, 174, 51, 62, 63, 147,
+    255, 255, 245, 245, 4, 85, 192, 28, 128, 4, 0, 0, 0, 0, 255, 255, 255, 255,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63,
+    240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 110,
+    24, 157, 164, 237, 183, 149, 64, 101, 4, 236, 103, 73, 0, 224, 64, 111, 24,
+    157, 165, 1, 191, 69, 64, 100, 228, 236, 103, 153, 126, 38, 64, 111, 32, 0,
+    27, 18, 72, 107, 64, 101, 32, 0, 24, 182, 255, 32, 64, 110, 32, 0, 26, 254,
+    64, 187, 64, 101, 64, 0, 24, 102, 129, 218, 0, 63, 247, 149, 178, 38, 100,
+    246, 86, 192, 9, 184, 134, 230, 194, 222, 110, 63, 150, 102, 102, 85, 138,
+    188, 43, 64, 81, 228, 41, 192, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 100, 147, 51, 63,
+    205, 146, 189, 64, 96, 153, 153, 160, 125, 112, 225, 64, 100, 224, 0, 12,
+    123, 135, 101, 64, 96, 128, 0, 6, 164, 45, 139, 64, 101, 35, 51, 64, 50,
+    109, 67, 64, 97, 73, 153, 159, 130, 143, 31, 64, 100, 214, 102, 115, 132,
+    120, 155, 64, 97, 99, 51, 57, 91, 210, 117, 0, 192, 33, 131, 192, 140, 90,
+    220, 117, 192, 54, 53, 227, 123, 209, 81, 175, 63, 136, 0, 0, 0, 0, 0, 0,
+    64, 86, 128, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 92, 128, 0, 0, 0, 0, 0, 64,
+    109, 128, 0, 0, 0, 0, 0, 64, 93, 64, 0, 0, 0, 0, 0, 64, 109, 128, 0, 0, 0,
+    0, 0, 64, 93, 64, 0, 0, 0, 0, 0, 64, 109, 224, 0, 0, 0, 0, 0, 64, 92, 128,
+    0, 0, 0, 0, 0, 64, 109, 224, 0, 0, 0, 0, 0, 0, 192, 25, 145, 202, 146, 13,
+    244, 248, 192, 36, 180, 8, 18, 34, 149, 42, 63, 128, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 95, 128, 0, 0, 0, 0, 0, 64, 101, 96, 0,
+    0, 0, 0, 0, 64, 96, 32, 0, 0, 0, 0, 0, 64, 101, 96, 0, 0, 0, 0, 0, 64, 96,
+    32, 0, 0, 0, 0, 0, 64, 101, 160, 0, 0, 0, 0, 0, 64, 95, 128, 0, 0, 0, 0, 0,
+    64, 101, 160, 0, 0, 0, 0, 0, 0, 192, 35, 144, 241, 16, 205, 7, 236, 192, 0,
+    254, 21, 96, 184, 112, 69, 63, 135, 255, 255, 219, 151, 33, 85, 192, 70,
+    128, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 91, 0, 0, 64, 109, 58, 156, 64, 96,
+    96, 0, 31, 237, 203, 144, 64, 91, 192, 0, 63, 219, 151, 34, 64, 96, 0, 0,
+    32, 54, 157, 78, 64, 92, 32, 0, 63, 146, 197, 100, 64, 96, 48, 0, 32, 18,
+    52, 112, 64, 91, 96, 0, 64, 36, 104, 222, 64, 96, 144, 0, 31, 201, 98, 178,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+]
diff --git a/photon-lib/py/test/photonlibpy_test.py b/photon-lib/py/test/photonlibpy_test.py
new file mode 100644
index 0000000000..92b1636351
--- /dev/null
+++ b/photon-lib/py/test/photonlibpy_test.py
@@ -0,0 +1,46 @@
+from photonlibpy.packet import Packet
+from photonlibpy.photonPipelineResult import PhotonPipelineResult
+from data import rawBytes1
+from data import rawBytes2
+from data import rawBytes3
+from data import rawBytes4
+from data import rawBytes5
+from data import rawBytes6
+
+
+def setupCommon(bytesIn):
+    res = PhotonPipelineResult()
+    packet = Packet(bytesIn)
+    res.populateFromPacket(packet)
+    assert packet.outOfBytes is False
+    return res
+
+
+def test_byteParse1():
+    res = setupCommon(rawBytes1)
+    assert len(res.getTargets()) == 0
+
+
+def test_byteParse2():
+    res = setupCommon(rawBytes2)
+    assert len(res.getTargets()) == 0
+
+
+def test_byteParse3():
+    res = setupCommon(rawBytes3)
+    assert len(res.getTargets()) == 0
+
+
+def test_byteParse4():
+    res = setupCommon(rawBytes4)
+    assert len(res.getTargets()) == 1
+
+
+def test_byteParse5():
+    res = setupCommon(rawBytes5)
+    assert len(res.getTargets()) == 1
+
+
+def test_byteParse6():
+    res = setupCommon(rawBytes6)
+    assert len(res.getTargets()) > 6
diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
index 95eecb444c..02d9b46527 100644
--- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
+++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
@@ -24,8 +24,11 @@
 
 #include "photon/PhotonCamera.h"
 
+#include <hal/FRCUsageReporting.h>
+
 #include <frc/Errors.h>
 #include <frc/Timer.h>
+#include <opencv2/core.hpp>
 #include <opencv2/core/mat.hpp>
 
 #include "PhotonVersion.h"
@@ -68,7 +71,10 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
           rootTable->GetBooleanTopic("driverModeRequest").Publish()),
       m_topicNameSubscriber(instance, PHOTON_PREFIX, {.topicsOnly = true}),
       path(rootTable->GetPath()),
-      m_cameraName(cameraName) {}
+      m_cameraName(cameraName) {
+  HAL_Report(HALUsageReporting::kResourceType_PhotonCamera, InstanceCount);
+  InstanceCount++;
+}
 
 PhotonCamera::PhotonCamera(const std::string_view cameraName)
     : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {}
@@ -128,8 +134,13 @@ LEDMode PhotonCamera::GetLEDMode() const {
 std::optional<cv::Mat> PhotonCamera::GetCameraMatrix() {
   auto camCoeffs = cameraIntrinsicsSubscriber.Get();
   if (camCoeffs.size() == 9) {
-    // clone should deal with ownership concerns? not sure
-    return cv::Mat(3, 3, CV_64FC1, camCoeffs.data()).clone();
+    cv::Mat retVal(3, 3, CV_64FC1);
+    for (int i = 0; i < 3; i++) {
+      for (int j = 0; j < 3; j++) {
+        retVal.at<double>(i, j) = camCoeffs[(j * 3) + i];
+      }
+    }
+    return retVal;
   }
   return std::nullopt;
 }
@@ -145,8 +156,11 @@ const std::string_view PhotonCamera::GetCameraName() const {
 std::optional<cv::Mat> PhotonCamera::GetDistCoeffs() {
   auto distCoeffs = cameraDistortionSubscriber.Get();
   if (distCoeffs.size() == 5) {
-    // clone should deal with ownership concerns? not sure
-    return cv::Mat(5, 1, CV_64FC1, distCoeffs.data()).clone();
+    cv::Mat retVal(5, 1, CV_64FC1);
+    for (int i = 0; i < 5; i++) {
+      retVal.at<double>(i, 0) = distCoeffs[i];
+    }
+    return retVal;
   }
   return std::nullopt;
 }
diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
index f2ff30f7ea..30bc6de2df 100644
--- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
+++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
@@ -24,6 +24,8 @@
 
 #include "photon/PhotonPoseEstimator.h"
 
+#include <hal/FRCUsageReporting.h>
+
 #include <cmath>
 #include <iostream>
 #include <limits>
@@ -68,7 +70,11 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
       m_robotToCamera(robotToCamera),
       lastPose(frc::Pose3d()),
       referencePose(frc::Pose3d()),
-      poseCacheTimestamp(-1_s) {}
+      poseCacheTimestamp(-1_s) {
+  HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator,
+             InstanceCount);
+  InstanceCount++;
+}
 
 PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
                                          PoseStrategy strat, PhotonCamera&& cam,
@@ -79,7 +85,11 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
       m_robotToCamera(robotToCamera),
       lastPose(frc::Pose3d()),
       referencePose(frc::Pose3d()),
-      poseCacheTimestamp(-1_s) {}
+      poseCacheTimestamp(-1_s) {
+  HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator,
+             InstanceCount);
+  InstanceCount++;
+}
 
 void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
   if (strategy == MULTI_TAG_PNP_ON_COPROCESSOR ||
diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h
index 2a1418f1b5..99aa52c61b 100644
--- a/photon-lib/src/main/native/include/photon/PhotonCamera.h
+++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h
@@ -39,11 +39,7 @@
 #include <units/time.h>
 #include <wpi/deprecated.h>
 
-#include "photon/dataflow/structures/Packet.h"
-#include "photon/targeting/MultiTargetPNPResult.h"
-#include "photon/targeting/PNPResult.h"
-#include "photon/targeting/PhotonPipelineResult.h"
-#include "photon/targeting/PhotonTrackedTarget.h"
+#include "photon/targeting//PhotonPipelineResult.h"
 
 namespace cv {
 class Mat;
@@ -172,6 +168,8 @@ class PhotonCamera {
     PhotonCamera::VERSION_CHECK_ENABLED = enabled;
   }
 
+  std::shared_ptr<nt::NetworkTable> GetCameraTable() const { return rootTable; }
+
   // For use in tests
   bool test = false;
   PhotonPipelineResult testResult;
@@ -207,6 +205,7 @@ class PhotonCamera {
  private:
   units::second_t lastVersionCheckTime = 0_s;
   inline static bool VERSION_CHECK_ENABLED = true;
+  inline static int InstanceCount = 0;
 
   void VerifyVersion();
 };
diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
index cdb60f6cee..2c863330bb 100644
--- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
+++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
@@ -231,6 +231,8 @@ class PhotonPoseEstimator {
 
   units::second_t poseCacheTimestamp;
 
+  inline static int InstanceCount = 0;
+
   inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; }
 
   std::optional<EstimatedRobotPose> Update(
diff --git a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h
new file mode 100644
index 0000000000..367b30bb8f
--- /dev/null
+++ b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h
@@ -0,0 +1,449 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <cameraserver/CameraServer.h>
+#include <photon/PhotonCamera.h>
+#include <photon/PhotonTargetSortMode.h>
+#include <photon/estimation/CameraTargetRelation.h>
+#include <photon/estimation/VisionEstimation.h>
+#include <photon/networktables/NTTopicSet.h>
+#include <photon/simulation/SimCameraProperties.h>
+#include <photon/simulation/VideoSimUtil.h>
+#include <photon/simulation/VisionTargetSim.h>
+
+#include <algorithm>
+#include <limits>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/apriltag/AprilTagFields.h>
+#include <units/math.h>
+#include <wpi/timestamp.h>
+
+namespace photon {
+class PhotonCameraSim {
+ public:
+  explicit PhotonCameraSim(PhotonCamera* camera)
+      : PhotonCameraSim(camera, photon::SimCameraProperties::PERFECT_90DEG()) {}
+  PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props)
+      : prop(props), cam(camera) {
+    SetMinTargetAreaPixels(kDefaultMinAreaPx);
+    videoSimRaw = frc::CameraServer::PutVideo(
+        std::string{camera->GetCameraName()} + "-raw", prop.GetResWidth(),
+        prop.GetResHeight());
+    videoSimRaw.SetPixelFormat(cs::VideoMode::PixelFormat::kGray);
+    videoSimProcessed = frc::CameraServer::PutVideo(
+        std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(),
+        prop.GetResHeight());
+    ts.subTable = cam->GetCameraTable();
+    ts.UpdateEntries();
+  }
+  PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props,
+                  double minTargetAreaPercent, units::meter_t maxSightRange)
+      : PhotonCameraSim(camera, props) {
+    this->minTargetAreaPercent = minTargetAreaPercent;
+    this->maxSightRange = maxSightRange;
+  }
+  PhotonCamera* GetCamera() { return cam; }
+  double GetMinTargetAreaPercent() { return minTargetAreaPercent; }
+  double GetMinTargetAreaPixels() {
+    return minTargetAreaPercent / 100.0 * prop.GetResArea();
+  }
+  units::meter_t GetMaxSightRange() { return maxSightRange; }
+  const cs::CvSource& GetVideoSimRaw() { return videoSimRaw; }
+  const cv::Mat& GetVideoSimFrameRaw() { return videoSimFrameRaw; }
+  bool CanSeeTargetPose(const frc::Pose3d& camPose,
+                        const VisionTargetSim& target) {
+    CameraTargetRelation rel{camPose, target.GetPose()};
+    return ((units::math::abs(rel.camToTargYaw.Degrees()) <
+             prop.GetHorizFOV().Degrees() / 2.0) &&
+            (units::math::abs(rel.camToTargPitch.Degrees()) <
+             prop.GetVertFOV().Degrees() / 2.0) &&
+            (!target.GetModel().GetIsPlanar() ||
+             units::math::abs(rel.targToCamAngle.Degrees()) < 90_deg) &&
+            (rel.camToTarg.Translation().Norm() <= maxSightRange));
+  }
+  bool CanSeeCorner(const std::vector<cv::Point2f>& points) {
+    for (const auto& pt : points) {
+      if (std::clamp<float>(pt.x, 0, prop.GetResWidth()) != pt.x ||
+          std::clamp<float>(pt.y, 0, prop.GetResHeight()) != pt.y) {
+        return false;
+      }
+    }
+    return true;
+  }
+  std::optional<uint64_t> ConsumeNextEntryTime() {
+    uint64_t now = wpi::Now();
+    uint64_t timestamp{};
+    int iter = 0;
+    while (now >= nextNTEntryTime) {
+      timestamp = nextNTEntryTime;
+      uint64_t frameTime = prop.EstSecUntilNextFrame()
+                               .convert<units::microseconds>()
+                               .to<uint64_t>();
+      nextNTEntryTime += frameTime;
+
+      if (iter++ > 50) {
+        timestamp = now;
+        nextNTEntryTime = now + frameTime;
+        break;
+      }
+    }
+
+    if (timestamp != 0) {
+      return timestamp;
+    } else {
+      return std::nullopt;
+    }
+  }
+  void SetMinTargetAreaPercent(double areaPercent) {
+    minTargetAreaPercent = areaPercent;
+  }
+  void SetMinTargetAreaPixels(double areaPx) {
+    minTargetAreaPercent = areaPx / prop.GetResArea() * 100;
+  }
+  void SetMaxSightRange(units::meter_t range) { maxSightRange = range; }
+  void EnableRawStream(bool enabled) { videoSimRawEnabled = enabled; }
+  void EnableDrawWireframe(bool enabled) { videoSimWireframeEnabled = enabled; }
+  void SetWireframeResolution(double resolution) {
+    videoSimWireframeResolution = resolution;
+  }
+  void EnabledProcessedStream(double enabled) { videoSimProcEnabled = enabled; }
+  PhotonPipelineResult Process(units::second_t latency,
+                               const frc::Pose3d& cameraPose,
+                               std::vector<VisionTargetSim> targets) {
+    std::sort(
+        targets.begin(), targets.end(),
+        [cameraPose](const VisionTargetSim& t1, const VisionTargetSim& t2) {
+          units::meter_t dist1 =
+              t1.GetPose().Translation().Distance(cameraPose.Translation());
+          units::meter_t dist2 =
+              t2.GetPose().Translation().Distance(cameraPose.Translation());
+          return dist1 > dist2;
+        });
+
+    std::vector<std::pair<VisionTargetSim, std::vector<cv::Point2f>>>
+        visibleTgts{};
+    std::vector<PhotonTrackedTarget> detectableTgts{};
+    RotTrlTransform3d camRt = RotTrlTransform3d::MakeRelativeTo(cameraPose);
+
+    VideoSimUtil::UpdateVideoProp(videoSimRaw, prop);
+    VideoSimUtil::UpdateVideoProp(videoSimProcessed, prop);
+    cv::Size videoFrameSize{prop.GetResWidth(), prop.GetResHeight()};
+    cv::Mat blankFrame = cv::Mat::zeros(videoFrameSize, CV_8UC1);
+    blankFrame.assignTo(videoSimFrameRaw);
+
+    for (const auto& tgt : targets) {
+      if (!CanSeeTargetPose(cameraPose, tgt)) {
+        continue;
+      }
+
+      std::vector<frc::Translation3d> fieldCorners = tgt.GetFieldVertices();
+      if (tgt.GetModel().GetIsSpherical()) {
+        TargetModel model = tgt.GetModel();
+        fieldCorners = model.GetFieldVertices(TargetModel::GetOrientedPose(
+            tgt.GetPose().Translation(), cameraPose.Translation()));
+      }
+
+      std::vector<cv::Point2f> imagePoints = OpenCVHelp::ProjectPoints(
+          prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, fieldCorners);
+      if (tgt.GetModel().GetIsSpherical()) {
+        cv::Point2d center = OpenCVHelp::AvgPoint(imagePoints);
+        int l = 0;
+        int t = 0;
+        int b = 0;
+        int r = 0;
+        for (int i = 0; i < 4; i++) {
+          if (imagePoints[i].x < imagePoints[l].x) {
+            l = i;
+          }
+        }
+        cv::Point2d lc = imagePoints[l];
+        std::array<double, 4> angles{};
+        t = (l + 1) % 4;
+        b = (l + 1) % 4;
+        for (int i = 0; i < 4; i++) {
+          if (i == l) {
+            continue;
+          }
+          cv::Point2d ic = imagePoints[i];
+          angles[i] = std::atan2(lc.y - ic.y, ic.x - lc.x);
+          if (angles[i] >= angles[t]) {
+            t = i;
+          }
+          if (angles[i] <= angles[b]) {
+            b = i;
+          }
+        }
+        for (int i = 0; i < 4; i++) {
+          if (i != t && i != l && i != b) {
+            r = i;
+          }
+        }
+        cv::RotatedRect rect{
+            cv::Point2d{center.x, center.y},
+            cv::Size2d{imagePoints[r].x - lc.x,
+                       imagePoints[b].y - imagePoints[t].y},
+            units::radian_t{-angles[r]}.convert<units::degrees>().to<float>()};
+        std::vector<cv::Point2f> points{};
+        rect.points(points);
+
+        // Can't find an easier way to convert from Point2f to Point2d
+        imagePoints.clear();
+        std::transform(points.begin(), points.end(),
+                       std::back_inserter(imagePoints),
+                       [](const cv::Point2f& p) { return (cv::Point2d)p; });
+      }
+
+      visibleTgts.emplace_back(std::make_pair(tgt, imagePoints));
+      std::vector<cv::Point2f> noisyTargetCorners =
+          prop.EstPixelNoise(imagePoints);
+      cv::RotatedRect minAreaRect =
+          OpenCVHelp::GetMinAreaRect(noisyTargetCorners);
+      std::vector<cv::Point2f> minAreaRectPts;
+      minAreaRectPts.reserve(4);
+      minAreaRect.points(minAreaRectPts);
+      cv::Point2d centerPt = minAreaRect.center;
+      frc::Rotation3d centerRot = prop.GetPixelRot(centerPt);
+      double areaPercent = prop.GetContourAreaPercent(noisyTargetCorners);
+
+      if (!(CanSeeCorner(noisyTargetCorners) &&
+            areaPercent >= minTargetAreaPercent)) {
+        continue;
+      }
+
+      PNPResult pnpSim{};
+      if (tgt.fiducialId >= 0 && tgt.GetFieldVertices().size() == 4) {
+        pnpSim = OpenCVHelp::SolvePNP_Square(
+            prop.GetIntrinsics(), prop.GetDistCoeffs(),
+            tgt.GetModel().GetVertices(), noisyTargetCorners);
+      }
+
+      std::vector<std::pair<float, float>> tempCorners =
+          OpenCVHelp::PointsToCorners(minAreaRectPts);
+      wpi::SmallVector<std::pair<double, double>, 4> smallVec;
+
+      for (const auto& corner : tempCorners) {
+        smallVec.emplace_back(
+            std::make_pair(static_cast<double>(corner.first),
+                           static_cast<double>(corner.second)));
+      }
+
+      std::vector<std::pair<float, float>> cornersFloat =
+          OpenCVHelp::PointsToCorners(noisyTargetCorners);
+
+      std::vector<std::pair<double, double>> cornersDouble{cornersFloat.begin(),
+                                                           cornersFloat.end()};
+      detectableTgts.emplace_back(PhotonTrackedTarget{
+          centerRot.Z().convert<units::degrees>().to<double>(),
+          -centerRot.Y().convert<units::degrees>().to<double>(), areaPercent,
+          centerRot.X().convert<units::degrees>().to<double>(), tgt.fiducialId,
+          pnpSim.best, pnpSim.alt, pnpSim.ambiguity, smallVec, cornersDouble});
+    }
+
+    if (videoSimRawEnabled) {
+      if (videoSimWireframeEnabled) {
+        VideoSimUtil::DrawFieldWireFrame(
+            camRt, prop, videoSimWireframeResolution, 1.5, cv::Scalar{80}, 6, 1,
+            cv::Scalar{30}, videoSimFrameRaw);
+      }
+
+      for (const auto& pair : visibleTgts) {
+        VisionTargetSim tgt = pair.first;
+        std::vector<cv::Point2f> corners = pair.second;
+
+        if (tgt.fiducialId > 0) {
+          VideoSimUtil::Warp165h5TagImage(tgt.fiducialId, corners, true,
+                                          videoSimFrameRaw);
+        } else if (!tgt.GetModel().GetIsSpherical()) {
+          std::vector<cv::Point2f> contour = corners;
+          if (!tgt.GetModel().GetIsPlanar()) {
+            contour = OpenCVHelp::GetConvexHull(contour);
+          }
+          VideoSimUtil::DrawPoly(contour, -1, cv::Scalar{255}, true,
+                                 videoSimFrameRaw);
+        } else {
+          VideoSimUtil::DrawInscribedEllipse(corners, cv::Scalar{255},
+                                             videoSimFrameRaw);
+        }
+      }
+      videoSimRaw.PutFrame(videoSimFrameRaw);
+    } else {
+      videoSimRaw.SetConnectionStrategy(
+          cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
+    }
+
+    if (videoSimProcEnabled) {
+      cv::cvtColor(videoSimFrameRaw, videoSimFrameProcessed,
+                   cv::COLOR_GRAY2BGR);
+      cv::drawMarker(
+          videoSimFrameProcessed,
+          cv::Point2d{prop.GetResWidth() / 2.0, prop.GetResHeight() / 2.0},
+          cv::Scalar{0, 255, 0}, cv::MARKER_CROSS,
+          static_cast<int>(
+              VideoSimUtil::GetScaledThickness(15, videoSimFrameProcessed)),
+          static_cast<int>(
+              VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)),
+          cv::LINE_AA);
+      for (const auto& tgt : detectableTgts) {
+        auto detectedCornersDouble = tgt.GetDetectedCorners();
+        std::vector<std::pair<float, float>> detectedCornerFloat{
+            detectedCornersDouble.begin(), detectedCornersDouble.end()};
+        if (tgt.GetFiducialId() >= 0) {
+          VideoSimUtil::DrawTagDetection(
+              tgt.GetFiducialId(),
+              OpenCVHelp::CornersToPoints(detectedCornerFloat),
+              videoSimFrameProcessed);
+        } else {
+          cv::rectangle(videoSimFrameProcessed,
+                        OpenCVHelp::GetBoundingRect(
+                            OpenCVHelp::CornersToPoints(detectedCornerFloat)),
+                        cv::Scalar{0, 0, 255},
+                        static_cast<int>(VideoSimUtil::GetScaledThickness(
+                            1, videoSimFrameProcessed)),
+                        cv::LINE_AA);
+
+          wpi::SmallVector<std::pair<double, double>, 4> smallVec =
+              tgt.GetMinAreaRectCorners();
+
+          std::vector<std::pair<float, float>> cornersCopy{};
+          cornersCopy.reserve(4);
+
+          for (const auto& corner : smallVec) {
+            cornersCopy.emplace_back(
+                std::make_pair(static_cast<float>(corner.first),
+                               static_cast<float>(corner.second)));
+          }
+
+          VideoSimUtil::DrawPoly(
+              OpenCVHelp::CornersToPoints(cornersCopy),
+              static_cast<int>(
+                  VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)),
+              cv::Scalar{255, 30, 30}, true, videoSimFrameProcessed);
+        }
+      }
+      videoSimProcessed.PutFrame(videoSimFrameProcessed);
+    } else {
+      videoSimProcessed.SetConnectionStrategy(
+          cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
+    }
+
+    MultiTargetPNPResult multiTagResults{};
+
+    std::vector<frc::AprilTag> visibleLayoutTags =
+        VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout);
+    if (visibleLayoutTags.size() > 1) {
+      wpi::SmallVector<int16_t, 32> usedIds{};
+      std::transform(visibleLayoutTags.begin(), visibleLayoutTags.end(),
+                     usedIds.begin(),
+                     [](const frc::AprilTag& tag) { return tag.ID; });
+      std::sort(usedIds.begin(), usedIds.end());
+      PNPResult pnpResult = VisionEstimation::EstimateCamPosePNP(
+          prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout,
+          kAprilTag16h5);
+      multiTagResults = MultiTargetPNPResult{pnpResult, usedIds};
+    }
+
+    return PhotonPipelineResult{latency, detectableTgts, multiTagResults};
+  }
+  void SubmitProcessedFrame(const PhotonPipelineResult& result) {
+    SubmitProcessedFrame(result, wpi::Now());
+  }
+  void SubmitProcessedFrame(const PhotonPipelineResult& result,
+                            uint64_t recieveTimestamp) {
+    ts.latencyMillisEntry.Set(
+        result.GetLatency().convert<units::milliseconds>().to<double>(),
+        recieveTimestamp);
+
+    Packet newPacket{};
+    newPacket << result;
+    ts.rawBytesEntry.Set(newPacket.GetData(), recieveTimestamp);
+
+    bool hasTargets = result.HasTargets();
+    ts.hasTargetEntry.Set(hasTargets, recieveTimestamp);
+    if (!hasTargets) {
+      ts.targetPitchEntry.Set(0.0, recieveTimestamp);
+      ts.targetYawEntry.Set(0.0, recieveTimestamp);
+      ts.targetAreaEntry.Set(0.0, recieveTimestamp);
+      std::array<double, 3> poseData{0.0, 0.0, 0.0};
+      ts.targetPoseEntry.Set(poseData, recieveTimestamp);
+      ts.targetSkewEntry.Set(0.0, recieveTimestamp);
+    } else {
+      PhotonTrackedTarget bestTarget = result.GetBestTarget();
+
+      ts.targetPitchEntry.Set(bestTarget.GetPitch(), recieveTimestamp);
+      ts.targetYawEntry.Set(bestTarget.GetYaw(), recieveTimestamp);
+      ts.targetAreaEntry.Set(bestTarget.GetArea(), recieveTimestamp);
+      ts.targetSkewEntry.Set(bestTarget.GetSkew(), recieveTimestamp);
+
+      frc::Transform3d transform = bestTarget.GetBestCameraToTarget();
+      std::array<double, 4> poseData{
+          transform.X().to<double>(), transform.Y().to<double>(),
+          transform.Rotation().ToRotation2d().Degrees().to<double>()};
+      ts.targetPoseEntry.Set(poseData, recieveTimestamp);
+    }
+
+    auto intrinsics = prop.GetIntrinsics();
+    std::vector<double> intrinsicsView{intrinsics.data(),
+                                       intrinsics.data() + intrinsics.size()};
+    ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp);
+
+    auto distortion = prop.GetDistCoeffs();
+    std::vector<double> distortionView{distortion.data(),
+                                       distortion.data() + distortion.size()};
+    ts.cameraDistortionPublisher.Set(distortionView, recieveTimestamp);
+
+    ts.heartbeatPublisher.Set(heartbeatCounter++, recieveTimestamp);
+  }
+  SimCameraProperties prop;
+
+ private:
+  PhotonCamera* cam;
+
+  NTTopicSet ts{};
+  uint64_t heartbeatCounter{0};
+
+  uint64_t nextNTEntryTime{wpi::Now()};
+
+  units::meter_t maxSightRange{std::numeric_limits<double>::max()};
+  static constexpr double kDefaultMinAreaPx{100};
+  double minTargetAreaPercent;
+
+  frc::AprilTagFieldLayout tagLayout{
+      frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp)};
+
+  cs::CvSource videoSimRaw;
+  cv::Mat videoSimFrameRaw{};
+  bool videoSimRawEnabled{true};
+  bool videoSimWireframeEnabled{false};
+  double videoSimWireframeResolution{0.1};
+  cs::CvSource videoSimProcessed;
+  cv::Mat videoSimFrameProcessed{};
+  bool videoSimProcEnabled{true};
+};
+}  // namespace photon
diff --git a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h
new file mode 100644
index 0000000000..f3a5d73677
--- /dev/null
+++ b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h
@@ -0,0 +1,475 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <photon/estimation/OpenCVHelp.h>
+
+#include <algorithm>
+#include <random>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <Eigen/Core>
+#include <frc/Errors.h>
+#include <frc/MathUtil.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation3d.h>
+#include <units/frequency.h>
+#include <units/time.h>
+
+namespace photon {
+class SimCameraProperties {
+ public:
+  SimCameraProperties() { SetCalibration(960, 720, frc::Rotation2d{90_deg}); }
+  SimCameraProperties(std::string path, int width, int height) {}
+  void SetCalibration(int width, int height, frc::Rotation2d fovDiag) {
+    if (fovDiag.Degrees() < 1_deg || fovDiag.Degrees() > 179_deg) {
+      fovDiag = frc::Rotation2d{
+          std::clamp<units::degree_t>(fovDiag.Degrees(), 1_deg, 179_deg)};
+      FRC_ReportError(
+          frc::err::Error,
+          "Requested invalid FOV! Clamping between (1, 179) degrees...");
+    }
+    double resDiag = std::sqrt(width * width + height * height);
+    double diagRatio = units::math::tan(fovDiag.Radians() / 2.0);
+    frc::Rotation2d fovWidth{
+        units::radian_t{std::atan(diagRatio * (width / resDiag)) * 2}};
+    frc::Rotation2d fovHeight{
+        units::radian_t{std::atan(diagRatio * (height / resDiag)) * 2}};
+
+    Eigen::Matrix<double, 5, 1> newDistCoeffs;
+    newDistCoeffs << 0, 0, 0, 0, 0;
+
+    double cx = width / 2.0 - 0.5;
+    double cy = height / 2.0 - 0.5;
+
+    double fx = cx / units::math::tan(fovWidth.Radians() / 2.0);
+    double fy = cy / units::math::tan(fovHeight.Radians() / 2.0);
+
+    Eigen::Matrix<double, 3, 3> newCamIntrinsics;
+    newCamIntrinsics << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0;
+    SetCalibration(width, height, newCamIntrinsics, newDistCoeffs);
+  }
+
+  void SetCalibration(int width, int height,
+                      const Eigen::Matrix<double, 3, 3>& newCamIntrinsics,
+                      const Eigen::Matrix<double, 5, 1>& newDistCoeffs) {
+    resWidth = width;
+    resHeight = height;
+    camIntrinsics = newCamIntrinsics;
+    distCoeffs = newDistCoeffs;
+
+    std::array<frc::Translation3d, 4> p{
+        frc::Translation3d{
+            1_m,
+            frc::Rotation3d{0_rad, 0_rad,
+                            (GetPixelYaw(0) + frc::Rotation2d{units::radian_t{
+                                                  -std::numbers::pi / 2.0}})
+                                .Radians()}},
+        frc::Translation3d{
+            1_m, frc::Rotation3d{0_rad, 0_rad,
+                                 (GetPixelYaw(width) +
+                                  frc::Rotation2d{
+                                      units::radian_t{std::numbers::pi / 2.0}})
+                                     .Radians()}},
+        frc::Translation3d{
+            1_m, frc::Rotation3d{0_rad,
+                                 (GetPixelPitch(0) +
+                                  frc::Rotation2d{
+                                      units::radian_t{std::numbers::pi / 2.0}})
+                                     .Radians(),
+                                 0_rad}},
+        frc::Translation3d{
+            1_m, frc::Rotation3d{0_rad,
+                                 (GetPixelPitch(height) +
+                                  frc::Rotation2d{
+                                      units::radian_t{-std::numbers::pi / 2.0}})
+                                     .Radians(),
+                                 0_rad}},
+    };
+    viewplanes.clear();
+    for (size_t i = 0; i < p.size(); i++) {
+      viewplanes.emplace_back(Eigen::Matrix<double, 3, 1>{
+          p[i].X().to<double>(), p[i].Y().to<double>(), p[i].Z().to<double>()});
+    }
+  }
+
+  void SetCalibError(double newAvgErrorPx, double newErrorStdDevPx) {
+    avgErrorPx = newAvgErrorPx;
+    errorStdDevPx = newErrorStdDevPx;
+  }
+
+  void SetFPS(units::hertz_t fps) {
+    frameSpeed = units::math::max(1 / fps, exposureTime);
+  }
+
+  void SetExposureTime(units::second_t newExposureTime) {
+    exposureTime = newExposureTime;
+    frameSpeed = units::math::max(frameSpeed, exposureTime);
+  }
+
+  void SetAvgLatency(units::second_t newAvgLatency) {
+    avgLatency = newAvgLatency;
+  }
+
+  void SetLatencyStdDev(units::second_t newLatencyStdDev) {
+    latencyStdDev = newLatencyStdDev;
+  }
+
+  int GetResWidth() const { return resWidth; }
+
+  int GetResHeight() const { return resHeight; }
+
+  int GetResArea() const { return resWidth * resHeight; }
+
+  double GetAspectRatio() const {
+    return static_cast<double>(resWidth) / static_cast<double>(resHeight);
+  }
+
+  Eigen::Matrix<double, 3, 3> GetIntrinsics() const { return camIntrinsics; }
+
+  Eigen::Matrix<double, 5, 1> GetDistCoeffs() const { return distCoeffs; }
+
+  units::hertz_t GetFPS() const { return 1 / frameSpeed; }
+
+  units::second_t GetFrameSpeed() const { return frameSpeed; }
+
+  units::second_t GetExposureTime() const { return exposureTime; }
+
+  units::second_t GetAverageLatency() const { return avgLatency; }
+
+  units::second_t GetLatencyStdDev() const { return latencyStdDev; }
+
+  double GetContourAreaPercent(const std::vector<cv::Point2f>& points) {
+    return cv::contourArea(photon::OpenCVHelp::GetConvexHull(points)) /
+           GetResArea() * 100;
+  }
+
+  frc::Rotation2d GetPixelYaw(double pixelX) const {
+    double fx = camIntrinsics(0, 0);
+    double cx = camIntrinsics(0, 2);
+    double xOffset = cx - pixelX;
+    return frc::Rotation2d{fx, xOffset};
+  }
+
+  frc::Rotation2d GetPixelPitch(double pixelY) const {
+    double fy = camIntrinsics(1, 1);
+    double cy = camIntrinsics(1, 2);
+    double yOffset = cy - pixelY;
+    return frc::Rotation2d{fy, -yOffset};
+  }
+
+  frc::Rotation3d GetPixelRot(const cv::Point2d& point) const {
+    return frc::Rotation3d{0_rad, GetPixelPitch(point.y).Radians(),
+                           GetPixelYaw(point.x).Radians()};
+  }
+
+  frc::Rotation3d GetCorrectedPixelRot(const cv::Point2d& point) const {
+    double fx = camIntrinsics(0, 0);
+    double cx = camIntrinsics(0, 2);
+    double xOffset = cx - point.x;
+
+    double fy = camIntrinsics(1, 1);
+    double cy = camIntrinsics(1, 2);
+    double yOffset = cy - point.y;
+
+    frc::Rotation2d yaw{fx, xOffset};
+    frc::Rotation2d pitch{fy / std::cos(std::atan(xOffset / fx)), -yOffset};
+    return frc::Rotation3d{0_rad, pitch.Radians(), yaw.Radians()};
+  }
+
+  frc::Rotation2d GetHorizFOV() const {
+    frc::Rotation2d left = GetPixelYaw(0);
+    frc::Rotation2d right = GetPixelYaw(resWidth);
+    return left - right;
+  }
+
+  frc::Rotation2d GetVertFOV() const {
+    frc::Rotation2d above = GetPixelPitch(0);
+    frc::Rotation2d below = GetPixelPitch(resHeight);
+    return below - above;
+  }
+
+  frc::Rotation2d GetDiagFOV() const {
+    return frc::Rotation2d{
+        units::math::hypot(GetHorizFOV().Radians(), GetVertFOV().Radians())};
+  }
+
+  std::pair<std::optional<double>, std::optional<double>> GetVisibleLine(
+      const RotTrlTransform3d& camRt, const frc::Translation3d& a,
+      const frc::Translation3d& b) const {
+    frc::Translation3d relA = camRt.Apply(a);
+    frc::Translation3d relB = camRt.Apply(b);
+
+    if (relA.X() <= 0_m && relB.X() <= 0_m) {
+      return {std::nullopt, std::nullopt};
+    }
+
+    Eigen::Matrix<double, 3, 1> av{relA.X().to<double>(), relA.Y().to<double>(),
+                                   relA.Z().to<double>()};
+    Eigen::Matrix<double, 3, 1> bv{relB.X().to<double>(), relB.Y().to<double>(),
+                                   relB.Z().to<double>()};
+    Eigen::Matrix<double, 3, 1> abv = bv - av;
+
+    bool aVisible = true;
+    bool bVisible = true;
+    for (size_t i = 0; i < viewplanes.size(); i++) {
+      Eigen::Matrix<double, 3, 1> normal = viewplanes[i];
+      double aVisibility = av.dot(normal);
+      if (aVisibility < 0) {
+        aVisible = false;
+      }
+      double bVisibility = bv.dot(normal);
+      if (bVisibility < 0) {
+        bVisible = false;
+      }
+      if (aVisibility <= 0 && bVisibility <= 0) {
+        return {std::nullopt, std::nullopt};
+      }
+    }
+
+    if (aVisible && bVisible) {
+      return {0, 1};
+    }
+
+    std::array<double, 4> intersections{std::nan(""), std::nan(""),
+                                        std::nan(""), std::nan("")};
+    std::vector<std::optional<Eigen::Matrix<double, 3, 1>>> ipts{
+        {std::nullopt, std::nullopt, std::nullopt, std::nullopt}};
+
+    for (size_t i = 0; i < viewplanes.size(); i++) {
+      Eigen::Matrix<double, 3, 1> normal = viewplanes[i];
+      Eigen::Matrix<double, 3, 1> a_projn{};
+      a_projn = (av.dot(normal) / normal.dot(normal)) * normal;
+
+      if (std::abs(abv.dot(normal)) < 1e-5) {
+        continue;
+      }
+      intersections[i] = a_projn.dot(a_projn) / -(abv.dot(a_projn));
+
+      Eigen::Matrix<double, 3, 1> apv{};
+      apv = intersections[i] * abv;
+      Eigen::Matrix<double, 3, 1> intersectpt{};
+      intersectpt = av + apv;
+      ipts[i] = intersectpt;
+
+      for (size_t j = 1; j < viewplanes.size(); j++) {
+        int oi = (i + j) % viewplanes.size();
+        Eigen::Matrix<double, 3, 1> onormal = viewplanes[oi];
+        if (intersectpt.dot(onormal) < 0) {
+          intersections[i] = std::nan("");
+          ipts[i] = std::nullopt;
+          break;
+        }
+      }
+
+      if (!ipts[i]) {
+        continue;
+      }
+
+      for (int j = i - 1; j >= 0; j--) {
+        std::optional<Eigen::Matrix<double, 3, 1>> oipt = ipts[j];
+        if (!oipt) {
+          continue;
+        }
+        Eigen::Matrix<double, 3, 1> diff{};
+        diff = oipt.value() - intersectpt;
+        if (diff.cwiseAbs().maxCoeff() < 1e-4) {
+          intersections[i] = std::nan("");
+          ipts[i] = std::nullopt;
+          break;
+        }
+      }
+    }
+
+    double inter1 = std::nan("");
+    double inter2 = std::nan("");
+    for (double inter : intersections) {
+      if (!std::isnan(inter)) {
+        if (std::isnan(inter1)) {
+          inter1 = inter;
+        } else {
+          inter2 = inter;
+        }
+      }
+    }
+
+    if (!std::isnan(inter2)) {
+      double max = std::max(inter1, inter2);
+      double min = std::min(inter1, inter2);
+      if (aVisible) {
+        min = 0;
+      }
+      if (bVisible) {
+        max = 1;
+      }
+      return {min, max};
+    } else if (!std::isnan(inter1)) {
+      if (aVisible) {
+        return {0, inter1};
+      }
+      if (bVisible) {
+        return {inter1, 1};
+      }
+      return {inter1, std::nullopt};
+    } else {
+      return {std::nullopt, std::nullopt};
+    }
+  }
+
+  std::vector<cv::Point2f> EstPixelNoise(
+      const std::vector<cv::Point2f>& points) {
+    if (avgErrorPx == 0 && errorStdDevPx == 0) {
+      return points;
+    }
+
+    std::vector<cv::Point2f> noisyPts;
+    noisyPts.reserve(points.size());
+    for (size_t i = 0; i < points.size(); i++) {
+      cv::Point2f p = points[i];
+      float error = avgErrorPx + gaussian(generator) * errorStdDevPx;
+      float errorAngle =
+          uniform(generator) * 2 * std::numbers::pi - std::numbers::pi;
+      noisyPts.emplace_back(cv::Point2f{p.x + error * std::cos(errorAngle),
+                                        p.y + error * std::sin(errorAngle)});
+    }
+    return noisyPts;
+  }
+
+  units::second_t EstLatency() {
+    return units::math::max(avgLatency + gaussian(generator) * latencyStdDev,
+                            0_s);
+  }
+
+  units::second_t EstSecUntilNextFrame() {
+    return frameSpeed + units::math::max(0_s, EstLatency() - frameSpeed);
+  }
+
+  static SimCameraProperties PERFECT_90DEG() { return SimCameraProperties{}; }
+
+  static SimCameraProperties PI4_LIFECAM_320_240() {
+    SimCameraProperties prop{};
+    prop.SetCalibration(
+        320, 240,
+        (Eigen::MatrixXd(3, 3) << 328.2733242048587, 0.0, 164.8190261141906,
+         0.0, 318.0609794305216, 123.8633838438093, 0.0, 0.0, 1.0)
+            .finished(),
+        Eigen::Matrix<double, 5, 1>{
+            0.09957946553445934, -0.9166265114485799, 0.0019519890627236526,
+            -0.0036071725380870333, 1.5627234622420942});
+    prop.SetCalibError(0.21, 0.0124);
+    prop.SetFPS(30_Hz);
+    prop.SetAvgLatency(30_ms);
+    prop.SetLatencyStdDev(10_ms);
+    return prop;
+  }
+
+  static SimCameraProperties PI4_LIFECAM_640_480() {
+    SimCameraProperties prop{};
+    prop.SetCalibration(
+        640, 480,
+        (Eigen::MatrixXd(3, 3) << 669.1428078983059, 0.0, 322.53377249329213,
+         0.0, 646.9843137061716, 241.26567383784163, 0.0, 0.0, 1.0)
+            .finished(),
+        Eigen::Matrix<double, 5, 1>{
+            0.12788470750464645, -1.2350335805796528, 0.0024990767286192732,
+            -0.0026958287600230705, 2.2951386729115537});
+    prop.SetCalibError(0.26, 0.046);
+    prop.SetFPS(15_Hz);
+    prop.SetAvgLatency(65_ms);
+    prop.SetLatencyStdDev(15_ms);
+    return prop;
+  }
+
+  static SimCameraProperties LL2_640_480() {
+    SimCameraProperties prop{};
+    prop.SetCalibration(
+        640, 480,
+        (Eigen::MatrixXd(3, 3) << 511.22843367007755, 0.0, 323.62049380211096,
+         0.0, 514.5452336723849, 261.8827920543568, 0.0, 0.0, 1.0)
+            .finished(),
+        Eigen::Matrix<double, 5, 1>{0.1917469998873756, -0.5142936883324216,
+                                    0.012461562046896614, 0.0014084973492408186,
+                                    0.35160648971214437});
+    prop.SetCalibError(0.25, 0.05);
+    prop.SetFPS(15_Hz);
+    prop.SetAvgLatency(35_ms);
+    prop.SetLatencyStdDev(8_ms);
+    return prop;
+  }
+
+  static SimCameraProperties LL2_960_720() {
+    SimCameraProperties prop{};
+    prop.SetCalibration(
+        960, 720,
+        (Eigen::MatrixXd(3, 3) << 769.6873145148892, 0.0, 486.1096609458122,
+         0.0, 773.8164483705323, 384.66071662358354, 0.0, 0.0, 1.0)
+            .finished(),
+        Eigen::Matrix<double, 5, 1>{0.189462064814501, -0.49903003669627627,
+                                    0.007468423590519429, 0.002496885298683693,
+                                    0.3443122090208624});
+    prop.SetCalibError(0.35, 0.10);
+    prop.SetFPS(10_Hz);
+    prop.SetAvgLatency(50_ms);
+    prop.SetLatencyStdDev(15_ms);
+    return prop;
+  }
+
+  static SimCameraProperties LL2_1280_720() {
+    SimCameraProperties prop{};
+    prop.SetCalibration(
+        1280, 720,
+        (Eigen::MatrixXd(3, 3) << 1011.3749416937393, 0.0, 645.4955139388737,
+         0.0, 1008.5391755084075, 508.32877656020196, 0.0, 0.0, 1.0)
+            .finished(),
+        Eigen::Matrix<double, 5, 1>{0.13730101577061535, -0.2904345656989261,
+                                    8.32475714507539E-4, -3.694397782014239E-4,
+                                    0.09487962227027584});
+    prop.SetCalibError(0.37, 0.06);
+    prop.SetFPS(7_Hz);
+    prop.SetAvgLatency(60_ms);
+    prop.SetLatencyStdDev(20_ms);
+    return prop;
+  }
+
+ private:
+  std::mt19937 generator{std::random_device{}()};
+  std::normal_distribution<float> gaussian{0.0, 1.0};
+  std::uniform_real_distribution<float> uniform{0.0, 1.0};
+
+  int resWidth;
+  int resHeight;
+  Eigen::Matrix<double, 3, 3> camIntrinsics;
+  Eigen::Matrix<double, 5, 1> distCoeffs;
+  double avgErrorPx;
+  double errorStdDevPx;
+  units::second_t frameSpeed{0};
+  units::second_t exposureTime{0};
+  units::second_t avgLatency{0};
+  units::second_t latencyStdDev{0};
+  std::vector<Eigen::Matrix<double, 3, 1>> viewplanes{};
+};
+}  // namespace photon
diff --git a/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h
new file mode 100644
index 0000000000..bb2c453a88
--- /dev/null
+++ b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h
@@ -0,0 +1,432 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <cscore_cv.h>
+
+#include <algorithm>
+#include <numeric>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include <frc/apriltag/AprilTag.h>
+#include <opencv2/core.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/objdetect.hpp>
+#include <units/length.h>
+
+#include "SimCameraProperties.h"
+#include "photon/estimation/RotTrlTransform3d.h"
+
+namespace mathutil {
+template <typename T>
+int sgn(T val) {
+  return (T(0) < val) - (val < T(0));
+}
+}  // namespace mathutil
+
+namespace photon {
+namespace VideoSimUtil {
+static constexpr int kNumTags16h5 = 30;
+
+static constexpr units::meter_t fieldLength{16.54175_m};
+static constexpr units::meter_t fieldWidth{8.0137_m};
+
+static cv::Mat Get16h5TagImage(int id) {
+  wpi::RawFrame frame = frc::AprilTag::Generate16h5AprilTagImage(id);
+  cv::Mat markerImage{frame.height, frame.width, CV_8UC1, frame.data};
+  cv::Mat markerClone = markerImage.colRange(0, frame.dataLength).clone();
+  return markerClone;
+}
+
+static std::unordered_map<int, cv::Mat> LoadAprilTagImages() {
+  std::unordered_map<int, cv::Mat> retVal{};
+  for (int i = 0; i < kNumTags16h5; i++) {
+    cv::Mat tagImage = Get16h5TagImage(i);
+    retVal[i] = tagImage;
+  }
+  return retVal;
+}
+
+static std::vector<cv::Point2f> GetImageCorners(const cv::Size& size) {
+  std::vector<cv::Point2f> retVal{};
+  retVal.emplace_back(cv::Point2f{-0.5f, -0.5f});
+  retVal.emplace_back(cv::Point2f{size.width - 0.5f, -0.5f});
+  retVal.emplace_back(cv::Point2f{size.width - 0.5f, size.height - 0.5f});
+  retVal.emplace_back(cv::Point2f{-0.5f, size.height - 0.5f});
+  return retVal;
+}
+
+static std::vector<cv::Point2f> Get16h5MarkerPts(int scale) {
+  cv::Rect2f roi16h5{cv::Point2f{1, 1}, cv::Point2f{6, 6}};
+  roi16h5.x *= scale;
+  roi16h5.y *= scale;
+  roi16h5.width *= scale;
+  roi16h5.height *= scale;
+  std::vector<cv::Point2f> pts = GetImageCorners(roi16h5.size());
+  for (size_t i = 0; i < pts.size(); i++) {
+    cv::Point2f pt = pts[i];
+    pts[i] = cv::Point2f{roi16h5.tl().x + pt.x, roi16h5.tl().y + pt.y};
+  }
+  return pts;
+}
+
+static std::vector<cv::Point2f> Get16h5MarkerPts() {
+  return Get16h5MarkerPts(1);
+}
+
+static const std::unordered_map<int, cv::Mat> kTag16h5Images =
+    LoadAprilTagImages();
+static const std::vector<cv::Point2f> kTag16h5MarkPts = Get16h5MarkerPts();
+
+[[maybe_unused]] static void UpdateVideoProp(cs::CvSource& video,
+                                             const SimCameraProperties& prop) {
+  video.SetResolution(prop.GetResWidth(), prop.GetResHeight());
+  video.SetFPS(prop.GetFPS().to<int>());
+}
+
+[[maybe_unused]] static void Warp165h5TagImage(
+    int tagId, const std::vector<cv::Point2f>& dstPoints, bool antialiasing,
+    cv::Mat& destination) {
+  if (!kTag16h5Images.contains(tagId)) {
+    return;
+  }
+  cv::Mat tagImage = kTag16h5Images.at(tagId);
+  std::vector<cv::Point2f> tagPoints{kTag16h5MarkPts};
+  std::vector<cv::Point2f> tagImageCorners{GetImageCorners(tagImage.size())};
+  std::vector<cv::Point2f> dstPointMat = dstPoints;
+  cv::Rect boundingRect = cv::boundingRect(dstPointMat);
+  cv::Mat perspecTrf = cv::getPerspectiveTransform(tagPoints, dstPointMat);
+  std::vector<cv::Point2f> extremeCorners{};
+  cv::perspectiveTransform(tagImageCorners, extremeCorners, perspecTrf);
+  boundingRect = cv::boundingRect(extremeCorners);
+
+  double warpedContourArea = cv::contourArea(extremeCorners);
+  double warpedTagUpscale =
+      std::sqrt(warpedContourArea) / std::sqrt(tagImage.size().area());
+  int warpStrat = cv::INTER_NEAREST;
+
+  int supersampling = 6;
+  supersampling = static_cast<int>(std::ceil(supersampling / warpedTagUpscale));
+  supersampling = std::max(std::min(supersampling, 8), 1);
+
+  cv::Mat scaledTagImage{};
+  if (warpedTagUpscale > 2.0) {
+    warpStrat = cv::INTER_LINEAR;
+    int scaleFactor = static_cast<int>(warpedTagUpscale / 3.0) + 2;
+    scaleFactor = std::max(std::min(scaleFactor, 40), 1);
+    scaleFactor *= supersampling;
+    cv::resize(tagImage, scaledTagImage, cv::Size{}, scaleFactor, scaleFactor,
+               cv::INTER_NEAREST);
+    tagPoints = Get16h5MarkerPts(scaleFactor);
+  } else {
+    scaledTagImage = tagImage;
+  }
+
+  boundingRect.x -= 1;
+  boundingRect.y -= 1;
+  boundingRect.width += 2;
+  boundingRect.height += 2;
+  if (boundingRect.x < 0) {
+    boundingRect.width += boundingRect.x;
+    boundingRect.x = 0;
+  }
+  if (boundingRect.y < 0) {
+    boundingRect.height += boundingRect.y;
+    boundingRect.y = 0;
+  }
+  boundingRect.width =
+      std::min(destination.size().width - boundingRect.x, boundingRect.width);
+  boundingRect.height =
+      std::min(destination.size().height - boundingRect.y, boundingRect.height);
+  if (boundingRect.width <= 0 || boundingRect.height <= 0) {
+    return;
+  }
+
+  std::vector<cv::Point2f> scaledDstPts{};
+  if (supersampling > 1) {
+    cv::multiply(dstPointMat,
+                 cv::Scalar{static_cast<double>(supersampling),
+                            static_cast<double>(supersampling)},
+                 scaledDstPts);
+    boundingRect.x *= supersampling;
+    boundingRect.y *= supersampling;
+    boundingRect.width *= supersampling;
+    boundingRect.height *= supersampling;
+  } else {
+    scaledDstPts = dstPointMat;
+  }
+
+  cv::subtract(scaledDstPts,
+               cv::Scalar{static_cast<double>(boundingRect.tl().x),
+                          static_cast<double>(boundingRect.tl().y)},
+               scaledDstPts);
+  perspecTrf = cv::getPerspectiveTransform(tagPoints, scaledDstPts);
+
+  cv::Mat tempRoi{};
+  cv::warpPerspective(scaledTagImage, tempRoi, perspecTrf, boundingRect.size(),
+                      warpStrat);
+
+  if (supersampling > 1) {
+    boundingRect.x /= supersampling;
+    boundingRect.y /= supersampling;
+    boundingRect.width /= supersampling;
+    boundingRect.height /= supersampling;
+    cv::resize(tempRoi, tempRoi, boundingRect.size(), 0, 0, cv::INTER_AREA);
+  }
+
+  cv::Mat tempMask{cv::Mat::zeros(tempRoi.size(), CV_8UC1)};
+  cv::subtract(extremeCorners,
+               cv::Scalar{static_cast<float>(boundingRect.tl().x),
+                          static_cast<float>(boundingRect.tl().y)},
+               extremeCorners);
+  cv::Point2f tempCenter{};
+  tempCenter.x =
+      std::accumulate(extremeCorners.begin(), extremeCorners.end(), 0.0,
+                      [extremeCorners](float acc, const cv::Point2f& p2) {
+                        return acc + p2.x / extremeCorners.size();
+                      });
+  tempCenter.y =
+      std::accumulate(extremeCorners.begin(), extremeCorners.end(), 0.0,
+                      [extremeCorners](float acc, const cv::Point2f& p2) {
+                        return acc + p2.y / extremeCorners.size();
+                      });
+
+  for (auto& corner : extremeCorners) {
+    float xDiff = corner.x - tempCenter.x;
+    float yDiff = corner.y - tempCenter.y;
+    xDiff += 1 * mathutil::sgn(xDiff);
+    yDiff += 1 * mathutil::sgn(yDiff);
+    corner = cv::Point2f{tempCenter.x + xDiff, tempCenter.y + yDiff};
+  }
+
+  std::vector<cv::Point> extremeCornerInt{extremeCorners.begin(),
+                                          extremeCorners.end()};
+  cv::fillConvexPoly(tempMask, extremeCornerInt, cv::Scalar{255});
+
+  cv::copyTo(tempRoi, destination(boundingRect), tempMask);
+}
+
+static double GetScaledThickness(double thickness480p,
+                                 const cv::Mat& destinationMat) {
+  double scaleX = destinationMat.size().width / 640.0;
+  double scaleY = destinationMat.size().height / 480.0;
+  double minScale = std::min(scaleX, scaleY);
+  return std::max(thickness480p * minScale, 1.0);
+}
+
+[[maybe_unused]] static void DrawInscribedEllipse(
+    const std::vector<cv::Point2f>& dstPoints, const cv::Scalar& color,
+    cv::Mat& destination) {
+  cv::RotatedRect rect = OpenCVHelp::GetMinAreaRect(dstPoints);
+  cv::ellipse(destination, rect, color, -1, cv::LINE_AA);
+}
+
+static void DrawPoly(const std::vector<cv::Point2f>& dstPoints, int thickness,
+                     const cv::Scalar& color, bool isClosed,
+                     cv::Mat& destination) {
+  std::vector<cv::Point> intDstPoints{dstPoints.begin(), dstPoints.end()};
+  std::vector<std::vector<cv::Point>> listOfListOfPoints;
+  listOfListOfPoints.emplace_back(intDstPoints);
+  if (thickness > 0) {
+    cv::polylines(destination, listOfListOfPoints, isClosed, color, thickness,
+                  cv::LINE_AA);
+  } else {
+    cv::fillPoly(destination, listOfListOfPoints, color, cv::LINE_AA);
+  }
+}
+
+[[maybe_unused]] static void DrawTagDetection(
+    int id, const std::vector<cv::Point2f>& dstPoints, cv::Mat& destination) {
+  double thickness = GetScaledThickness(1, destination);
+  DrawPoly(dstPoints, static_cast<int>(thickness), cv::Scalar{0, 0, 255}, true,
+           destination);
+  cv::Rect2d rect{cv::boundingRect(dstPoints)};
+  cv::Point2d textPt{rect.x + rect.width, rect.y};
+  textPt.x += thickness;
+  textPt.y += thickness;
+  cv::putText(destination, std::to_string(id), textPt, cv::FONT_HERSHEY_PLAIN,
+              1.5 * thickness, cv::Scalar{0, 200, 0},
+              static_cast<int>(thickness), cv::LINE_AA);
+}
+
+static std::vector<std::vector<frc::Translation3d>> GetFieldWallLines() {
+  std::vector<std::vector<frc::Translation3d>> list;
+
+  const units::meter_t sideHt = 19.5_in;
+  const units::meter_t driveHt = 35_in;
+  const units::meter_t topHt = 78_in;
+
+  // field floor
+  list.emplace_back(std::vector<frc::Translation3d>{
+      frc::Translation3d{0_m, 0_m, 0_m},
+      frc::Translation3d{fieldLength, 0_m, 0_m},
+      frc::Translation3d{fieldLength, fieldWidth, 0_m},
+      frc::Translation3d{0_m, fieldWidth, 0_m},
+      frc::Translation3d{0_m, 0_m, 0_m}});
+
+  // right side wall
+  list.emplace_back(std::vector<frc::Translation3d>{
+      frc::Translation3d{0_m, 0_m, 0_m}, frc::Translation3d{0_m, 0_m, sideHt},
+      frc::Translation3d{fieldLength, 0_m, sideHt},
+      frc::Translation3d{fieldLength, 0_m, 0_m}});
+
+  // red driverstation
+  list.emplace_back(std::vector<frc::Translation3d>{
+      frc::Translation3d{fieldLength, 0_m, sideHt},
+      frc::Translation3d{fieldLength, 0_m, topHt},
+      frc::Translation3d{fieldLength, fieldWidth, topHt},
+      frc::Translation3d{fieldLength, fieldWidth, sideHt},
+  });
+  list.emplace_back(std::vector<frc::Translation3d>{
+      frc::Translation3d{fieldLength, 0_m, driveHt},
+      frc::Translation3d{fieldLength, fieldWidth, driveHt}});
+
+  // left side wall
+  list.emplace_back(std::vector<frc::Translation3d>{
+      frc::Translation3d{0_m, fieldWidth, 0_m},
+      frc::Translation3d{0_m, fieldWidth, sideHt},
+      frc::Translation3d{fieldLength, fieldWidth, sideHt},
+      frc::Translation3d{fieldLength, fieldWidth, 0_m}});
+
+  // blue driverstation
+  list.emplace_back(std::vector<frc::Translation3d>{
+      frc::Translation3d{0_m, 0_m, sideHt},
+      frc::Translation3d{0_m, 0_m, topHt},
+      frc::Translation3d{0_m, fieldWidth, topHt},
+      frc::Translation3d{0_m, fieldWidth, sideHt},
+  });
+  list.emplace_back(std::vector<frc::Translation3d>{
+      frc::Translation3d{0_m, 0_m, driveHt},
+      frc::Translation3d{0_m, fieldWidth, driveHt}});
+
+  return list;
+}
+
+static std::vector<std::vector<frc::Translation3d>> GetFieldFloorLines(
+    int subdivisions) {
+  std::vector<std::vector<frc::Translation3d>> list;
+  const units::meter_t subLength = fieldLength / subdivisions;
+  const units::meter_t subWidth = fieldWidth / subdivisions;
+
+  for (int i = 0; i < subdivisions; i++) {
+    list.emplace_back(std::vector<frc::Translation3d>{
+        frc::Translation3d{0_m, subWidth * (i + 1), 0_m},
+        frc::Translation3d{fieldLength, subWidth * (i + 1), 0_m}});
+    list.emplace_back(std::vector<frc::Translation3d>{
+        frc::Translation3d{subLength * (i + 1), 0_m, 0_m},
+        frc::Translation3d{subLength * (i + 1), fieldWidth, 0_m}});
+  }
+  return list;
+}
+
+static std::vector<std::vector<cv::Point2f>> PolyFrom3dLines(
+    const RotTrlTransform3d& camRt, const SimCameraProperties& prop,
+    const std::vector<frc::Translation3d>& trls, double resolution,
+    bool isClosed, cv::Mat& destination) {
+  resolution = std::hypot(destination.size().height, destination.size().width) *
+               resolution;
+  std::vector<frc::Translation3d> pts{trls};
+  if (isClosed) {
+    pts.emplace_back(pts[0]);
+  }
+  std::vector<std::vector<cv::Point2f>> polyPointList{};
+
+  for (size_t i = 0; i < pts.size() - 1; i++) {
+    frc::Translation3d pta = pts[i];
+    frc::Translation3d ptb = pts[i + 1];
+
+    std::pair<std::optional<double>, std::optional<double>> inter =
+        prop.GetVisibleLine(camRt, pta, ptb);
+    if (!inter.second) {
+      continue;
+    }
+
+    double inter1 = inter.first.value();
+    double inter2 = inter.second.value();
+    frc::Translation3d baseDelta = ptb - pta;
+    frc::Translation3d old_pta = pta;
+    if (inter1 > 0) {
+      pta = old_pta + baseDelta * inter1;
+    }
+    if (inter2 < 1) {
+      ptb = old_pta + baseDelta * inter2;
+    }
+    baseDelta = ptb - pta;
+
+    std::vector<cv::Point2f> poly = OpenCVHelp::ProjectPoints(
+        prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, {pta, ptb});
+    cv::Point2d pxa = poly[0];
+    cv::Point2d pxb = poly[1];
+
+    double pxDist = std::hypot(pxb.x - pxa.x, pxb.y - pxa.y);
+    int subdivisions = static_cast<int>(pxDist / resolution);
+    frc::Translation3d subDelta = baseDelta / (subdivisions + 1);
+    std::vector<frc::Translation3d> subPts{};
+    for (int j = 0; j < subdivisions; j++) {
+      subPts.emplace_back(pta + (subDelta * (j + 1)));
+    }
+    if (subPts.size() > 0) {
+      std::vector<cv::Point2f> toAdd = OpenCVHelp::ProjectPoints(
+          prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, subPts);
+      poly.insert(poly.begin() + 1, toAdd.begin(), toAdd.end());
+    }
+
+    polyPointList.emplace_back(poly);
+  }
+
+  return polyPointList;
+}
+
+[[maybe_unused]] static void DrawFieldWireFrame(
+    const RotTrlTransform3d& camRt, const SimCameraProperties& prop,
+    double resolution, double wallThickness, const cv::Scalar& wallColor,
+    int floorSubdivisions, double floorThickness, const cv::Scalar& floorColor,
+    cv::Mat& destination) {
+  for (const auto& trls : GetFieldFloorLines(floorSubdivisions)) {
+    auto polys =
+        PolyFrom3dLines(camRt, prop, trls, resolution, false, destination);
+    for (const auto& poly : polys) {
+      DrawPoly(poly,
+               static_cast<int>(
+                   std::round(GetScaledThickness(floorThickness, destination))),
+               floorColor, false, destination);
+    }
+  }
+  for (const auto& trls : GetFieldWallLines()) {
+    auto polys =
+        PolyFrom3dLines(camRt, prop, trls, resolution, false, destination);
+    for (const auto& poly : polys) {
+      DrawPoly(poly,
+               static_cast<int>(
+                   std::round(GetScaledThickness(wallThickness, destination))),
+               wallColor, false, destination);
+    }
+  }
+}
+}  // namespace VideoSimUtil
+}  // namespace photon
diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h
new file mode 100644
index 0000000000..55988bc32f
--- /dev/null
+++ b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h
@@ -0,0 +1,282 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include <frc/Timer.h>
+#include <frc/interpolation/TimeInterpolatableBuffer.h>
+#include <frc/smartdashboard/Field2d.h>
+#include <frc/smartdashboard/FieldObject2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+#include "photon/simulation/PhotonCameraSim.h"
+
+namespace photon {
+class VisionSystemSim {
+ public:
+  explicit VisionSystemSim(std::string visionSystemName) {
+    std::string tableName = "VisionSystemSim-" + visionSystemName;
+    frc::SmartDashboard::PutData(tableName + "/Sim Field", &dbgField);
+  }
+  std::optional<PhotonCameraSim*> GetCameraSim(std::string name) {
+    auto it = camSimMap.find(name);
+    if (it != camSimMap.end()) {
+      return std::make_optional(it->second);
+    } else {
+      return std::nullopt;
+    }
+  }
+  std::vector<PhotonCameraSim*> GetCameraSims() {
+    std::vector<PhotonCameraSim*> retVal;
+    for (auto const& cam : camSimMap) {
+      retVal.emplace_back(cam.second);
+    }
+    return retVal;
+  }
+  void AddCamera(PhotonCameraSim* cameraSim,
+                 const frc::Transform3d& robotToCamera) {
+    auto found =
+        camSimMap.find(std::string{cameraSim->GetCamera()->GetCameraName()});
+    if (found == camSimMap.end()) {
+      camSimMap[std::string{cameraSim->GetCamera()->GetCameraName()}] =
+          cameraSim;
+      camTrfMap.insert(std::make_pair(
+          std::move(cameraSim),
+          frc::TimeInterpolatableBuffer<frc::Pose3d>{bufferLength}));
+      camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(),
+                                        frc::Pose3d{} + robotToCamera);
+    }
+  }
+  void ClearCameras() {
+    camSimMap.clear();
+    camTrfMap.clear();
+  }
+  bool RemoveCamera(PhotonCameraSim* cameraSim) {
+    int numOfElementsRemoved =
+        camSimMap.erase(std::string{cameraSim->GetCamera()->GetCameraName()});
+    if (numOfElementsRemoved == 1) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+  std::optional<frc::Transform3d> GetRobotToCamera(PhotonCameraSim* cameraSim) {
+    return GetRobotToCamera(cameraSim, frc::Timer::GetFPGATimestamp());
+  }
+  std::optional<frc::Transform3d> GetRobotToCamera(PhotonCameraSim* cameraSim,
+                                                   units::second_t time) {
+    if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
+      frc::TimeInterpolatableBuffer<frc::Pose3d> trfBuffer =
+          camTrfMap.at(cameraSim);
+      std::optional<frc::Pose3d> sample = trfBuffer.Sample(time);
+      if (!sample) {
+        return std::nullopt;
+      } else {
+        return std::make_optional(
+            frc::Transform3d{frc::Pose3d{}, sample.value_or(frc::Pose3d{})});
+      }
+    } else {
+      return std::nullopt;
+    }
+  }
+  std::optional<frc::Pose3d> GetCameraPose(PhotonCameraSim* cameraSim) {
+    return GetCameraPose(cameraSim, frc::Timer::GetFPGATimestamp());
+  }
+  std::optional<frc::Pose3d> GetCameraPose(PhotonCameraSim* cameraSim,
+                                           units::second_t time) {
+    auto robotToCamera = GetRobotToCamera(cameraSim, time);
+    if (!robotToCamera) {
+      return std::nullopt;
+    } else {
+      return std::make_optional(GetRobotPose(time) + robotToCamera.value());
+    }
+  }
+  bool AdjustCamera(PhotonCameraSim* cameraSim,
+                    const frc::Transform3d& robotToCamera) {
+    if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
+      camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(),
+                                        frc::Pose3d{} + robotToCamera);
+      return true;
+    } else {
+      return false;
+    }
+  }
+  void ResetCameraTransforms() {
+    for (const auto& pair : camTrfMap) {
+      ResetCameraTransforms(pair.first);
+    }
+  }
+  bool ResetCameraTransforms(PhotonCameraSim* cameraSim) {
+    units::second_t now = frc::Timer::GetFPGATimestamp();
+    if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
+      auto trfBuffer = camTrfMap.at(cameraSim);
+      frc::Transform3d lastTrf{frc::Pose3d{},
+                               trfBuffer.Sample(now).value_or(frc::Pose3d{})};
+      trfBuffer.Clear();
+      AdjustCamera(cameraSim, lastTrf);
+      return true;
+    } else {
+      return false;
+    }
+  }
+  std::vector<VisionTargetSim> GetVisionTargets() {
+    std::vector<VisionTargetSim> all{};
+    for (const auto& entry : targetSets) {
+      for (const auto& target : entry.second) {
+        all.emplace_back(target);
+      }
+    }
+    return all;
+  }
+  std::vector<VisionTargetSim> GetVisionTargets(std::string type) {
+    return targetSets[type];
+  }
+  void AddVisionTargets(const std::vector<VisionTargetSim>& targets) {
+    AddVisionTargets("targets", targets);
+  }
+  void AddVisionTargets(std::string type,
+                        const std::vector<VisionTargetSim>& targets) {
+    if (!targetSets.contains(type)) {
+      targetSets[type] = std::vector<VisionTargetSim>{};
+    }
+    for (const auto& tgt : targets) {
+      targetSets[type].emplace_back(tgt);
+    }
+  }
+  void AddAprilTags(const frc::AprilTagFieldLayout& layout) {
+    std::vector<VisionTargetSim> targets;
+    for (const frc::AprilTag& tag : layout.GetTags()) {
+      targets.emplace_back(VisionTargetSim{layout.GetTagPose(tag.ID).value(),
+                                           photon::kAprilTag16h5, tag.ID});
+    }
+    AddVisionTargets("apriltag", targets);
+  }
+  void ClearVisionTargets() { targetSets.clear(); }
+  void ClearAprilTags() { RemoveVisionTargets("apriltag"); }
+  void RemoveVisionTargets(std::string type) { targetSets.erase(type); }
+  std::vector<VisionTargetSim> RemoveVisionTargets(
+      const std::vector<VisionTargetSim>& targets) {
+    std::vector<VisionTargetSim> removedList;
+    for (auto& entry : targetSets) {
+      for (auto target : entry.second) {
+        auto it = std::find(targets.begin(), targets.end(), target);
+        if (it != targets.end()) {
+          removedList.emplace_back(target);
+          entry.second.erase(it);
+        }
+      }
+    }
+    return removedList;
+  }
+  frc::Pose3d GetRobotPose() {
+    return GetRobotPose(frc::Timer::GetFPGATimestamp());
+  }
+  frc::Pose3d GetRobotPose(units::second_t timestamp) {
+    return robotPoseBuffer.Sample(timestamp).value_or(frc::Pose3d{});
+  }
+  void ResetRobotPose(const frc::Pose2d& robotPose) {
+    ResetRobotPose(frc::Pose3d{robotPose});
+  }
+  void ResetRobotPose(const frc::Pose3d& robotPose) {
+    robotPoseBuffer.Clear();
+    robotPoseBuffer.AddSample(frc::Timer::GetFPGATimestamp(), robotPose);
+  }
+  frc::Field2d& GetDebugField() { return dbgField; }
+  void Update(const frc::Pose2d& robotPose) { Update(frc::Pose3d{robotPose}); }
+  void Update(const frc::Pose3d& robotPose) {
+    for (auto& set : targetSets) {
+      std::vector<frc::Pose2d> posesToAdd{};
+      for (auto& target : set.second) {
+        posesToAdd.emplace_back(target.GetPose().ToPose2d());
+      }
+      dbgField.GetObject(set.first)->SetPoses(posesToAdd);
+    }
+
+    units::second_t now = frc::Timer::GetFPGATimestamp();
+    robotPoseBuffer.AddSample(now, robotPose);
+    dbgField.SetRobotPose(robotPose.ToPose2d());
+
+    std::vector<VisionTargetSim> allTargets{};
+    for (const auto& set : targetSets) {
+      for (const auto& target : set.second) {
+        allTargets.emplace_back(target);
+      }
+    }
+
+    std::vector<frc::Pose2d> visTgtPoses2d{};
+    std::vector<frc::Pose2d> cameraPoses2d{};
+    bool processed{false};
+    for (const auto& entry : camSimMap) {
+      auto camSim = entry.second;
+      auto optTimestamp = camSim->ConsumeNextEntryTime();
+      if (!optTimestamp) {
+        continue;
+      } else {
+        processed = true;
+      }
+      uint64_t timestampNt = optTimestamp.value();
+      units::second_t latency = camSim->prop.EstLatency();
+      units::second_t timestampCapture =
+          units::microsecond_t{static_cast<double>(timestampNt)} - latency;
+
+      frc::Pose3d lateRobotPose = GetRobotPose(timestampCapture);
+      frc::Pose3d lateCameraPose =
+          lateRobotPose + GetRobotToCamera(camSim, timestampCapture).value();
+      cameraPoses2d.push_back(lateCameraPose.ToPose2d());
+
+      auto camResult = camSim->Process(latency, lateCameraPose, allTargets);
+      camSim->SubmitProcessedFrame(camResult, timestampNt);
+      for (const auto& target : camResult.GetTargets()) {
+        auto trf = target.GetBestCameraToTarget();
+        if (trf == kEmptyTrf) {
+          continue;
+        }
+        visTgtPoses2d.push_back(lateCameraPose.TransformBy(trf).ToPose2d());
+      }
+    }
+    if (processed) {
+      dbgField.GetObject("visibleTargetPoses")->SetPoses(visTgtPoses2d);
+    }
+    if (cameraPoses2d.size() != 0) {
+      dbgField.GetObject("cameras")->SetPoses(cameraPoses2d);
+    }
+  }
+
+ private:
+  std::unordered_map<std::string, PhotonCameraSim*> camSimMap{};
+  static constexpr units::second_t bufferLength{1.5_s};
+  std::unordered_map<PhotonCameraSim*,
+                     frc::TimeInterpolatableBuffer<frc::Pose3d>>
+      camTrfMap;
+  frc::TimeInterpolatableBuffer<frc::Pose3d> robotPoseBuffer{bufferLength};
+  std::unordered_map<std::string, std::vector<VisionTargetSim>> targetSets{};
+  frc::Field2d dbgField{};
+  const frc::Transform3d kEmptyTrf{};
+};
+}  // namespace photon
diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h
new file mode 100644
index 0000000000..f8d4d6676b
--- /dev/null
+++ b/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h
@@ -0,0 +1,73 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <vector>
+
+#include <frc/geometry/Pose3d.h>
+
+#include "photon/estimation/TargetModel.h"
+
+namespace photon {
+class VisionTargetSim {
+ public:
+  VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model)
+      : fiducialId(-1), pose(pose), model(model) {}
+  VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model, int id)
+      : fiducialId(id), pose(pose), model(model) {}
+  void SetPose(const frc::Pose3d& newPose) { pose = newPose; }
+  void SetModel(const TargetModel& newModel) { model = newModel; }
+  frc::Pose3d GetPose() const { return pose; }
+  TargetModel GetModel() const { return model; }
+  std::vector<frc::Translation3d> GetFieldVertices() const {
+    return model.GetFieldVertices(pose);
+  }
+  int fiducialId;
+
+  bool operator<(const VisionTargetSim& right) const {
+    return pose.Translation().Norm() < right.pose.Translation().Norm();
+  }
+
+  bool operator==(const VisionTargetSim& other) const {
+    return units::math::abs(pose.Translation().X() -
+                            other.GetPose().Translation().X()) < 1_in &&
+           units::math::abs(pose.Translation().Y() -
+                            other.GetPose().Translation().Y()) < 1_in &&
+           units::math::abs(pose.Translation().Z() -
+                            other.GetPose().Translation().Z()) < 1_in &&
+           units::math::abs(pose.Rotation().X() -
+                            other.GetPose().Rotation().X()) < 1_deg &&
+           units::math::abs(pose.Rotation().Y() -
+                            other.GetPose().Rotation().Y()) < 1_deg &&
+           units::math::abs(pose.Rotation().Z() -
+                            other.GetPose().Rotation().Z()) < 1_deg &&
+           model.GetIsPlanar() == other.GetModel().GetIsPlanar();
+  }
+
+ private:
+  frc::Pose3d pose;
+  TargetModel model;
+};
+}  // namespace photon
diff --git a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp
new file mode 100644
index 0000000000..1c3f9eac58
--- /dev/null
+++ b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp
@@ -0,0 +1,461 @@
+/*
+ * 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 "gtest/gtest.h"
+#include "photon/PhotonUtils.h"
+#include "photon/simulation/VisionSystemSim.h"
+
+class VisionSystemSimTest : public ::testing::Test {
+  void SetUp() override {
+    nt::NetworkTableInstance::GetDefault().StartServer();
+    photon::PhotonCamera::SetVersionCheckEnabled(false);
+  }
+
+  void TearDown() override {}
+};
+
+class VisionSystemSimTestWithParamsTest
+    : public VisionSystemSimTest,
+      public testing::WithParamInterface<units::degree_t> {};
+class VisionSystemSimTestDistanceParamsTest
+    : public VisionSystemSimTest,
+      public testing::WithParamInterface<
+          std::tuple<units::foot_t, units::degree_t, units::foot_t>> {};
+
+TEST_F(VisionSystemSimTest, TestVisibilityCupidShuffle) {
+  frc::Pose3d targetPose{
+      frc::Translation3d{15.98_m, 0_m, 2_m},
+      frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
+
+  photon::VisionSystemSim visionSysSim{"Test"};
+  photon::PhotonCamera camera{"camera"};
+  photon::PhotonCameraSim cameraSim{&camera};
+  visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
+  cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPose, photon::TargetModel{1.0_m, 1.0_m}, 3}});
+
+  // To the right, to the right
+  frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-70_deg}};
+  visionSysSim.Update(robotPose);
+  ASSERT_FALSE(camera.GetLatestResult().HasTargets());
+
+  // To the right, to the right
+  robotPose =
+      frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-95_deg}};
+  visionSysSim.Update(robotPose);
+  ASSERT_FALSE(camera.GetLatestResult().HasTargets());
+
+  // To the left, to the left
+  robotPose =
+      frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{90_deg}};
+  visionSysSim.Update(robotPose);
+  ASSERT_FALSE(camera.GetLatestResult().HasTargets());
+
+  // To the left, to the left
+  robotPose =
+      frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{65_deg}};
+  visionSysSim.Update(robotPose);
+  ASSERT_FALSE(camera.GetLatestResult().HasTargets());
+
+  // Now kick, now kick
+  robotPose = frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{5_deg}};
+  visionSysSim.Update(robotPose);
+  ASSERT_TRUE(camera.GetLatestResult().HasTargets());
+
+  // Now kick, now kick
+  robotPose =
+      frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-5_deg}};
+  visionSysSim.Update(robotPose);
+  ASSERT_TRUE(camera.GetLatestResult().HasTargets());
+
+  // Now walk it by yourself
+  robotPose =
+      frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-179_deg}};
+  visionSysSim.Update(robotPose);
+  ASSERT_FALSE(camera.GetLatestResult().HasTargets());
+
+  // Now walk it by yourself
+  visionSysSim.AdjustCamera(
+      &cameraSim,
+      frc::Transform3d{
+          frc::Translation3d{},
+          frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}});
+  visionSysSim.Update(robotPose);
+  ASSERT_TRUE(camera.GetLatestResult().HasTargets());
+}
+
+TEST_F(VisionSystemSimTest, TestNotVisibleVert1) {
+  frc::Pose3d targetPose{
+      frc::Translation3d{15.98_m, 0_m, 1_m},
+      frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
+
+  photon::VisionSystemSim visionSysSim{"Test"};
+  photon::PhotonCamera camera{"camera"};
+  photon::PhotonCameraSim cameraSim{&camera};
+  visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
+  cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPose, photon::TargetModel{3.0_m, 3.0_m}, 3}});
+
+  frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{5_deg}};
+  visionSysSim.Update(robotPose);
+  ASSERT_TRUE(camera.GetLatestResult().HasTargets());
+
+  visionSysSim.AdjustCamera(
+      &cameraSim,
+      frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 5000_m},
+          frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}});
+  visionSysSim.Update(robotPose);
+  ASSERT_FALSE(camera.GetLatestResult().HasTargets());
+}
+
+TEST_F(VisionSystemSimTest, TestNotVisibleVert2) {
+  frc::Pose3d targetPose{
+      frc::Translation3d{15.98_m, 0_m, 2_m},
+      frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
+
+  frc::Transform3d robotToCamera{
+      frc::Translation3d{0_m, 0_m, 1_m},
+      frc::Rotation3d{0_rad, units::radian_t{-std::numbers::pi / 4}, 0_rad}};
+
+  photon::VisionSystemSim visionSysSim{"Test"};
+  photon::PhotonCamera camera{"camera"};
+  photon::PhotonCameraSim cameraSim{&camera};
+  visionSysSim.AddCamera(&cameraSim, robotToCamera);
+  cameraSim.prop.SetCalibration(1234, 1234, frc::Rotation2d{80_deg});
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPose, photon::TargetModel{0.5_m, 0.5_m}, 1736}});
+
+  frc::Pose2d robotPose{frc::Translation2d{13.98_m, 0_m},
+                        frc::Rotation2d{5_deg}};
+  visionSysSim.Update(robotPose);
+  ASSERT_TRUE(camera.GetLatestResult().HasTargets());
+
+  robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}};
+  visionSysSim.Update(robotPose);
+  ASSERT_FALSE(camera.GetLatestResult().HasTargets());
+}
+
+TEST_F(VisionSystemSimTest, TestNotVisibleTargetSize) {
+  frc::Pose3d targetPose{
+      frc::Translation3d{15.98_m, 0_m, 1_m},
+      frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
+
+  photon::VisionSystemSim visionSysSim{"Test"};
+  photon::PhotonCamera camera{"camera"};
+  photon::PhotonCameraSim cameraSim{&camera};
+  visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
+  cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
+  cameraSim.SetMinTargetAreaPixels(20.0);
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPose, photon::TargetModel{0.1_m, 0.1_m}, 24}});
+
+  frc::Pose2d robotPose{frc::Translation2d{12_m, 0_m}, frc::Rotation2d{5_deg}};
+  visionSysSim.Update(robotPose);
+  ASSERT_TRUE(camera.GetLatestResult().HasTargets());
+
+  robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}};
+  visionSysSim.Update(robotPose);
+  ASSERT_FALSE(camera.GetLatestResult().HasTargets());
+}
+
+TEST_F(VisionSystemSimTest, TestNotVisibleTooFarLeds) {
+  frc::Pose3d targetPose{
+      frc::Translation3d{15.98_m, 0_m, 1_m},
+      frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
+
+  photon::VisionSystemSim visionSysSim{"Test"};
+  photon::PhotonCamera camera{"camera"};
+  photon::PhotonCameraSim cameraSim{&camera};
+  visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
+  cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
+  cameraSim.SetMinTargetAreaPixels(1.0);
+  cameraSim.SetMaxSightRange(10_m);
+  visionSysSim.AddVisionTargets(
+      {photon::VisionTargetSim{targetPose, photon::TargetModel{1_m, 1_m}, 25}});
+
+  frc::Pose2d robotPose{frc::Translation2d{10_m, 0_m}, frc::Rotation2d{5_deg}};
+  visionSysSim.Update(robotPose);
+  ASSERT_TRUE(camera.GetLatestResult().HasTargets());
+
+  robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}};
+  visionSysSim.Update(robotPose);
+  ASSERT_FALSE(camera.GetLatestResult().HasTargets());
+}
+
+TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) {
+  const frc::Pose3d targetPose{
+      {15.98_m, 0_m, 0_m},
+      frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}};
+  frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}};
+  photon::VisionSystemSim visionSysSim{"Test"};
+  photon::PhotonCamera camera{"camera"};
+  photon::PhotonCameraSim cameraSim{&camera};
+  visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
+  cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
+  cameraSim.SetMinTargetAreaPixels(0.0);
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}});
+
+  robotPose = frc::Pose2d{frc::Translation2d{10_m, 0_m},
+                          frc::Rotation2d{-1 * GetParam()}};
+  visionSysSim.Update(robotPose);
+  ASSERT_TRUE(camera.GetLatestResult().HasTargets());
+  ASSERT_NEAR(GetParam().to<double>(),
+              camera.GetLatestResult().GetBestTarget().GetYaw(), 0.25);
+}
+
+TEST_P(VisionSystemSimTestWithParamsTest, PitchAngles) {
+  const frc::Pose3d targetPose{
+      {15.98_m, 0_m, 0_m},
+      frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}};
+  frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}};
+  photon::VisionSystemSim visionSysSim{"Test"};
+  photon::PhotonCamera camera{"camera"};
+  photon::PhotonCameraSim cameraSim{&camera};
+  visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
+  cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{120_deg});
+  cameraSim.SetMinTargetAreaPixels(0.0);
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}});
+
+  robotPose = frc::Pose2d{frc::Translation2d{10_m, 0_m},
+                          frc::Rotation2d{-1 * GetParam()}};
+  visionSysSim.AdjustCamera(
+      &cameraSim,
+      frc::Transform3d{
+          frc::Translation3d{},
+          frc::Rotation3d{0_rad, units::degree_t{GetParam()}, 0_rad}});
+  visionSysSim.Update(robotPose);
+  ASSERT_TRUE(camera.GetLatestResult().HasTargets());
+  ASSERT_NEAR(GetParam().to<double>(),
+              camera.GetLatestResult().GetBestTarget().GetPitch(), 0.25);
+}
+
+INSTANTIATE_TEST_SUITE_P(AnglesTests, VisionSystemSimTestWithParamsTest,
+                         testing::Values(-10_deg, -5_deg, -0_deg, -1_deg,
+                                         -2_deg, 5_deg, 7_deg, 10.23_deg));
+
+TEST_P(VisionSystemSimTestDistanceParamsTest, DistanceCalc) {
+  units::foot_t distParam;
+  units::degree_t pitchParam;
+  units::foot_t heightParam;
+  std::tie(distParam, pitchParam, heightParam) = GetParam();
+
+  const frc::Pose3d targetPose{
+      {15.98_m, 0_m, 1_m},
+      frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi * 0.98}}};
+  frc::Pose3d robotPose{{15.98_m - distParam, 0_m, 0_m}, frc::Rotation3d{}};
+  frc::Transform3d robotToCamera{frc::Translation3d{0_m, 0_m, heightParam},
+                                 frc::Rotation3d{0_deg, pitchParam, 0_deg}};
+  photon::VisionSystemSim visionSysSim{
+      "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho"
+      "wsyourdaygoingihopegoodhaveagreatrestofyourlife"};
+  photon::PhotonCamera camera{"camera"};
+  photon::PhotonCameraSim cameraSim{&camera};
+  visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
+  cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{160_deg});
+  cameraSim.SetMinTargetAreaPixels(0.0);
+  visionSysSim.AdjustCamera(&cameraSim, robotToCamera);
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPose, photon::TargetModel{0.5_m, 0.5_m}, 0}});
+  visionSysSim.Update(robotPose);
+
+  photon::PhotonPipelineResult res = camera.GetLatestResult();
+  ASSERT_TRUE(res.HasTargets());
+  photon::PhotonTrackedTarget target = res.GetBestTarget();
+
+  ASSERT_NEAR(0.0, target.GetYaw(), 0.5);
+
+  units::meter_t dist = photon::PhotonUtils::CalculateDistanceToTarget(
+      robotToCamera.Z(), targetPose.Z(), -pitchParam,
+      units::degree_t{target.GetPitch()});
+  ASSERT_NEAR(dist.to<double>(),
+              distParam.convert<units::meters>().to<double>(), 0.25);
+}
+
+INSTANTIATE_TEST_SUITE_P(
+    DistanceParamsTests, VisionSystemSimTestDistanceParamsTest,
+    testing::Values(std::make_tuple(5_ft, -15.98_deg, 0_ft),
+                    std::make_tuple(6_ft, -15.98_deg, 1_ft),
+                    std::make_tuple(10_ft, -15.98_deg, 0_ft),
+                    std::make_tuple(15_ft, -15.98_deg, 2_ft),
+                    std::make_tuple(19.95_ft, -15.98_deg, 0_ft),
+                    std::make_tuple(20_ft, -15.98_deg, 0_ft),
+                    std::make_tuple(5_ft, -42_deg, 1_ft),
+                    std::make_tuple(6_ft, -42_deg, 0_ft),
+                    std::make_tuple(10_ft, -42_deg, 2_ft),
+                    std::make_tuple(15_ft, -42_deg, 0.5_ft),
+                    std::make_tuple(19.42_ft, -15.98_deg, 0_ft),
+                    std::make_tuple(20_ft, -42_deg, 0_ft),
+                    std::make_tuple(5_ft, -55_deg, 2_ft),
+                    std::make_tuple(6_ft, -55_deg, 0_ft),
+                    std::make_tuple(10_ft, -54_deg, 2.2_ft),
+                    std::make_tuple(15_ft, -53_deg, 0_ft),
+                    std::make_tuple(19.52_ft, -15.98_deg, 1.1_ft)));
+
+TEST_F(VisionSystemSimTest, TestMultipleTargets) {
+  frc::Pose3d targetPoseL{
+      frc::Translation3d{15.98_m, 2_m, 0_m},
+      frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
+  frc::Pose3d targetPoseC{
+      frc::Translation3d{15.98_m, 0_m, 0_m},
+      frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
+  frc::Pose3d targetPoseR{
+      frc::Translation3d{15.98_m, -2_m, 0_m},
+      frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
+
+  photon::VisionSystemSim visionSysSim{"Test"};
+  photon::PhotonCamera camera{"camera"};
+  photon::PhotonCameraSim cameraSim{&camera};
+  visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
+  cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
+  cameraSim.SetMinTargetAreaPixels(20.0);
+
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPoseL.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
+      photon::kAprilTag16h5, 1}});
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPoseC.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
+      photon::kAprilTag16h5, 2}});
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPoseR.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
+      photon::kAprilTag16h5, 3}});
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPoseL.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
+      photon::kAprilTag16h5, 4}});
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPoseC.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
+      photon::kAprilTag16h5, 5}});
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPoseR.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
+      photon::kAprilTag16h5, 6}});
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPoseL.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}),
+      photon::kAprilTag16h5, 7}});
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPoseC.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}),
+      photon::kAprilTag16h5, 8}});
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPoseL.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}),
+      photon::kAprilTag16h5, 9}});
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPoseR.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}),
+      photon::kAprilTag16h5, 10}});
+  visionSysSim.AddVisionTargets({photon::VisionTargetSim{
+      targetPoseL.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0.25_m}, frc::Rotation3d{}}),
+      photon::kAprilTag16h5, 11}});
+
+  frc::Pose2d robotPose{frc::Translation2d{6_m, 0_m}, frc::Rotation2d{.25_deg}};
+  visionSysSim.Update(robotPose);
+  photon::PhotonPipelineResult res = camera.GetLatestResult();
+  ASSERT_TRUE(res.HasTargets());
+  std::span<const photon::PhotonTrackedTarget> tgtList = res.GetTargets();
+  ASSERT_EQ(static_cast<size_t>(11), tgtList.size());
+}
+
+TEST_F(VisionSystemSimTest, TestPoseEstimation) {
+  photon::VisionSystemSim visionSysSim{"Test"};
+  photon::PhotonCamera camera{"camera"};
+  photon::PhotonCameraSim cameraSim{&camera};
+  visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
+  cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg});
+  cameraSim.SetMinTargetAreaPixels(20.0);
+
+  std::vector<frc::AprilTag> tagList;
+  tagList.emplace_back(frc::AprilTag{
+      0, frc::Pose3d{12_m, 3_m, 1_m,
+                     frc::Rotation3d{0_rad, 0_rad,
+                                     units::radian_t{std::numbers::pi}}}});
+  tagList.emplace_back(frc::AprilTag{
+      1, frc::Pose3d{12_m, 1_m, -1_m,
+                     frc::Rotation3d{0_rad, 0_rad,
+                                     units::radian_t{std::numbers::pi}}}});
+  tagList.emplace_back(frc::AprilTag{
+      2, frc::Pose3d{11_m, 0_m, 2_m,
+                     frc::Rotation3d{0_rad, 0_rad,
+                                     units::radian_t{std::numbers::pi}}}});
+  units::meter_t fieldLength{54};
+  units::meter_t fieldWidth{27};
+  frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth};
+  frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{5_deg}};
+  visionSysSim.AddVisionTargets(
+      {photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag16h5, 0}});
+  visionSysSim.Update(robotPose);
+
+  Eigen::Matrix<double, 3, 3> camEigen;
+  cv::cv2eigen(camera.GetCameraMatrix().value(), camEigen);
+
+  Eigen::Matrix<double, 5, 1> distEigen;
+  cv::cv2eigen(camera.GetDistCoeffs().value(), distEigen);
+
+  auto camResults = camera.GetLatestResult();
+  auto targetSpan = camResults.GetTargets();
+  std::vector<photon::PhotonTrackedTarget> targets;
+  for (photon::PhotonTrackedTarget tar : targetSpan) {
+    targets.push_back(tar);
+  }
+  photon::PNPResult results = photon::VisionEstimation::EstimateCamPosePNP(
+      camEigen, distEigen, targets, layout, photon::kAprilTag16h5);
+  frc::Pose3d pose = frc::Pose3d{} + results.best;
+  ASSERT_NEAR(5, pose.X().to<double>(), 0.01);
+  ASSERT_NEAR(1, pose.Y().to<double>(), 0.01);
+  ASSERT_NEAR(0, pose.Z().to<double>(), 0.01);
+  ASSERT_NEAR(units::degree_t{5}.convert<units::radians>().to<double>(),
+              pose.Rotation().Z().to<double>(), 0.01);
+
+  visionSysSim.AddVisionTargets(
+      {photon::VisionTargetSim{tagList[1].pose, photon::kAprilTag16h5, 1}});
+  visionSysSim.AddVisionTargets(
+      {photon::VisionTargetSim{tagList[2].pose, photon::kAprilTag16h5, 2}});
+  visionSysSim.Update(robotPose);
+
+  auto camResults2 = camera.GetLatestResult();
+  auto targetSpan2 = camResults2.GetTargets();
+  std::vector<photon::PhotonTrackedTarget> targets2;
+  for (photon::PhotonTrackedTarget tar : targetSpan2) {
+    targets2.push_back(tar);
+  }
+  photon::PNPResult results2 = photon::VisionEstimation::EstimateCamPosePNP(
+      camEigen, distEigen, targets2, layout, photon::kAprilTag16h5);
+  frc::Pose3d pose2 = frc::Pose3d{} + results2.best;
+  ASSERT_NEAR(5, pose2.X().to<double>(), 0.01);
+  ASSERT_NEAR(1, pose2.Y().to<double>(), 0.01);
+  ASSERT_NEAR(0, pose2.Z().to<double>(), 0.01);
+  ASSERT_NEAR(units::degree_t{5}.convert<units::radians>().to<double>(),
+              pose2.Rotation().Z().to<double>(), 0.01);
+}
diff --git a/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h b/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h
new file mode 100644
index 0000000000..a0a0fb0e61
--- /dev/null
+++ b/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <frc/geometry/Pose3d.h>
+
+namespace photon {
+class CameraTargetRelation {
+ public:
+  const frc::Pose3d camPose;
+  const frc::Transform3d camToTarg;
+  const units::meter_t camToTargDist;
+  const units::meter_t camToTargDistXY;
+  const frc::Rotation2d camToTargYaw;
+  const frc::Rotation2d camToTargPitch;
+
+  const frc::Rotation2d camToTargAngle;
+
+  const frc::Transform3d targToCam;
+  const frc::Rotation2d targToCamYaw;
+  const frc::Rotation2d targToCamPitch;
+
+  const frc::Rotation2d targToCamAngle;
+
+  CameraTargetRelation(const frc::Pose3d& cameraPose,
+                       const frc::Pose3d& targetPose)
+      : camPose(cameraPose),
+        camToTarg(frc::Transform3d{cameraPose, targetPose}),
+        camToTargDist(camToTarg.Translation().Norm()),
+        camToTargDistXY(units::math::hypot(camToTarg.Translation().X(),
+                                           camToTarg.Translation().Y())),
+        camToTargYaw(frc::Rotation2d{camToTarg.X().to<double>(),
+                                     camToTarg.Y().to<double>()}),
+        camToTargPitch(frc::Rotation2d{camToTargDistXY.to<double>(),
+                                       -camToTarg.Z().to<double>()}),
+        camToTargAngle(frc::Rotation2d{units::math::hypot(
+            camToTargYaw.Radians(), camToTargPitch.Radians())}),
+        targToCam(frc::Transform3d{targetPose, cameraPose}),
+        targToCamYaw(frc::Rotation2d{targToCam.X().to<double>(),
+                                     targToCam.Y().to<double>()}),
+        targToCamPitch(frc::Rotation2d{camToTargDistXY.to<double>(),
+                                       -targToCam.Z().to<double>()}),
+        targToCamAngle(frc::Rotation2d{units::math::hypot(
+            targToCamYaw.Radians(), targToCamPitch.Radians())}) {}
+};
+}  // namespace photon
diff --git a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h
new file mode 100644
index 0000000000..2d79e661e0
--- /dev/null
+++ b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h
@@ -0,0 +1,291 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <utility>
+#include <vector>
+
+#include <Eigen/Core>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "RotTrlTransform3d.h"
+
+#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT
+#include <opencv2/core/eigen.hpp>
+#include "photon/targeting/PNPResult.h"
+#include "photon/targeting/MultiTargetPNPResult.h"
+
+namespace photon {
+namespace OpenCVHelp {
+
+static frc::Rotation3d NWU_TO_EDN{
+    (Eigen::Matrix3d() << 0, -1, 0, 0, 0, -1, 1, 0, 0).finished()};
+static frc::Rotation3d EDN_TO_NWU{
+    (Eigen::Matrix3d() << 0, 0, 1, -1, 0, 0, 0, -1, 0).finished()};
+
+static std::vector<cv::Point2f> GetConvexHull(
+    const std::vector<cv::Point2f>& points) {
+  std::vector<int> outputHull{};
+  cv::convexHull(points, outputHull);
+  std::vector<cv::Point2f> convexPoints;
+  for (size_t i = 0; i < outputHull.size(); i++) {
+    convexPoints.push_back(points[outputHull[i]]);
+  }
+  return convexPoints;
+}
+
+static cv::RotatedRect GetMinAreaRect(const std::vector<cv::Point2f>& points) {
+  return cv::minAreaRect(points);
+}
+
+static frc::Translation3d TranslationNWUtoEDN(const frc::Translation3d& trl) {
+  return trl.RotateBy(NWU_TO_EDN);
+}
+
+static frc::Rotation3d RotationNWUtoEDN(const frc::Rotation3d& rot) {
+  return -NWU_TO_EDN + (rot + NWU_TO_EDN);
+}
+
+static std::vector<cv::Point3f> TranslationToTVec(
+    const std::vector<frc::Translation3d>& translations) {
+  std::vector<cv::Point3f> retVal;
+  retVal.reserve(translations.size());
+  for (size_t i = 0; i < translations.size(); i++) {
+    frc::Translation3d trl = TranslationNWUtoEDN(translations[i]);
+    retVal.emplace_back(cv::Point3f{trl.X().to<float>(), trl.Y().to<float>(),
+                                    trl.Z().to<float>()});
+  }
+  return retVal;
+}
+
+static std::vector<cv::Point3f> RotationToRVec(
+    const frc::Rotation3d& rotation) {
+  std::vector<cv::Point3f> retVal{};
+  frc::Rotation3d rot = RotationNWUtoEDN(rotation);
+  retVal.emplace_back(cv::Point3d{
+      rot.GetQuaternion().ToRotationVector()(0),
+      rot.GetQuaternion().ToRotationVector()(1),
+      rot.GetQuaternion().ToRotationVector()(2),
+  });
+  return retVal;
+}
+
+[[maybe_unused]] static cv::Point2f AvgPoint(std::vector<cv::Point2f> points) {
+  if (points.size() == 0) {
+    return cv::Point2f{};
+  }
+  cv::reduce(points, points, 0, cv::REDUCE_AVG);
+  return points[0];
+}
+
+[[maybe_unused]] static std::vector<std::pair<float, float>> PointsToCorners(
+    const std::vector<cv::Point2f>& points) {
+  std::vector<std::pair<float, float>> retVal;
+  retVal.reserve(points.size());
+  for (size_t i = 0; i < points.size(); i++) {
+    retVal.emplace_back(std::make_pair(points[i].x, points[i].y));
+  }
+  return retVal;
+}
+
+[[maybe_unused]] static std::vector<cv::Point2f> CornersToPoints(
+    const std::vector<std::pair<float, float>>& corners) {
+  std::vector<cv::Point2f> retVal;
+  retVal.reserve(corners.size());
+  for (size_t i = 0; i < corners.size(); i++) {
+    retVal.emplace_back(cv::Point2f{corners[i].first, corners[i].second});
+  }
+  return retVal;
+}
+
+[[maybe_unused]] static cv::Rect GetBoundingRect(
+    const std::vector<cv::Point2f>& points) {
+  return cv::boundingRect(points);
+}
+
+static std::vector<cv::Point2f> ProjectPoints(
+    const Eigen::Matrix<double, 3, 3>& cameraMatrix,
+    const Eigen::Matrix<double, 5, 1>& distCoeffs,
+    const RotTrlTransform3d& camRt,
+    const std::vector<frc::Translation3d>& objectTranslations) {
+  std::vector<cv::Point3f> objectPoints = TranslationToTVec(objectTranslations);
+  std::vector<cv::Point3f> rvec = RotationToRVec(camRt.GetRotation());
+  std::vector<cv::Point3f> tvec = TranslationToTVec({camRt.GetTranslation()});
+  cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F);
+  cv::eigen2cv(cameraMatrix, cameraMat);
+  cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F);
+  cv::eigen2cv(distCoeffs, distCoeffsMat);
+  std::vector<cv::Point2f> imagePoints{};
+  cv::projectPoints(objectPoints, rvec, tvec, cameraMat, distCoeffsMat,
+                    imagePoints);
+  return imagePoints;
+}
+
+template <typename T>
+static std::vector<T> ReorderCircular(const std::vector<T> elements,
+                                      bool backwards, int shiftStart) {
+  size_t size = elements.size();
+  int dir = backwards ? -1 : 1;
+  std::vector<T> reordered{elements};
+  for (size_t i = 0; i < size; i++) {
+    int index = (i * dir + shiftStart * dir) % size;
+    if (index < 0) {
+      index = size + index;
+    }
+    reordered[i] = elements[index];
+  }
+  return reordered;
+}
+
+static frc::Translation3d TranslationEDNToNWU(const frc::Translation3d& trl) {
+  return trl.RotateBy(EDN_TO_NWU);
+}
+
+static frc::Rotation3d RotationEDNToNWU(const frc::Rotation3d& rot) {
+  return -EDN_TO_NWU + (rot + EDN_TO_NWU);
+}
+
+static frc::Translation3d TVecToTranslation(const cv::Mat& tvecInput) {
+  cv::Vec3f data{};
+  cv::Mat wrapped{tvecInput.rows, tvecInput.cols, CV_32F};
+  tvecInput.convertTo(wrapped, CV_32F);
+  data = wrapped.at<cv::Vec3f>(cv::Point{0, 0});
+  return TranslationEDNToNWU(frc::Translation3d{units::meter_t{data[0]},
+                                                units::meter_t{data[1]},
+                                                units::meter_t{data[2]}});
+}
+
+static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) {
+  cv::Vec3f data{};
+  cv::Mat wrapped{rvecInput.rows, rvecInput.cols, CV_32F};
+  rvecInput.convertTo(wrapped, CV_32F);
+  data = wrapped.at<cv::Vec3f>(cv::Point{0, 0});
+  return RotationEDNToNWU(frc::Rotation3d{units::radian_t{data[0]},
+                                          units::radian_t{data[1]},
+                                          units::radian_t{data[2]}});
+}
+
+[[maybe_unused]] static photon::PNPResult SolvePNP_Square(
+    const Eigen::Matrix<double, 3, 3>& cameraMatrix,
+    const Eigen::Matrix<double, 5, 1>& distCoeffs,
+    std::vector<frc::Translation3d> modelTrls,
+    std::vector<cv::Point2f> imagePoints) {
+  modelTrls = ReorderCircular(modelTrls, true, -1);
+  imagePoints = ReorderCircular(imagePoints, true, -1);
+  std::vector<cv::Point3f> objectMat = TranslationToTVec(modelTrls);
+  std::vector<cv::Mat> rvecs;
+  std::vector<cv::Mat> tvecs;
+  cv::Mat rvec = cv::Mat::zeros(3, 1, CV_32F);
+  cv::Mat tvec = cv::Mat::zeros(3, 1, CV_32F);
+  cv::Mat reprojectionError = cv::Mat::zeros(2, 1, CV_32F);
+
+  cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_32F);
+  cv::eigen2cv(cameraMatrix, cameraMat);
+  cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_32F);
+  cv::eigen2cv(distCoeffs, distCoeffsMat);
+
+  cv::Vec2d errors{};
+  frc::Transform3d best{};
+  std::optional<frc::Transform3d> alt{std::nullopt};
+
+  for (int tries = 0; tries < 2; tries++) {
+    cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs,
+                        tvecs, false, cv::SOLVEPNP_IPPE_SQUARE, rvec, tvec,
+                        reprojectionError);
+
+    errors = reprojectionError.at<cv::Vec2f>(cv::Point{0, 0});
+    best = frc::Transform3d{TVecToTranslation(tvecs.at(0)),
+                            RVecToRotation(rvecs[0])};
+
+    if (tvecs.size() > 1) {
+      alt = frc::Transform3d{TVecToTranslation(tvecs.at(1)),
+                             RVecToRotation(rvecs[1])};
+    }
+
+    if (!std::isnan(errors[0])) {
+      break;
+    } else {
+      cv::Point2f pt = imagePoints[0];
+      pt.x -= 0.001f;
+      pt.y -= 0.001f;
+      imagePoints[0] = pt;
+    }
+  }
+
+  if (std::isnan(errors[0])) {
+    fmt::print("SolvePNP_Square failed!\n");
+  }
+  if (alt) {
+    photon::PNPResult result;
+    result.best = best;
+    result.alt = alt.value();
+    result.ambiguity = errors[0] / errors[1];
+    result.bestReprojErr = errors[0];
+    result.altReprojErr = errors[1];
+    result.isPresent = true;
+    return result;
+  } else {
+    photon::PNPResult result;
+    result.best = best;
+    result.bestReprojErr = errors[0];
+    result.isPresent = true;
+    return result;
+  }
+}
+
+[[maybe_unused]] static photon::PNPResult SolvePNP_SQPNP(
+    const Eigen::Matrix<double, 3, 3>& cameraMatrix,
+    const Eigen::Matrix<double, 5, 1>& distCoeffs,
+    std::vector<frc::Translation3d> modelTrls,
+    std::vector<cv::Point2f> imagePoints) {
+  std::vector<cv::Point3f> objectMat = TranslationToTVec(modelTrls);
+  std::vector<cv::Mat> rvecs{};
+  std::vector<cv::Mat> tvecs{};
+  cv::Mat rvec = cv::Mat::zeros(3, 1, CV_32F);
+  cv::Mat tvec = cv::Mat::zeros(3, 1, CV_32F);
+  cv::Mat reprojectionError = cv::Mat::zeros(2, 1, CV_32F);
+
+  cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F);
+  cv::eigen2cv(cameraMatrix, cameraMat);
+  cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F);
+  cv::eigen2cv(distCoeffs, distCoeffsMat);
+
+  float error = 0;
+  frc::Transform3d best{};
+
+  cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs,
+                      tvecs, false, cv::SOLVEPNP_SQPNP, rvec, tvec,
+                      reprojectionError);
+
+  error = reprojectionError.at<float>(cv::Point{0, 0});
+  best = frc::Transform3d{TVecToTranslation(tvecs.at(0)),
+                          RVecToRotation(rvecs[0])};
+
+  if (std::isnan(error)) {
+    fmt::print("SolvePNP_Square failed!\n");
+  }
+  photon::PNPResult result;
+  result.best = best;
+  result.bestReprojErr = error;
+  result.isPresent = true;
+  return result;
+}
+}  // namespace OpenCVHelp
+}  // namespace photon
diff --git a/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h
new file mode 100644
index 0000000000..99e19bf2c3
--- /dev/null
+++ b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h
@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <vector>
+
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Rotation3d.h>
+#include <frc/geometry/Translation3d.h>
+
+namespace photon {
+class RotTrlTransform3d {
+ public:
+  RotTrlTransform3d(const frc::Rotation3d& rot, const frc::Translation3d& trl)
+      : trl(trl), rot(rot) {}
+  RotTrlTransform3d(const frc::Pose3d& initial, const frc::Pose3d& last)
+      : trl(last.Translation() - initial.Translation().RotateBy(rot)),
+        rot(last.Rotation() - initial.Rotation()) {}
+  explicit RotTrlTransform3d(const frc::Transform3d& trf)
+      : RotTrlTransform3d(trf.Rotation(), trf.Translation()) {}
+  RotTrlTransform3d()
+      : RotTrlTransform3d(frc::Rotation3d{}, frc::Translation3d{}) {}
+
+  static RotTrlTransform3d MakeRelativeTo(const frc::Pose3d& pose) {
+    return RotTrlTransform3d{pose.Rotation(), pose.Translation()}.Inverse();
+  }
+
+  RotTrlTransform3d Inverse() const {
+    frc::Rotation3d invRot = -rot;
+    frc::Translation3d invTrl = -(trl.RotateBy(invRot));
+    return RotTrlTransform3d{invRot, invTrl};
+  }
+
+  frc::Transform3d GetTransform() const { return frc::Transform3d{trl, rot}; }
+
+  frc::Translation3d GetTranslation() const { return trl; }
+
+  frc::Rotation3d GetRotation() const { return rot; }
+
+  frc::Translation3d Apply(const frc::Translation3d& trlToApply) const {
+    return trlToApply.RotateBy(rot) + trl;
+  }
+
+  std::vector<frc::Translation3d> ApplyTrls(
+      const std::vector<frc::Translation3d>& trls) const {
+    std::vector<frc::Translation3d> retVal;
+    retVal.reserve(trls.size());
+    for (const auto& currentTrl : trls) {
+      retVal.emplace_back(Apply(currentTrl));
+    }
+    return retVal;
+  }
+
+  frc::Rotation3d Apply(const frc::Rotation3d& rotToApply) const {
+    return rotToApply + rot;
+  }
+
+  std::vector<frc::Rotation3d> ApplyTrls(
+      const std::vector<frc::Rotation3d>& rots) const {
+    std::vector<frc::Rotation3d> retVal;
+    retVal.reserve(rots.size());
+    for (const auto& currentRot : rots) {
+      retVal.emplace_back(Apply(currentRot));
+    }
+    return retVal;
+  }
+
+  frc::Pose3d Apply(const frc::Pose3d& poseToApply) const {
+    return frc::Pose3d{Apply(poseToApply.Translation()),
+                       Apply(poseToApply.Rotation())};
+  }
+
+  std::vector<frc::Pose3d> ApplyPoses(
+      const std::vector<frc::Pose3d>& poses) const {
+    std::vector<frc::Pose3d> retVal;
+    retVal.reserve(poses.size());
+    for (const auto& currentPose : poses) {
+      retVal.emplace_back(Apply(currentPose));
+    }
+    return retVal;
+  }
+
+ private:
+  const frc::Translation3d trl;
+  const frc::Rotation3d rot;
+};
+}  // namespace photon
diff --git a/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h b/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h
new file mode 100644
index 0000000000..64392fdb11
--- /dev/null
+++ b/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h
@@ -0,0 +1,115 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <vector>
+
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Translation3d.h>
+
+#include "RotTrlTransform3d.h"
+
+namespace photon {
+class TargetModel {
+ public:
+  TargetModel(units::meter_t width, units::meter_t height)
+      : vertices({frc::Translation3d{0_m, -width / 2.0, -height / 2.0},
+                  frc::Translation3d{0_m, width / 2.0, -height / 2.0},
+                  frc::Translation3d{0_m, width / 2.0, height / 2.0},
+                  frc::Translation3d{0_m, -width / 2.0, height / 2.0}}),
+        isPlanar(true),
+        isSpherical(false) {}
+
+  TargetModel(units::meter_t length, units::meter_t width,
+              units::meter_t height)
+      : TargetModel({
+            frc::Translation3d{length / 2.0, -width / 2.0, -height / 2.0},
+            frc::Translation3d{length / 2.0, width / 2.0, -height / 2.0},
+            frc::Translation3d{length / 2.0, width / 2.0, height / 2.0},
+            frc::Translation3d{length / 2.0, -width / 2.0, height / 2.0},
+            frc::Translation3d{-length / 2.0, -width / 2.0, height / 2.0},
+            frc::Translation3d{-length / 2.0, width / 2.0, height / 2.0},
+            frc::Translation3d{-length / 2.0, width / 2.0, -height / 2.0},
+            frc::Translation3d{-length / 2.0, -width / 2.0, -height / 2.0},
+        }) {}
+
+  explicit TargetModel(units::meter_t diameter)
+      : vertices({
+            frc::Translation3d{0_m, -diameter / 2.0, 0_m},
+            frc::Translation3d{0_m, 0_m, -diameter / 2.0},
+            frc::Translation3d{0_m, diameter / 2.0, 0_m},
+            frc::Translation3d{0_m, 0_m, diameter / 2.0},
+        }),
+        isPlanar(false),
+        isSpherical(true) {}
+
+  explicit TargetModel(const std::vector<frc::Translation3d>& verts)
+      : isSpherical(false) {
+    if (verts.size() <= 2) {
+      vertices = std::vector<frc::Translation3d>();
+      isPlanar = false;
+    } else {
+      bool cornersPlanar = true;
+      for (const auto& corner : verts) {
+        if (corner.X() != 0_m) {
+          cornersPlanar = false;
+        }
+      }
+      isPlanar = cornersPlanar;
+    }
+    vertices = verts;
+  }
+
+  std::vector<frc::Translation3d> GetFieldVertices(
+      const frc::Pose3d& targetPose) const {
+    RotTrlTransform3d basisChange{targetPose.Rotation(),
+                                  targetPose.Translation()};
+    std::vector<frc::Translation3d> retVal;
+    retVal.reserve(vertices.size());
+    for (const auto& vert : vertices) {
+      retVal.emplace_back(basisChange.Apply(vert));
+    }
+    return retVal;
+  }
+
+  static frc::Pose3d GetOrientedPose(const frc::Translation3d& tgtTrl,
+                                     const frc::Translation3d& cameraTrl) {
+    frc::Translation3d relCam = cameraTrl - tgtTrl;
+    frc::Rotation3d orientToCam = frc::Rotation3d{
+        0_rad,
+        frc::Rotation2d{units::math::hypot(relCam.X(), relCam.Y()).to<double>(),
+                        -relCam.Z().to<double>()}
+            .Radians(),
+        frc::Rotation2d{relCam.X().to<double>(), relCam.Y().to<double>()}
+            .Radians()};
+    return frc::Pose3d{tgtTrl, orientToCam};
+  }
+
+  std::vector<frc::Translation3d> GetVertices() const { return vertices; }
+  bool GetIsPlanar() const { return isPlanar; }
+  bool GetIsSpherical() const { return isSpherical; }
+
+ private:
+  std::vector<frc::Translation3d> vertices;
+  bool isPlanar;
+  bool isSpherical;
+};
+
+static const TargetModel kAprilTag16h5{6_in, 6_in};
+static const TargetModel kAprilTag36h11{6.5_in, 6.5_in};
+}  // namespace photon
diff --git a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h
new file mode 100644
index 0000000000..cb3ae3bdc5
--- /dev/null
+++ b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h
@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <utility>
+#include <vector>
+
+#include <Eigen/Core>
+#include <frc/apriltag/AprilTag.h>
+#include <frc/apriltag/AprilTagFieldLayout.h>
+
+#include "OpenCVHelp.h"
+#include "TargetModel.h"
+#include "photon/targeting/MultiTargetPNPResult.h"
+#include "photon/targeting/PhotonTrackedTarget.h"
+
+namespace photon {
+namespace VisionEstimation {
+
+static std::vector<frc::AprilTag> GetVisibleLayoutTags(
+    const std::vector<PhotonTrackedTarget>& visTags,
+    const frc::AprilTagFieldLayout& layout) {
+  std::vector<frc::AprilTag> retVal{};
+  for (const auto& tag : visTags) {
+    int id = tag.GetFiducialId();
+    auto maybePose = layout.GetTagPose(id);
+    if (maybePose) {
+      retVal.emplace_back(frc::AprilTag{id, maybePose.value()});
+    }
+  }
+  return retVal;
+}
+
+static PNPResult EstimateCamPosePNP(
+    const Eigen::Matrix<double, 3, 3>& cameraMatrix,
+    const Eigen::Matrix<double, 5, 1>& distCoeffs,
+    const std::vector<PhotonTrackedTarget>& visTags,
+    const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) {
+  if (visTags.size() == 0) {
+    return PNPResult();
+  }
+
+  std::vector<std::pair<float, float>> corners{};
+  std::vector<frc::AprilTag> knownTags{};
+
+  for (const auto& tgt : visTags) {
+    int id = tgt.GetFiducialId();
+    auto maybePose = layout.GetTagPose(id);
+    if (maybePose) {
+      knownTags.emplace_back(frc::AprilTag{id, maybePose.value()});
+      auto currentCorners = tgt.GetDetectedCorners();
+      corners.insert(corners.end(), currentCorners.begin(),
+                     currentCorners.end());
+    }
+  }
+  if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
+    return PNPResult{};
+  }
+
+  std::vector<cv::Point2f> points = OpenCVHelp::CornersToPoints(corners);
+
+  if (knownTags.size() == 1) {
+    PNPResult camToTag = OpenCVHelp::SolvePNP_Square(
+        cameraMatrix, distCoeffs, tagModel.GetVertices(), points);
+    if (!camToTag.isPresent) {
+      return PNPResult{};
+    }
+    frc::Pose3d bestPose =
+        knownTags[0].pose.TransformBy(camToTag.best.Inverse());
+    frc::Pose3d altPose{};
+    if (camToTag.ambiguity != 0) {
+      altPose = knownTags[0].pose.TransformBy(camToTag.alt.Inverse());
+    }
+    frc::Pose3d o{};
+    PNPResult result{};
+    result.best = frc::Transform3d{o, bestPose};
+    result.alt = frc::Transform3d{o, altPose};
+    result.ambiguity = camToTag.ambiguity;
+    result.bestReprojErr = camToTag.bestReprojErr;
+    result.altReprojErr = camToTag.altReprojErr;
+    return result;
+  } else {
+    std::vector<frc::Translation3d> objectTrls{};
+    for (const auto& tag : knownTags) {
+      auto verts = tagModel.GetFieldVertices(tag.pose);
+      objectTrls.insert(objectTrls.end(), verts.begin(), verts.end());
+    }
+    PNPResult camToOrigin = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs,
+                                                       objectTrls, points);
+    if (!camToOrigin.isPresent) {
+      return PNPResult{};
+    } else {
+      PNPResult result{};
+      result.best = camToOrigin.best.Inverse(),
+      result.alt = camToOrigin.alt.Inverse(),
+      result.ambiguity = camToOrigin.ambiguity;
+      result.bestReprojErr = camToOrigin.bestReprojErr;
+      result.altReprojErr = camToOrigin.altReprojErr;
+      result.isPresent = true;
+      return result;
+    }
+  }
+}
+
+}  // namespace VisionEstimation
+}  // namespace photon
diff --git a/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h b/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h
new file mode 100644
index 0000000000..ff647cd7cf
--- /dev/null
+++ b/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <memory>
+
+#include <networktables/BooleanTopic.h>
+#include <networktables/DoubleArrayTopic.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/IntegerTopic.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/RawTopic.h>
+
+namespace photon {
+class NTTopicSet {
+ public:
+  std::shared_ptr<nt::NetworkTable> subTable;
+  nt::RawPublisher rawBytesEntry;
+
+  nt::IntegerPublisher pipelineIndexPublisher;
+  nt::IntegerSubscriber pipelineIndexRequestSub;
+
+  nt::BooleanTopic driverModeEntry;
+  nt::BooleanPublisher driverModePublisher;
+  nt::BooleanSubscriber driverModeSubscriber;
+
+  nt::DoublePublisher latencyMillisEntry;
+  nt::BooleanPublisher hasTargetEntry;
+  nt::DoublePublisher targetPitchEntry;
+  nt::DoublePublisher targetYawEntry;
+  nt::DoublePublisher targetAreaEntry;
+  nt::DoubleArrayPublisher targetPoseEntry;
+  nt::DoublePublisher targetSkewEntry;
+
+  nt::DoublePublisher bestTargetPosX;
+  nt::DoublePublisher bestTargetPosY;
+
+  nt::IntegerTopic heartbeatTopic;
+  nt::IntegerPublisher heartbeatPublisher;
+
+  nt::DoubleArrayPublisher cameraIntrinsicsPublisher;
+  nt::DoubleArrayPublisher cameraDistortionPublisher;
+
+  void UpdateEntries() {
+    nt::PubSubOptions options;
+    options.periodic = 0.01;
+    options.sendAll = true;
+    rawBytesEntry =
+        subTable->GetRawTopic("rawBytes").Publish("rawBytes", options);
+
+    pipelineIndexPublisher =
+        subTable->GetIntegerTopic("pipelineIndexState").Publish();
+    pipelineIndexRequestSub =
+        subTable->GetIntegerTopic("pipelineIndexRequest").Subscribe(0);
+
+    driverModePublisher = subTable->GetBooleanTopic("driverMode").Publish();
+    driverModeSubscriber =
+        subTable->GetBooleanTopic("driverModeRequest").Subscribe(0);
+
+    driverModeSubscriber.GetTopic().Publish().SetDefault(false);
+
+    latencyMillisEntry = subTable->GetDoubleTopic("latencyMillis").Publish();
+    hasTargetEntry = subTable->GetBooleanTopic("hasTargets").Publish();
+
+    targetPitchEntry = subTable->GetDoubleTopic("targetPitch").Publish();
+    targetAreaEntry = subTable->GetDoubleTopic("targetArea").Publish();
+    targetYawEntry = subTable->GetDoubleTopic("targetYaw").Publish();
+    targetPoseEntry = subTable->GetDoubleArrayTopic("targetPose").Publish();
+    targetSkewEntry = subTable->GetDoubleTopic("targetSkew").Publish();
+
+    bestTargetPosX = subTable->GetDoubleTopic("targetPixelsX").Publish();
+    bestTargetPosY = subTable->GetDoubleTopic("targetPixelsY").Publish();
+
+    heartbeatTopic = subTable->GetIntegerTopic("heartbeat");
+    heartbeatPublisher = heartbeatTopic.Publish();
+
+    cameraIntrinsicsPublisher =
+        subTable->GetDoubleArrayTopic("cameraIntrinsics").Publish();
+    cameraDistortionPublisher =
+        subTable->GetDoubleArrayTopic("cameraDistortion").Publish();
+  }
+
+ private:
+};
+}  // namespace photon
diff --git a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h
index e97f6c8d71..49f73ed53b 100644
--- a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h
+++ b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h
@@ -27,15 +27,15 @@ class PNPResult {
  public:
   // This could be wrapped in an std::optional, but chose to do it this way to
   // mirror Java
-  bool isPresent;
+  bool isPresent{false};
 
-  frc::Transform3d best;
-  double bestReprojErr;
+  frc::Transform3d best{};
+  double bestReprojErr{0};
 
-  frc::Transform3d alt;
-  double altReprojErr;
+  frc::Transform3d alt{};
+  double altReprojErr{0};
 
-  double ambiguity;
+  double ambiguity{0};
 
   bool operator==(const PNPResult& other) const;
 
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/.gitignore b/photonlib-cpp-examples/swervedriveposeestsim/.gitignore
new file mode 100644
index 0000000000..34878ab18c
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/.gitignore
@@ -0,0 +1 @@
+vendordeps
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json b/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json
new file mode 100644
index 0000000000..a6e62683de
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json
@@ -0,0 +1,6 @@
+{
+    "enableCppIntellisense": true,
+    "currentLanguage": "cpp",
+    "projectYear": "2023",
+    "teamNumber": 5
+}
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md b/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md
new file mode 100644
index 0000000000..3d5a824cad
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md
@@ -0,0 +1,24 @@
+Copyright (c) 2009-2021 FIRST and other WPILib contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+   * Neither the name of FIRST, WPILib, nor the names of other WPILib
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/build.gradle b/photonlib-cpp-examples/swervedriveposeestsim/build.gradle
new file mode 100644
index 0000000000..8eeef20126
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/build.gradle
@@ -0,0 +1,121 @@
+plugins {
+    id "cpp"
+    id "google-test-test-suite"
+    id "edu.wpi.first.GradleRIO"
+
+    id "com.dorongold.task-tree" version "2.1.0"
+}
+
+wpi.maven.useLocal = false
+wpi.maven.useDevelopment = true
+wpi.versions.wpilibVersion = '2024.1.1-beta-3-53-g31cd015'
+wpi.versions.wpimathVersion = '2024.1.1-beta-3-53-g31cd015'
+
+repositories {
+    mavenLocal()
+    jcenter()
+}
+
+apply from: "${rootDir}/../shared/examples_common.gradle"
+
+// Define my targets (RoboRIO) and artifacts (deployable files)
+// This is added by GradleRIO's backing project DeployUtils.
+deploy {
+    targets {
+        roborio(getTargetTypeClass('RoboRIO')) {
+            // Team number is loaded either from the .wpilib/wpilib_preferences.json
+            // or from command line. If not found an exception will be thrown.
+            // You can use getTeamOrDefault(team) instead of getTeamNumber if you
+            // want to store a team number in this file.
+            team = project.frc.getTeamOrDefault(5940)
+            debug = project.frc.getDebugOrDefault(false)
+
+            artifacts {
+                // First part is artifact name, 2nd is artifact type
+                // getTargetTypeClass is a shortcut to get the class type using a string
+
+                frcCpp(getArtifactTypeClass('FRCNativeArtifact')) {
+                }
+
+                // Static files artifact
+                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+                    files = project.fileTree('src/main/deploy')
+                    directory = '/home/lvuser/deploy'
+                }
+            }
+        }
+    }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcCpp
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = true
+
+// Set to true to run simulation in debug mode
+wpi.cpp.debugSimulation = false
+
+// Default enable simgui
+wpi.sim.addGui().defaultEnabled = true
+// Enable DS but not by default
+wpi.sim.addDriverstation()
+
+model {
+    components {
+        frcUserProgram(NativeExecutableSpec) {
+            // We don't need to build for roborio -- if we do, we need to install
+            // a roborio toolchain every time we build in CI
+            // Ideally, we'd be able to set the roborio toolchain as optional, but
+            // I can't figure out how to set that environment variable from build.gradle
+            // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71)
+            // for now, commented out
+
+            // targetPlatform wpi.platforms.roborio
+
+            if (includeDesktopSupport) {
+                targetPlatform wpi.platforms.desktop
+            }
+
+            sources.cpp {
+                source {
+                    srcDir 'src/main/cpp'
+                    include '**/*.cpp', '**/*.cc'
+                }
+                exportedHeaders {
+                    srcDir 'src/main/include'
+                }
+            }
+
+            // Set deploy task to deploy this component
+            deployArtifact.component = it
+
+            // Enable run tasks for this component
+            wpi.cpp.enableExternalTasks(it)
+
+            // Enable simulation for this component
+            wpi.sim.enable(it)
+            // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+            wpi.cpp.vendor.cpp(it)
+            wpi.cpp.deps.wpilib(it)
+        }
+    }
+    testSuites {
+        frcUserProgramTest(GoogleTestTestSuiteSpec) {
+            testing $.components.frcUserProgram
+
+            sources.cpp {
+                source {
+                    srcDir 'src/test/cpp'
+                    include '**/*.cpp'
+                }
+            }
+
+            // Enable run tasks for this component
+            wpi.cpp.enableExternalTasks(it)
+
+            wpi.cpp.vendor.cpp(it)
+            wpi.cpp.deps.wpilib(it)
+            wpi.cpp.deps.googleTest(it)
+        }
+    }
+}
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/gradlew b/photonlib-cpp-examples/swervedriveposeestsim/gradlew
new file mode 100644
index 0000000000..0ef4c1e860
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/gradlew
@@ -0,0 +1,241 @@
+#!/bin/sh
+
+#
+# Copyright © 2015-2021 the original authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+##############################################################################
+#
+#   Gradle start up script for POSIX generated by Gradle.
+#
+#   Important for running:
+#
+#   (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
+#       noncompliant, but you have some other compliant shell such as ksh or
+#       bash, then to run this script, type that shell name before the whole
+#       command line, like:
+#
+#           ksh Gradle
+#
+#       Busybox and similar reduced shells will NOT work, because this script
+#       requires all of these POSIX shell features:
+#         * functions;
+#         * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
+#           «${var#prefix}», «${var%suffix}», and «$( cmd )»;
+#         * compound commands having a testable exit status, especially «case»;
+#         * various built-in commands including «command», «set», and «ulimit».
+#
+#   Important for patching:
+#
+#   (2) This script targets any POSIX shell, so it avoids extensions provided
+#       by Bash, Ksh, etc; in particular arrays are avoided.
+#
+#       The "traditional" practice of packing multiple parameters into a
+#       space-separated string is a well documented source of bugs and security
+#       problems, so this is (mostly) avoided, by progressively accumulating
+#       options in "$@", and eventually passing that to Java.
+#
+#       Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
+#       and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
+#       see the in-line comments for details.
+#
+#       There are tweaks for specific operating systems such as AIX, CygWin,
+#       Darwin, MinGW, and NonStop.
+#
+#   (3) This script is generated from the Groovy template
+#       https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+#       within the Gradle project.
+#
+#       You can find Gradle at https://github.com/gradle/gradle/.
+#
+##############################################################################
+
+# Attempt to set APP_HOME
+
+# Resolve links: $0 may be a link
+app_path=$0
+
+# Need this for daisy-chained symlinks.
+while
+    APP_HOME=${app_path%"${app_path##*/}"}  # leaves a trailing /; empty if no leading path
+    [ -h "$app_path" ]
+do
+    ls=$( ls -ld "$app_path" )
+    link=${ls#*' -> '}
+    case $link in             #(
+      /*)   app_path=$link ;; #(
+      *)    app_path=$APP_HOME$link ;;
+    esac
+done
+
+# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036)
+APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit
+
+APP_NAME="Gradle"
+APP_BASE_NAME=${0##*/}
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD=maximum
+
+warn () {
+    echo "$*"
+} >&2
+
+die () {
+    echo
+    echo "$*"
+    echo
+    exit 1
+} >&2
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+nonstop=false
+case "$( uname )" in                #(
+  CYGWIN* )         cygwin=true  ;; #(
+  Darwin* )         darwin=true  ;; #(
+  MSYS* | MINGW* )  msys=true    ;; #(
+  NONSTOP* )        nonstop=true ;;
+esac
+
+CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
+
+
+# Determine the Java command to use to start the JVM.
+if [ -n "$JAVA_HOME" ] ; then
+    if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
+        # IBM's JDK on AIX uses strange locations for the executables
+        JAVACMD=$JAVA_HOME/jre/sh/java
+    else
+        JAVACMD=$JAVA_HOME/bin/java
+    fi
+    if [ ! -x "$JAVACMD" ] ; then
+        die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+    fi
+else
+    JAVACMD=java
+    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+fi
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+    case $MAX_FD in #(
+      max*)
+        MAX_FD=$( ulimit -H -n ) ||
+            warn "Could not query maximum file descriptor limit"
+    esac
+    case $MAX_FD in  #(
+      '' | soft) :;; #(
+      *)
+        ulimit -n "$MAX_FD" ||
+            warn "Could not set maximum file descriptor limit to $MAX_FD"
+    esac
+fi
+
+# Collect all arguments for the java command, stacking in reverse order:
+#   * args from the command line
+#   * the main class name
+#   * -classpath
+#   * -D...appname settings
+#   * --module-path (only if needed)
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
+
+# For Cygwin or MSYS, switch paths to Windows format before running java
+if "$cygwin" || "$msys" ; then
+    APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
+    CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
+
+    JAVACMD=$( cygpath --unix "$JAVACMD" )
+
+    # Now convert the arguments - kludge to limit ourselves to /bin/sh
+    for arg do
+        if
+            case $arg in                                #(
+              -*)   false ;;                            # don't mess with options #(
+              /?*)  t=${arg#/} t=/${t%%/*}              # looks like a POSIX filepath
+                    [ -e "$t" ] ;;                      #(
+              *)    false ;;
+            esac
+        then
+            arg=$( cygpath --path --ignore --mixed "$arg" )
+        fi
+        # Roll the args list around exactly as many times as the number of
+        # args, so each arg winds up back in the position where it started, but
+        # possibly modified.
+        #
+        # NB: a `for` loop captures its iteration list before it begins, so
+        # changing the positional parameters here affects neither the number of
+        # iterations, nor the values presented in `arg`.
+        shift                   # remove old arg
+        set -- "$@" "$arg"      # push replacement arg
+    done
+fi
+
+# Collect all arguments for the java command;
+#   * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
+#     shell script including quotes and variable substitutions, so put them in
+#     double quotes to make sure that they get re-expanded; and
+#   * put everything else in single quotes, so that it's not re-expanded.
+
+set -- \
+        "-Dorg.gradle.appname=$APP_BASE_NAME" \
+        -classpath "$CLASSPATH" \
+        org.gradle.wrapper.GradleWrapperMain \
+        "$@"
+
+# Stop when "xargs" is not available.
+if ! command -v xargs >/dev/null 2>&1
+then
+    die "xargs is not available"
+fi
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+#   readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+#   set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+        printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+        xargs -n1 |
+        sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+        tr '\n' ' '
+    )" '"$@"'
+
+exec "$JAVACMD" "$@"
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat b/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat
new file mode 100644
index 0000000000..f127cfd49d
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat
@@ -0,0 +1,91 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if %ERRORLEVEL% equ 0 goto execute
+
+echo.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo.
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if %ERRORLEVEL% equ 0 goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/networktables.json b/photonlib-cpp-examples/swervedriveposeestsim/networktables.json
new file mode 100644
index 0000000000..fe51488c70
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/networktables.json
@@ -0,0 +1 @@
+[]
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle b/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle
new file mode 100644
index 0000000000..44fbca7512
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle
@@ -0,0 +1,30 @@
+import org.gradle.internal.os.OperatingSystem
+
+rootProject.name = 'aimattarget'
+
+pluginManagement {
+    repositories {
+        mavenLocal()
+        jcenter()
+        gradlePluginPortal()
+        String frcYear = '2024'
+        File frcHome
+        if (OperatingSystem.current().isWindows()) {
+            String publicFolder = System.getenv('PUBLIC')
+            if (publicFolder == null) {
+                publicFolder = "C:\\Users\\Public"
+            }
+            def homeRoot = new File(publicFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        } else {
+            def userFolder = System.getProperty("user.home")
+            def homeRoot = new File(userFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        }
+        def frcHomeMaven = new File(frcHome, 'maven')
+        maven {
+            name 'frcHome'
+            url frcHomeMaven
+        }
+    }
+}
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json
new file mode 100644
index 0000000000..c4b7efd3d8
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json
@@ -0,0 +1,98 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
+    }
+  ]
+}
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json
new file mode 100644
index 0000000000..1605887709
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json
@@ -0,0 +1,64 @@
+{
+  "Docking": {
+    "Data": []
+  },
+  "MainWindow": {
+    "GLOBAL": {
+      "fps": "120",
+      "height": "910",
+      "maximized": "0",
+      "style": "0",
+      "userScale": "2",
+      "width": "1550",
+      "xpos": "-1602",
+      "ypos": "79"
+    }
+  },
+  "Window": {
+    "###/SmartDashboard/VisionSystemSim-main/Sim Field": {
+      "Collapsed": "0",
+      "Pos": "199,200",
+      "Size": "1342,628"
+    },
+    "###FMS": {
+      "Collapsed": "0",
+      "Pos": "5,540",
+      "Size": "283,146"
+    },
+    "###Joysticks": {
+      "Collapsed": "0",
+      "Pos": "359,95",
+      "Size": "796,240"
+    },
+    "###NetworkTables": {
+      "Collapsed": "0",
+      "Pos": "865,52",
+      "Size": "830,620"
+    },
+    "###Other Devices": {
+      "Collapsed": "0",
+      "Pos": "1025,20",
+      "Size": "250,695"
+    },
+    "###System Joysticks": {
+      "Collapsed": "0",
+      "Pos": "5,350",
+      "Size": "192,218"
+    },
+    "###Timing": {
+      "Collapsed": "0",
+      "Pos": "5,150",
+      "Size": "135,127"
+    },
+    "Debug##Default": {
+      "Collapsed": "0",
+      "Pos": "60,60",
+      "Size": "400,400"
+    },
+    "Robot State": {
+      "Collapsed": "0",
+      "Pos": "5,20",
+      "Size": "92,99"
+    }
+  }
+}
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui.json
new file mode 100644
index 0000000000..e1ba9acce8
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui.json
@@ -0,0 +1,57 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/VisionSystemSim-main/Sim Field": {
+        "EstimatedRobotModules": {
+          "arrows": false,
+          "image": "swerve_module.png",
+          "length": 0.30000001192092896,
+          "width": 0.30000001192092896
+        },
+        "apriltag": {
+          "image": "tag-green.png",
+          "length": 0.6000000238418579,
+          "width": 0.6000000238418579
+        },
+        "bottom": 544,
+        "builtin": "2023 Charged Up",
+        "cameras": {
+          "arrowSize": 19,
+          "arrowWeight": 1.0,
+          "style": "Hidden"
+        },
+        "height": 8.013679504394531,
+        "left": 46,
+        "right": 1088,
+        "top": 36,
+        "visibleTargetPoses": {
+          "image": "tag-blue.png"
+        },
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "SmartDashboard": {
+        "Drive": {
+          "open": true
+        },
+        "open": true
+      },
+      "photonvision": {
+        "open": true,
+        "photonvision": {
+          "open": true
+        }
+      }
+    }
+  }
+}
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp
new file mode 100644
index 0000000000..4505740955
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp
@@ -0,0 +1,123 @@
+/*
+ * 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 "Robot.h"
+
+#include <iostream>
+
+#include <frc/simulation/BatterySim.h>
+#include <frc/simulation/RoboRioSim.h>
+
+void Robot::RobotInit() {}
+
+void Robot::RobotPeriodic() {
+  drivetrain.Periodic();
+
+  auto visionEst = vision.GetEstimatedGlobalPose();
+  if (visionEst.has_value()) {
+    auto est = visionEst.value();
+    auto estPose = est.estimatedPose.ToPose2d();
+    auto estStdDevs = vision.GetEstimationStdDevs(estPose);
+    drivetrain.AddVisionMeasurement(est.estimatedPose.ToPose2d(), est.timestamp,
+                                    estStdDevs);
+  }
+
+  drivetrain.Log();
+}
+
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() { drivetrain.Stop(); }
+
+void Robot::DisabledExit() {}
+
+void Robot::AutonomousInit() {
+  autoTimer.Restart();
+  frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}};
+  drivetrain.ResetPose(pose, true);
+}
+
+void Robot::AutonomousPeriodic() {
+  if (autoTimer.Get() < 10_s) {
+    drivetrain.Drive(0.5_mps, 0.5_mps, 0.5_rad_per_s, false);
+  } else {
+    autoTimer.Stop();
+    drivetrain.Stop();
+  }
+}
+
+void Robot::AutonomousExit() {}
+
+void Robot::TeleopInit() {}
+
+void Robot::TeleopPeriodic() {
+  double forward = -controller.GetLeftY() * kDriveSpeed;
+  if (std::abs(forward) < 0.1) {
+    forward = 0;
+  }
+  forward = forwardLimiter.Calculate(forward);
+
+  double strafe = -controller.GetLeftX() * kDriveSpeed;
+  if (std::abs(strafe) < 0.1) {
+    strafe = 0;
+  }
+  strafe = strafeLimiter.Calculate(strafe);
+
+  double turn = -controller.GetRightX() * kDriveSpeed;
+  if (std::abs(turn) < 0.1) {
+    turn = 0;
+  }
+  turn = turnLimiter.Calculate(turn);
+
+  drivetrain.Drive(forward * constants::Swerve::kMaxLinearSpeed,
+                   strafe * constants::Swerve::kMaxLinearSpeed,
+                   turn * constants::Swerve::kMaxAngularSpeed, true);
+}
+
+void Robot::TeleopExit() {}
+
+void Robot::TestInit() {}
+
+void Robot::TestPeriodic() {}
+
+void Robot::TestExit() {}
+
+void Robot::SimulationPeriodic() {
+  drivetrain.SimulationPeriodic();
+  vision.SimPeriodic(drivetrain.GetSimPose());
+
+  frc::Field2d& debugField = vision.GetSimDebugField();
+  debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose());
+  debugField.GetObject("EstimatedRobotModules")
+      ->SetPoses(drivetrain.GetModulePoses());
+
+  units::ampere_t totalCurrent = drivetrain.GetCurrentDraw();
+  units::volt_t loadedBattVolts =
+      frc::sim::BatterySim::Calculate({totalCurrent});
+  frc::sim::RoboRioSim::SetVInVoltage(loadedBattVolts);
+}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp
new file mode 100644
index 0000000000..bf33d7189d
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp
@@ -0,0 +1,211 @@
+/*
+ * 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 "subsystems/SwerveDrive.h"
+
+#include <iostream>
+
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+SwerveDrive::SwerveDrive()
+    : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(),
+                    frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}),
+      gyroSim(gyro),
+      swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1),
+                     constants::Swerve::kDriveGearRatio,
+                     constants::Swerve::kWheelDiameter / 2,
+                     constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1),
+                     constants::Swerve::kSteerGearRatio, kinematics) {}
+
+void SwerveDrive::Periodic() {
+  for (auto& currentModule : swerveMods) {
+    currentModule.Periodic();
+  }
+
+  poseEstimator.Update(GetGyroYaw(), GetModulePositions());
+}
+
+void SwerveDrive::Drive(units::meters_per_second_t vx,
+                        units::meters_per_second_t vy,
+                        units::radians_per_second_t omega, bool openLoop) {
+  frc::ChassisSpeeds newChassisSpeeds =
+      frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading());
+  SetChassisSpeeds(newChassisSpeeds, openLoop, false);
+}
+
+void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds,
+                                   bool openLoop, bool steerInPlace) {
+  SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), openLoop,
+                  steerInPlace);
+  this->targetChassisSpeeds = newChassisSpeeds;
+}
+
+void SwerveDrive::SetModuleStates(
+    const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
+    bool steerInPlace) {
+  std::array<frc::SwerveModuleState, 4> desaturatedStates = desiredStates;
+  frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
+      static_cast<wpi::array<frc::SwerveModuleState, 4>*>(&desaturatedStates),
+      constants::Swerve::kMaxLinearSpeed);
+  for (int i = 0; i < swerveMods.size(); i++) {
+    swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace);
+  }
+}
+
+void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s, true); }
+
+void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
+                                       units::second_t timestamp) {
+  poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp);
+}
+
+void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
+                                       units::second_t timestamp,
+                                       const Eigen::Vector3d& stdDevs) {
+  wpi::array<double, 3> newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)};
+  poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp, newStdDevs);
+}
+
+void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) {
+  if (resetSimPose) {
+    swerveDriveSim.Reset(pose, false);
+    for (int i = 0; i < swerveMods.size(); i++) {
+      swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A);
+    }
+    gyroSim.SetAngle(-pose.Rotation().Degrees());
+    gyroSim.SetRate(0_rad_per_s);
+  }
+
+  poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose);
+}
+
+frc::Pose2d SwerveDrive::GetPose() const {
+  return poseEstimator.GetEstimatedPosition();
+}
+
+frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); }
+
+frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); }
+
+frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const {
+  return kinematics.ToChassisSpeeds(GetModuleStates());
+}
+
+std::array<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
+  std::array<frc::SwerveModuleState, 4> moduleStates;
+  moduleStates[0] = swerveMods[0].GetState();
+  moduleStates[1] = swerveMods[1].GetState();
+  moduleStates[2] = swerveMods[2].GetState();
+  moduleStates[3] = swerveMods[3].GetState();
+  return moduleStates;
+}
+
+std::array<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
+    const {
+  std::array<frc::SwerveModulePosition, 4> modulePositions;
+  modulePositions[0] = swerveMods[0].GetPosition();
+  modulePositions[1] = swerveMods[1].GetPosition();
+  modulePositions[2] = swerveMods[2].GetPosition();
+  modulePositions[3] = swerveMods[3].GetPosition();
+  return modulePositions;
+}
+
+std::array<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
+  std::array<frc::Pose2d, 4> modulePoses;
+  for (int i = 0; i < swerveMods.size(); i++) {
+    const SwerveModule& module = swerveMods[i];
+    modulePoses[i] = GetPose().TransformBy(frc::Transform2d{
+        module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()});
+  }
+  return modulePoses;
+}
+
+void SwerveDrive::Log() {
+  std::string table = "Drive/";
+  frc::Pose2d pose = GetPose();
+  frc::SmartDashboard::PutNumber(table + "X", pose.X().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Heading",
+                                 pose.Rotation().Degrees().to<double>());
+  frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
+  frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
+  frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Omega Degrees",
+      chassisSpeeds.omega.convert<units::degrees_per_second>().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Target VX",
+                                 targetChassisSpeeds.vx.to<double>());
+  frc::SmartDashboard::PutNumber(table + "Target VY",
+                                 targetChassisSpeeds.vy.to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Target Omega Degrees",
+      targetChassisSpeeds.omega.convert<units::degrees_per_second>()
+          .to<double>());
+
+  for (auto& module : swerveMods) {
+    module.Log();
+  }
+}
+
+void SwerveDrive::SimulationPeriodic() {
+  std::array<units::volt_t, 4> driveInputs;
+  std::array<units::volt_t, 4> steerInputs;
+  for (int i = 0; i < swerveMods.size(); i++) {
+    driveInputs[i] = swerveMods[i].GetDriveVoltage();
+    steerInputs[i] = swerveMods[i].GetSteerVoltage();
+  }
+  swerveDriveSim.SetDriveInputs(driveInputs);
+  swerveDriveSim.SetSteerInputs(steerInputs);
+
+  swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod);
+
+  auto driveStates = swerveDriveSim.GetDriveStates();
+  auto steerStates = swerveDriveSim.GetSteerStates();
+  totalCurrentDraw = 0_A;
+  std::array<units::ampere_t, 4> driveCurrents =
+      swerveDriveSim.GetDriveCurrentDraw();
+  for (const auto& current : driveCurrents) {
+    totalCurrentDraw += current;
+  }
+  std::array<units::ampere_t, 4> steerCurrents =
+      swerveDriveSim.GetSteerCurrentDraw();
+  for (const auto& current : steerCurrents) {
+    totalCurrentDraw += current;
+  }
+  for (int i = 0; i < swerveMods.size(); i++) {
+    units::meter_t drivePos{driveStates[i](0, 0)};
+    units::meters_per_second_t driveRate{driveStates[i](1, 0)};
+    units::radian_t steerPos{steerStates[i](0, 0)};
+    units::radians_per_second_t steerRate{steerStates[i](1, 0)};
+    swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i],
+                                   steerPos, steerRate, steerCurrents[i]);
+  }
+  gyroSim.SetRate(-swerveDriveSim.GetOmega());
+  gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees());
+}
+
+frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); }
+
+units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; }
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp
new file mode 100644
index 0000000000..31bbe09e61
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp
@@ -0,0 +1,285 @@
+/*
+ * 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 "subsystems/SwerveDriveSim.h"
+
+#include <iostream>
+
+#include <frc/RobotController.h>
+#include <frc/system/Discretization.h>
+
+template <typename T>
+int sgn(T val) {
+  return (T(0) < val) - (val < T(0));
+}
+
+SwerveDriveSim::SwerveDriveSim(
+    const frc::SimpleMotorFeedforward<units::meters>& driveFF,
+    const frc::DCMotor& driveMotor, double driveGearing,
+    units::meter_t driveWheelRadius,
+    const frc::SimpleMotorFeedforward<units::radians>& steerFF,
+    const frc::DCMotor& steerMotor, double steerGearing,
+    const frc::SwerveDriveKinematics<numModules>& kinematics)
+    : SwerveDriveSim(
+          frc::LinearSystem<2, 1, 2>{
+              (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
+               -driveFF.kV.to<double>() / driveFF.kA.to<double>())
+                  .finished(),
+              Eigen::Matrix<double, 2, 1>{0.0, 1.0 / driveFF.kA.to<double>()},
+              (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
+              Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
+          driveFF.kS, driveMotor, driveGearing, driveWheelRadius,
+          frc::LinearSystem<2, 1, 2>{
+              (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
+               -steerFF.kV.to<double>() / steerFF.kA.to<double>())
+                  .finished(),
+              Eigen::Matrix<double, 2, 1>{0.0, 1.0 / steerFF.kA.to<double>()},
+              (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
+              Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
+          steerFF.kS, steerMotor, steerGearing, kinematics) {}
+
+SwerveDriveSim::SwerveDriveSim(
+    const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs,
+    const frc::DCMotor& driveMotor, double driveGearing,
+    units::meter_t driveWheelRadius,
+    const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs,
+    const frc::DCMotor& steerMotor, double steerGearing,
+    const frc::SwerveDriveKinematics<numModules>& kinematics)
+    : drivePlant(drivePlant),
+      driveKs(driveKs),
+      driveMotor(driveMotor),
+      driveGearing(driveGearing),
+      driveWheelRadius(driveWheelRadius),
+      steerPlant(steerPlant),
+      steerKs(steerKs),
+      steerMotor(steerMotor),
+      steerGearing(steerGearing),
+      kinematics(kinematics) {}
+
+void SwerveDriveSim::SetDriveInputs(
+    const std::array<units::volt_t, numModules>& inputs) {
+  units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
+  for (int i = 0; i < driveInputs.size(); i++) {
+    units::volt_t input = inputs[i];
+    driveInputs[i] = std::clamp(input, -battVoltage, battVoltage);
+  }
+}
+
+void SwerveDriveSim::SetSteerInputs(
+    const std::array<units::volt_t, numModules>& inputs) {
+  units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
+  for (int i = 0; i < steerInputs.size(); i++) {
+    units::volt_t input = inputs[i];
+    steerInputs[i] = std::clamp(input, -battVoltage, battVoltage);
+  }
+}
+
+Eigen::Matrix<double, 2, 1> SwerveDriveSim::CalculateX(
+    const Eigen::Matrix<double, 2, 2>& discA,
+    const Eigen::Matrix<double, 2, 1>& discB,
+    const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
+    units::volt_t kS) {
+  auto Ax = discA * x;
+  double nextStateVel = Ax(1, 0);
+  double inputToStop = nextStateVel / -discB(1, 0);
+  double ksSystemEffect =
+      std::clamp(inputToStop, -kS.to<double>(), kS.to<double>());
+
+  nextStateVel += discB(1, 0) * ksSystemEffect;
+  inputToStop = nextStateVel / -discB(1, 0);
+  double signToStop = sgn(inputToStop);
+  double inputSign = sgn(input.to<double>());
+  double ksInputEffect = 0;
+
+  if (std::abs(ksSystemEffect) < kS.to<double>()) {
+    double absInput = std::abs(input.to<double>());
+    ksInputEffect =
+        -std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
+  } else if ((input.to<double>() * signToStop) > (inputToStop * signToStop)) {
+    double absInput = std::abs(input.to<double>() - inputToStop);
+    ksInputEffect =
+        -std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
+  }
+
+  auto sF = Eigen::Matrix<double, 1, 1>{input.to<double>() + ksSystemEffect +
+                                        ksInputEffect};
+  auto Bu = discB * sF;
+  auto retVal = Ax + Bu;
+  return retVal;
+}
+
+void SwerveDriveSim::Update(units::second_t dt) {
+  Eigen::Matrix<double, 2, 2> driveDiscA;
+  Eigen::Matrix<double, 2, 1> driveDiscB;
+  frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA,
+                          &driveDiscB);
+
+  Eigen::Matrix<double, 2, 2> steerDiscA;
+  Eigen::Matrix<double, 2, 1> steerDiscB;
+  frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA,
+                          &steerDiscB);
+
+  std::array<frc::SwerveModulePosition, 4> moduleDeltas;
+
+  for (int i = 0; i < numModules; i++) {
+    double prevDriveStatePos = driveStates[i](0, 0);
+    driveStates[i] = CalculateX(driveDiscA, driveDiscB, driveStates[i],
+                                driveInputs[i], driveKs);
+    double currentDriveStatePos = driveStates[i](0, 0);
+    steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i],
+                                steerInputs[i], steerKs);
+    double currentSteerStatePos = steerStates[i](0, 0);
+    moduleDeltas[i] = frc::SwerveModulePosition{
+        units::meter_t{currentDriveStatePos - prevDriveStatePos},
+        frc::Rotation2d{units::radian_t{currentSteerStatePos}}};
+  }
+
+  frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas);
+  pose = pose.Exp(twist);
+  omega = twist.dtheta / dt;
+}
+
+void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) {
+  this->pose = pose;
+  if (!preserveMotion) {
+    for (int i = 0; i < numModules; i++) {
+      driveStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
+      steerStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
+    }
+    omega = 0_rad_per_s;
+  }
+}
+
+void SwerveDriveSim::Reset(const frc::Pose2d& pose,
+                           const std::array<Eigen::Matrix<double, 2, 1>,
+                                            numModules>& moduleDriveStates,
+                           const std::array<Eigen::Matrix<double, 2, 1>,
+                                            numModules>& moduleSteerStates) {
+  this->pose = pose;
+  driveStates = moduleDriveStates;
+  steerStates = moduleSteerStates;
+  omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega;
+}
+
+frc::Pose2d SwerveDriveSim::GetPose() const { return pose; }
+
+std::array<frc::SwerveModulePosition, numModules>
+SwerveDriveSim::GetModulePositions() const {
+  std::array<frc::SwerveModulePosition, numModules> positions;
+  for (int i = 0; i < numModules; i++) {
+    positions[i] = frc::SwerveModulePosition{
+        units::meter_t{driveStates[i](0, 0)},
+        frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
+  }
+  return positions;
+}
+
+std::array<frc::SwerveModulePosition, numModules>
+SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev,
+                                        units::radian_t steerStdDev) {
+  std::array<frc::SwerveModulePosition, numModules> positions;
+  for (int i = 0; i < numModules; i++) {
+    positions[i] = frc::SwerveModulePosition{
+        units::meter_t{driveStates[i](0, 0)} +
+            randDist(generator) * driveStdDev,
+        frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} +
+                        randDist(generator) * steerStdDev}};
+  }
+  return positions;
+}
+
+std::array<frc::SwerveModuleState, numModules>
+SwerveDriveSim::GetModuleStates() {
+  std::array<frc::SwerveModuleState, numModules> states;
+  for (int i = 0; i < numModules; i++) {
+    states[i] = frc::SwerveModuleState{
+        units::meters_per_second_t{driveStates[i](1, 0)},
+        frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
+  }
+  return states;
+}
+
+std::array<Eigen::Matrix<double, 2, 1>, numModules>
+SwerveDriveSim::GetDriveStates() const {
+  return driveStates;
+}
+
+std::array<Eigen::Matrix<double, 2, 1>, numModules>
+SwerveDriveSim::GetSteerStates() const {
+  return steerStates;
+}
+
+units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; }
+
+units::ampere_t SwerveDriveSim::GetCurrentDraw(
+    const frc::DCMotor& motor, units::radians_per_second_t velocity,
+    units::volt_t inputVolts, units::volt_t batteryVolts) const {
+  units::volt_t effVolts = inputVolts - velocity / motor.Kv;
+  if (inputVolts >= 0_V) {
+    effVolts = std::clamp(effVolts, 0_V, inputVolts);
+  } else {
+    effVolts = std::clamp(effVolts, inputVolts, 0_V);
+  }
+  auto retVal = (inputVolts / batteryVolts) * (effVolts / motor.R);
+  return retVal;
+}
+
+std::array<units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
+    const {
+  std::array<units::ampere_t, numModules> currents;
+  for (int i = 0; i < numModules; i++) {
+    units::radians_per_second_t speed =
+        units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing /
+        driveWheelRadius.to<double>();
+    currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i],
+                                 frc::RobotController::GetBatteryVoltage());
+  }
+  return currents;
+}
+
+std::array<units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
+    const {
+  std::array<units::ampere_t, numModules> currents;
+  for (int i = 0; i < numModules; i++) {
+    units::radians_per_second_t speed =
+        units::radians_per_second_t{steerStates[i](1, 0) * steerGearing};
+    // TODO: If uncommented we get huge current values.. Not sure how to fix
+    // atm. :(
+    currents[i] = 20_A;
+    // currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i],
+    // frc::RobotController::GetBatteryVoltage());
+  }
+  return currents;
+}
+
+units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const {
+  units::ampere_t total{0};
+  for (const auto& val : GetDriveCurrentDraw()) {
+    total += val;
+  }
+  for (const auto& val : GetSteerCurrentDraw()) {
+    total += val;
+  }
+  return total;
+}
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp
new file mode 100644
index 0000000000..781c28a338
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp
@@ -0,0 +1,146 @@
+/*
+ * 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 "subsystems/SwerveModule.h"
+
+#include <iostream>
+
+#include <frc/MathUtil.h>
+#include <frc/RobotController.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts)
+    : moduleConstants(consts),
+      driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}),
+      driveEncoder(frc::Encoder{moduleConstants.driveEncoderA,
+                                moduleConstants.driveEncoderB}),
+      steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}),
+      steerEncoder(frc::Encoder{moduleConstants.steerEncoderA,
+                                moduleConstants.steerEncoderB}),
+      driveEncoderSim(driveEncoder),
+      steerEncoderSim(steerEncoder) {
+  driveEncoder.SetDistancePerPulse(
+      constants::Swerve::kDriveDistPerPulse.to<double>());
+  steerEncoder.SetDistancePerPulse(
+      constants::Swerve::kSteerRadPerPulse.to<double>());
+  steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi);
+}
+
+void SwerveModule::Periodic() {
+  units::volt_t steerPID = units::volt_t{
+      steerPIDController.Calculate(GetAbsoluteHeading().Radians().to<double>(),
+                                   desiredState.angle.Radians().to<double>())};
+  steerMotor.SetVoltage(steerPID);
+
+  units::volt_t driveFF =
+      constants::Swerve::kDriveFF.Calculate(desiredState.speed);
+  units::volt_t drivePID{0};
+  if (!openLoop) {
+    drivePID = units::volt_t{drivePIDController.Calculate(
+        driveEncoder.GetRate(), desiredState.speed.to<double>())};
+  }
+  driveMotor.SetVoltage(driveFF + drivePID);
+}
+
+void SwerveModule::SetDesiredState(const frc::SwerveModuleState& newState,
+                                   bool shouldBeOpenLoop, bool steerInPlace) {
+  frc::Rotation2d currentRotation = GetAbsoluteHeading();
+  frc::SwerveModuleState optimizedState =
+      frc::SwerveModuleState::Optimize(newState, currentRotation);
+  desiredState = optimizedState;
+}
+
+frc::Rotation2d SwerveModule::GetAbsoluteHeading() const {
+  return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}};
+}
+
+frc::SwerveModuleState SwerveModule::GetState() const {
+  return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps,
+                                GetAbsoluteHeading()};
+}
+
+frc::SwerveModulePosition SwerveModule::GetPosition() const {
+  return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m,
+                                   GetAbsoluteHeading()};
+}
+
+units::volt_t SwerveModule::GetDriveVoltage() const {
+  return driveMotor.Get() * frc::RobotController::GetBatteryVoltage();
+}
+
+units::volt_t SwerveModule::GetSteerVoltage() const {
+  return steerMotor.Get() * frc::RobotController::GetBatteryVoltage();
+}
+
+units::ampere_t SwerveModule::GetDriveCurrentSim() const {
+  return driveCurrentSim;
+}
+
+units::ampere_t SwerveModule::GetSteerCurrentSim() const {
+  return steerCurrentSim;
+}
+
+constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const {
+  return moduleConstants;
+}
+
+void SwerveModule::Log() {
+  frc::SwerveModuleState state = GetState();
+
+  std::string table =
+      "Module " + std::to_string(moduleConstants.moduleNum) + "/";
+  frc::SmartDashboard::PutNumber(table + "Steer Degrees",
+                                 frc::AngleModulus(state.angle.Radians())
+                                     .convert<units::degrees>()
+                                     .to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Steer Target Degrees",
+      units::radian_t{steerPIDController.GetSetpoint()}
+          .convert<units::degrees>()
+          .to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Drive Velocity Feet",
+      state.speed.convert<units::feet_per_second>().to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Drive Velocity Target Feet",
+      desiredState.speed.convert<units::feet_per_second>().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Drive Current",
+                                 driveCurrentSim.to<double>());
+  frc::SmartDashboard::PutNumber(table + "Steer Current",
+                                 steerCurrentSim.to<double>());
+}
+
+void SwerveModule::SimulationUpdate(
+    units::meter_t driveEncoderDist,
+    units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent,
+    units::radian_t steerEncoderDist,
+    units::radians_per_second_t steerEncoderRate,
+    units::ampere_t steerCurrent) {
+  driveEncoderSim.SetDistance(driveEncoderDist.to<double>());
+  driveEncoderSim.SetRate(driveEncoderRate.to<double>());
+  driveCurrentSim = driveCurrent;
+  steerEncoderSim.SetDistance(steerEncoderDist.to<double>());
+  steerEncoderSim.SetRate(steerEncoderRate.to<double>());
+  steerCurrentSim = steerCurrent;
+}
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt b/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt
new file mode 100644
index 0000000000..15bc5c1ebe
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt
@@ -0,0 +1,4 @@
+Files placed in this directory will be deployed to the RoboRIO into the
+  'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory'
+  function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy
+  directory.
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h
new file mode 100644
index 0000000000..70be37d406
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h
@@ -0,0 +1,118 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <numbers>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/apriltag/AprilTagFields.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/geometry/Transform3d.h>
+#include <frc/geometry/Translation2d.h>
+#include <units/length.h>
+
+namespace constants {
+namespace Vision {
+static constexpr std::string_view kCameraName{"YOUR CAMERA NAME"};
+static const frc::Transform3d kRobotToCam{
+    frc::Translation3d{0.5_m, 0.0_m, 0.5_m},
+    frc::Rotation3d{0_rad, 0_rad, 0_rad}};
+static const frc::AprilTagFieldLayout kTagLayout{
+    frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp)};
+
+static const Eigen::Matrix<double, 3, 1> kSingleTagStdDevs{4, 4, 8};
+static const Eigen::Matrix<double, 3, 1> kMultiTagStdDevs{0.5, 0.5, 1};
+}  // namespace Vision
+namespace Swerve {
+
+static constexpr units::meter_t kTrackWidth{18.5_in};
+static constexpr units::meter_t kTrackLength{18.5_in};
+static constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2};
+static constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2};
+static constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps};
+static constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s};
+static constexpr units::meter_t kWheelDiameter{4_in};
+static constexpr units::meter_t kWheelCircumference{kWheelDiameter *
+                                                    std::numbers::pi};
+
+static constexpr double kDriveGearRatio = 6.75;
+static constexpr double kSteerGearRatio = 12.8;
+
+static constexpr units::meter_t kDriveDistPerPulse =
+    kWheelCircumference / 1024.0 / kDriveGearRatio;
+static constexpr units::radian_t kSteerRadPerPulse =
+    units::radian_t{2 * std::numbers::pi} / 1024.0;
+
+static constexpr double kDriveKP = 1.0;
+static constexpr double kDriveKI = 0.0;
+static constexpr double kDriveKD = 0.0;
+
+static constexpr double kSteerKP = 20.0;
+static constexpr double kSteerKI = 0.0;
+static constexpr double kSteerKD = 0.25;
+
+static const frc::SimpleMotorFeedforward<units::meters> kDriveFF{
+    0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq};
+
+static const frc::SimpleMotorFeedforward<units::radians> kSteerFF{
+    0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq};
+
+struct ModuleConstants {
+ public:
+  const int moduleNum;
+  const int driveMotorId;
+  const int driveEncoderA;
+  const int driveEncoderB;
+  const int steerMotorId;
+  const int steerEncoderA;
+  const int steerEncoderB;
+  const double angleOffset;
+  const frc::Translation2d centerOffset;
+
+  ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA,
+                  int driveEncoderB, int steerMotorId, int steerEncoderA,
+                  int steerEncoderB, double angleOffset, units::meter_t xOffset,
+                  units::meter_t yOffset)
+      : moduleNum(moduleNum),
+        driveMotorId(driveMotorId),
+        driveEncoderA(driveEncoderA),
+        driveEncoderB(driveEncoderB),
+        steerMotorId(steerMotorId),
+        steerEncoderA(steerEncoderA),
+        steerEncoderB(steerEncoderB),
+        angleOffset(angleOffset),
+        centerOffset(frc::Translation2d{xOffset, yOffset}) {}
+};
+
+static const ModuleConstants FL_CONSTANTS{
+    1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2};
+static const ModuleConstants FR_CONSTANTS{
+    2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2};
+static const ModuleConstants BL_CONSTANTS{
+    3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2};
+static const ModuleConstants BR_CONSTANTS{
+    4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2};
+}  // namespace Swerve
+}  // namespace constants
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h
new file mode 100644
index 0000000000..7a3697494a
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h
@@ -0,0 +1,62 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc/Timer.h>
+#include <frc/XboxController.h>
+#include <frc/filter/SlewRateLimiter.h>
+
+#include "Vision.h"
+#include "subsystems/SwerveDrive.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void DisabledExit() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void AutonomousExit() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TeleopExit() override;
+  void TestInit() override;
+  void TestPeriodic() override;
+  void TestExit() override;
+  void SimulationPeriodic() override;
+
+ private:
+  SwerveDrive drivetrain{};
+  Vision vision{};
+  frc::XboxController controller{0};
+  frc::SlewRateLimiter<units::scalar> forwardLimiter{1.0 / 0.6_s};
+  frc::SlewRateLimiter<units::scalar> strafeLimiter{1.0 / 0.6_s};
+  frc::SlewRateLimiter<units::scalar> turnLimiter{1.0 / 0.33_s};
+  frc::Timer autoTimer{};
+  double kDriveSpeed{0.6};
+};
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h
new file mode 100644
index 0000000000..619b34ade7
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h
@@ -0,0 +1,152 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <photon/PhotonCamera.h>
+#include <photon/PhotonPoseEstimator.h>
+#include <photon/estimation/VisionEstimation.h>
+#include <photon/simulation/VisionSystemSim.h>
+#include <photon/simulation/VisionTargetSim.h>
+#include <photon/targeting/PhotonPipelineResult.h>
+
+#include <limits>
+#include <memory>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/apriltag/AprilTagFields.h>
+
+#include "Constants.h"
+
+class Vision {
+ public:
+  Vision() {
+    photonEstimator.SetMultiTagFallbackStrategy(
+        photon::PoseStrategy::LOWEST_AMBIGUITY);
+
+    if (frc::RobotBase::IsSimulation()) {
+      visionSim = std::make_unique<photon::VisionSystemSim>("main");
+
+      visionSim->AddAprilTags(constants::Vision::kTagLayout);
+
+      cameraProp = std::make_unique<photon::SimCameraProperties>();
+
+      cameraProp->SetCalibration(960, 720, frc::Rotation2d{90_deg});
+      cameraProp->SetCalibError(.35, .10);
+      cameraProp->SetFPS(15_Hz);
+      cameraProp->SetAvgLatency(50_ms);
+      cameraProp->SetLatencyStdDev(15_ms);
+
+      cameraSim = std::make_shared<photon::PhotonCameraSim>(camera.get(),
+                                                            *cameraProp.get());
+
+      visionSim->AddCamera(cameraSim.get(), robotToCam);
+      cameraSim->EnableDrawWireframe(true);
+    }
+  }
+
+  photon::PhotonPipelineResult GetLatestResult() {
+    return camera->GetLatestResult();
+  }
+
+  std::optional<photon::EstimatedRobotPose> GetEstimatedGlobalPose() {
+    auto visionEst = photonEstimator.Update();
+    units::second_t latestTimestamp = camera->GetLatestResult().GetTimestamp();
+    bool newResult =
+        units::math::abs(latestTimestamp - lastEstTimestamp) > 1e-5_s;
+    if (frc::RobotBase::IsSimulation()) {
+      if (visionEst.has_value()) {
+        GetSimDebugField()
+            .GetObject("VisionEstimation")
+            ->SetPose(visionEst.value().estimatedPose.ToPose2d());
+      } else {
+        if (newResult) {
+          GetSimDebugField().GetObject("VisionEstimation")->SetPoses({});
+        }
+      }
+    }
+    if (newResult) {
+      lastEstTimestamp = latestTimestamp;
+    }
+    return visionEst;
+  }
+
+  Eigen::Matrix<double, 3, 1> GetEstimationStdDevs(frc::Pose2d estimatedPose) {
+    Eigen::Matrix<double, 3, 1> estStdDevs =
+        constants::Vision::kSingleTagStdDevs;
+    auto targets = GetLatestResult().GetTargets();
+    int numTags = 0;
+    units::meter_t avgDist = 0_m;
+    for (const auto& tgt : targets) {
+      auto tagPose =
+          photonEstimator.GetFieldLayout().GetTagPose(tgt.GetFiducialId());
+      if (tagPose.has_value()) {
+        numTags++;
+        avgDist += tagPose.value().ToPose2d().Translation().Distance(
+            estimatedPose.Translation());
+      }
+    }
+    if (numTags == 0) {
+      return estStdDevs;
+    }
+    avgDist /= numTags;
+    if (numTags > 1) {
+      estStdDevs = constants::Vision::kMultiTagStdDevs;
+    }
+    if (numTags == 1 && avgDist > 4_m) {
+      estStdDevs = (Eigen::MatrixXd(3, 1) << std::numeric_limits<double>::max(),
+                    std::numeric_limits<double>::max(),
+                    std::numeric_limits<double>::max())
+                       .finished();
+    } else {
+      estStdDevs = estStdDevs * (1 + (avgDist.value() * avgDist.value() / 30));
+    }
+    return estStdDevs;
+  }
+
+  void SimPeriodic(frc::Pose2d robotSimPose) {
+    visionSim->Update(robotSimPose);
+  }
+
+  void ResetSimPose(frc::Pose2d pose) {
+    if (frc::RobotBase::IsSimulation()) {
+      visionSim->ResetRobotPose(pose);
+    }
+  }
+
+  frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); }
+
+ private:
+  frc::Transform3d robotToCam{frc::Translation3d{0.5_m, 0.5_m, 0.5_m},
+                              frc::Rotation3d{}};
+  photon::PhotonPoseEstimator photonEstimator{
+      LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp),
+      photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR,
+      photon::PhotonCamera{"photonvision"}, robotToCam};
+  std::shared_ptr<photon::PhotonCamera> camera{photonEstimator.GetCamera()};
+  std::unique_ptr<photon::VisionSystemSim> visionSim;
+  std::unique_ptr<photon::SimCameraProperties> cameraProp;
+  std::shared_ptr<photon::PhotonCameraSim> cameraSim;
+  units::second_t lastEstTimestamp{0_s};
+};
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h
new file mode 100644
index 0000000000..1e3d26b32f
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h
@@ -0,0 +1,85 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <frc/ADXRS450_Gyro.h>
+#include <frc/SPI.h>
+#include <frc/estimator/SwerveDrivePoseEstimator.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/simulation/ADXRS450_GyroSim.h>
+
+#include "SwerveDriveSim.h"
+#include "SwerveModule.h"
+
+class SwerveDrive {
+ public:
+  SwerveDrive();
+  void Periodic();
+  void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy,
+             units::radians_per_second_t omega, bool openLoop);
+  void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds,
+                        bool openLoop, bool steerInPlace);
+  void SetModuleStates(
+      const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
+      bool steerInPlace);
+  void Stop();
+  void AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
+                            units::second_t timestamp);
+  void AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
+                            units::second_t timestamp,
+                            const Eigen::Vector3d& stdDevs);
+  void ResetPose(const frc::Pose2d& pose, bool resetSimPose);
+  frc::Pose2d GetPose() const;
+  frc::Rotation2d GetHeading() const;
+  frc::Rotation2d GetGyroYaw() const;
+  frc::ChassisSpeeds GetChassisSpeeds() const;
+  std::array<frc::SwerveModuleState, 4> GetModuleStates() const;
+  std::array<frc::SwerveModulePosition, 4> GetModulePositions() const;
+  std::array<frc::Pose2d, 4> GetModulePoses() const;
+  void Log();
+  void SimulationPeriodic();
+  frc::Pose2d GetSimPose() const;
+  units::ampere_t GetCurrentDraw() const;
+
+ private:
+  std::array<SwerveModule, 4> swerveMods{
+      SwerveModule{constants::Swerve::FL_CONSTANTS},
+      SwerveModule{constants::Swerve::FR_CONSTANTS},
+      SwerveModule{constants::Swerve::BL_CONSTANTS},
+      SwerveModule{constants::Swerve::BR_CONSTANTS}};
+  frc::SwerveDriveKinematics<4> kinematics{
+      swerveMods[0].GetModuleConstants().centerOffset,
+      swerveMods[1].GetModuleConstants().centerOffset,
+      swerveMods[2].GetModuleConstants().centerOffset,
+      swerveMods[3].GetModuleConstants().centerOffset,
+  };
+  frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0};
+  frc::SwerveDrivePoseEstimator<4> poseEstimator;
+  frc::ChassisSpeeds targetChassisSpeeds{};
+
+  frc::sim::ADXRS450_GyroSim gyroSim;
+  SwerveDriveSim swerveDriveSim;
+  units::ampere_t totalCurrentDraw{0};
+};
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h
new file mode 100644
index 0000000000..c1cee3e6f1
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h
@@ -0,0 +1,102 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <random>
+
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/system/LinearSystem.h>
+#include <frc/system/plant/DCMotor.h>
+#include <units/voltage.h>
+
+static constexpr int numModules{4};
+
+class SwerveDriveSim {
+ public:
+  SwerveDriveSim(const frc::SimpleMotorFeedforward<units::meters>& driveFF,
+                 const frc::DCMotor& driveMotor, double driveGearing,
+                 units::meter_t driveWheelRadius,
+                 const frc::SimpleMotorFeedforward<units::radians>& steerFF,
+                 const frc::DCMotor& steerMotor, double steerGearing,
+                 const frc::SwerveDriveKinematics<numModules>& kinematics);
+  SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant,
+                 units::volt_t driveKs, const frc::DCMotor& driveMotor,
+                 double driveGearing, units::meter_t driveWheelRadius,
+                 const frc::LinearSystem<2, 1, 2>& steerPlant,
+                 units::volt_t steerKs, const frc::DCMotor& steerMotor,
+                 double steerGearing,
+                 const frc::SwerveDriveKinematics<numModules>& kinematics);
+  void SetDriveInputs(const std::array<units::volt_t, numModules>& inputs);
+  void SetSteerInputs(const std::array<units::volt_t, numModules>& inputs);
+  static Eigen::Matrix<double, 2, 1> CalculateX(
+      const Eigen::Matrix<double, 2, 2>& discA,
+      const Eigen::Matrix<double, 2, 1>& discB,
+      const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
+      units::volt_t kS);
+  void Update(units::second_t dt);
+  void Reset(const frc::Pose2d& pose, bool preserveMotion);
+  void Reset(const frc::Pose2d& pose,
+             const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
+                 moduleDriveStates,
+             const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
+                 moduleSteerStates);
+  frc::Pose2d GetPose() const;
+  std::array<frc::SwerveModulePosition, numModules> GetModulePositions() const;
+  std::array<frc::SwerveModulePosition, numModules> GetNoisyModulePositions(
+      units::meter_t driveStdDev, units::radian_t steerStdDev);
+  std::array<frc::SwerveModuleState, numModules> GetModuleStates();
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> GetDriveStates() const;
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> GetSteerStates() const;
+  units::radians_per_second_t GetOmega() const;
+  units::ampere_t GetCurrentDraw(const frc::DCMotor& motor,
+                                 units::radians_per_second_t velocity,
+                                 units::volt_t inputVolts,
+                                 units::volt_t batteryVolts) const;
+  std::array<units::ampere_t, numModules> GetDriveCurrentDraw() const;
+  std::array<units::ampere_t, numModules> GetSteerCurrentDraw() const;
+  units::ampere_t GetTotalCurrentDraw() const;
+
+ private:
+  std::random_device rd{};
+  std::mt19937 generator{rd()};
+  std::normal_distribution<double> randDist{0.0, 1.0};
+  const frc::LinearSystem<2, 1, 2> drivePlant;
+  const units::volt_t driveKs;
+  const frc::DCMotor driveMotor;
+  const double driveGearing;
+  const units::meter_t driveWheelRadius;
+  const frc::LinearSystem<2, 1, 2> steerPlant;
+  const units::volt_t steerKs;
+  const frc::DCMotor steerMotor;
+  const double steerGearing;
+  const frc::SwerveDriveKinematics<numModules> kinematics;
+  std::array<units::volt_t, numModules> driveInputs{};
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> driveStates{};
+  std::array<units::volt_t, numModules> steerInputs{};
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> steerStates{};
+  frc::Pose2d pose{frc::Pose2d{}};
+  units::radians_per_second_t omega{0};
+};
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h
new file mode 100644
index 0000000000..444e4bdf44
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h
@@ -0,0 +1,81 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/controller/PIDController.h>
+#include <frc/kinematics/SwerveModulePosition.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/EncoderSim.h>
+#include <units/current.h>
+
+#include "Constants.h"
+
+class SwerveModule {
+ public:
+  explicit SwerveModule(const constants::Swerve::ModuleConstants& consts);
+  void Periodic();
+  void SetDesiredState(const frc::SwerveModuleState& newState,
+                       bool shouldBeOpenLoop, bool steerInPlace);
+  frc::Rotation2d GetAbsoluteHeading() const;
+  frc::SwerveModuleState GetState() const;
+  frc::SwerveModulePosition GetPosition() const;
+  units::volt_t GetDriveVoltage() const;
+  units::volt_t GetSteerVoltage() const;
+  units::ampere_t GetDriveCurrentSim() const;
+  units::ampere_t GetSteerCurrentSim() const;
+  constants::Swerve::ModuleConstants GetModuleConstants() const;
+  void Log();
+  void SimulationUpdate(units::meter_t driveEncoderDist,
+                        units::meters_per_second_t driveEncoderRate,
+                        units::ampere_t driveCurrent,
+                        units::radian_t steerEncoderDist,
+                        units::radians_per_second_t steerEncoderRate,
+                        units::ampere_t steerCurrent);
+
+ private:
+  const constants::Swerve::ModuleConstants moduleConstants;
+
+  frc::PWMSparkMax driveMotor;
+  frc::Encoder driveEncoder;
+  frc::PWMSparkMax steerMotor;
+  frc::Encoder steerEncoder;
+
+  frc::SwerveModuleState desiredState{};
+  bool openLoop{false};
+
+  frc::PIDController drivePIDController{constants::Swerve::kDriveKP,
+                                        constants::Swerve::kDriveKI,
+                                        constants::Swerve::kDriveKD};
+  frc::PIDController steerPIDController{constants::Swerve::kSteerKP,
+                                        constants::Swerve::kSteerKI,
+                                        constants::Swerve::kSteerKD};
+
+  frc::sim::EncoderSim driveEncoderSim;
+  units::ampere_t driveCurrentSim{0};
+  frc::sim::EncoderSim steerEncoderSim;
+  units::ampere_t steerCurrentSim{0};
+};
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp
new file mode 100644
index 0000000000..031d1ce96b
--- /dev/null
+++ b/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp
@@ -0,0 +1,34 @@
+/*
+ * 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 <hal/HAL.h>
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png b/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png
new file mode 100644
index 0000000000..25990c8399
Binary files /dev/null and b/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png differ
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png b/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png
new file mode 100644
index 0000000000..04b9e4f7b7
Binary files /dev/null and b/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png differ
diff --git a/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png b/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png
new file mode 100644
index 0000000000..63029fcf20
Binary files /dev/null and b/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png differ
diff --git a/shared/javacpp/setupBuild.gradle b/shared/javacpp/setupBuild.gradle
index 66106c6057..dd8a5bec32 100644
--- a/shared/javacpp/setupBuild.gradle
+++ b/shared/javacpp/setupBuild.gradle
@@ -88,6 +88,8 @@ model {
                 }
             }
 
+            nativeUtils.useRequiredLibrary(it, "cscore_shared")
+            nativeUtils.useRequiredLibrary(it, "cameraserver_shared")
             nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared")
             nativeUtils.useRequiredLibrary(it, "googletest_static")
             nativeUtils.useRequiredLibrary(it, "apriltag_shared")