[py] Add wpimath/kinematics tests (#8461)

Used Gemini to convert the C++ tests in wpimath/.../kinematics to pytest. For ease of use required a better constructor for `MecanumDriveWheelPositions` I did this conversion months ago (before robotpy landed and before the reorg), so new tests might be missing. At least its better than nothing 🤷 Broke the huge pr that does almost everything into parts so its easier to review. --------- Co-authored-by: David Vo <auscompgeek@users.noreply.github.com>
2026-06-19 00:41:43 +00:00 · 2026-01-12 22:14:14 -05:00
parent c2b339cfc9
commit eccc2301ac
15 changed files with 2061 additions and 1 deletions
--- a/wpimath/src/main/python/semiwrap/MecanumDriveWheelPositions.yml
+++ b/wpimath/src/main/python/semiwrap/MecanumDriveWheelPositions.yml
@@ -1,5 +1,6 @@
 classes:
  wpi::math::MecanumDriveWheelPositions:
+    force_no_default_constructor: true
    attributes:
      frontLeft:
      frontRight:
@@ -8,3 +9,10 @@ classes:
    methods:
      operator==:
      Interpolate:
+    inline_code: |
+      .def(
+        py::init<
+          wpi::units::meter_t, wpi::units::meter_t, wpi::units::meter_t, wpi::units::meter_t
+        >(),
+        py::arg("frontLeft") = 0, py::arg("frontRight") = 0, py::arg("rearLeft") = 0, py::arg("rearRight") = 0
+      )
--- a/wpimath/src/test/python/kinematics/test_chassis_speeds.py
+++ b/wpimath/src/test/python/kinematics/test_chassis_speeds.py
@@ -1,6 +1,49 @@
+import pytest
 import math
+import numpy as np

-from wpimath import ChassisSpeeds
+from wpimath import ChassisSpeeds, Twist2d, Pose2d, Rotation2d
+
+
+def test_discretize():
+    target = ChassisSpeeds(vx=1, vy=0, omega=0.5)
+    duration = 1
+    dt = 0.01
+
+    speeds = target.discretize(duration)
+    twist = Twist2d(speeds.vx * dt, speeds.vy * dt, speeds.omega * dt)
+
+    pose = Pose2d()
+    time = 0
+    while time < duration:
+        pose = pose + twist.exp()
+        time += dt
+
+    assert pose.x == pytest.approx((target.vx * duration), abs=1e-9)
+    assert pose.y == pytest.approx((target.vy * duration), abs=1e-9)
+    assert pose.rotation().radians() == pytest.approx(
+        (target.omega * duration), abs=1e-9
+    )
+
+
+def test_to_robot_relative():
+    chassis_speeds = ChassisSpeeds(vx=1, vy=0, omega=0.5).toRobotRelative(
+        Rotation2d.fromDegrees(-90)
+    )
+
+    assert chassis_speeds.vx == pytest.approx(0.0, abs=1e-9)
+    assert chassis_speeds.vy == pytest.approx(1.0, abs=1e-9)
+    assert chassis_speeds.omega == pytest.approx(0.5, abs=1e-9)
+
+
+def test_to_field_relative():
+    chassis_speeds = ChassisSpeeds(vx=1, vy=0, omega=0.5).toFieldRelative(
+        Rotation2d.fromDegrees(45)
+    )
+
+    assert chassis_speeds.vx == pytest.approx(1.0 / math.sqrt(2.0), abs=1e-9)
+    assert chassis_speeds.vy == pytest.approx(1.0 / math.sqrt(2.0), abs=1e-9)
+    assert chassis_speeds.omega == pytest.approx(0.5, abs=1e-9)


 def test_plus() -> None:
--- a/wpimath/src/test/python/kinematics/test_differential_drive_kinematics.py
+++ b/wpimath/src/test/python/kinematics/test_differential_drive_kinematics.py
@@ -0,0 +1,69 @@
+import pytest
+import math
+import numpy as np
+
+from wpimath import (
+    ChassisSpeeds,
+    DifferentialDriveKinematics,
+    DifferentialDriveWheelSpeeds,
+    Rotation2d,
+)
+
+
+def test_inverse_kinematics_from_zero():
+    kinematics = DifferentialDriveKinematics(trackwidth=0.381 * 2)
+    chassis_speeds = ChassisSpeeds()
+    wheel_speeds = kinematics.toWheelSpeeds(chassis_speeds)
+
+    assert wheel_speeds.left == pytest.approx(0, abs=1e-9)
+    assert wheel_speeds.right == pytest.approx(0, abs=1e-9)
+
+
+def test_forward_kinematics_from_zero():
+    kinematics = DifferentialDriveKinematics(trackwidth=0.381 * 2)
+    wheel_speeds = DifferentialDriveWheelSpeeds()
+    chassis_speeds = kinematics.toChassisSpeeds(wheel_speeds)
+
+    assert chassis_speeds.vx == pytest.approx(0, abs=1e-9)
+    assert chassis_speeds.vy == pytest.approx(0, abs=1e-9)
+    assert chassis_speeds.omega == pytest.approx(0, abs=1e-9)
+
+
+def test_inverse_kinematics_for_straight_line():
+    kinematics = DifferentialDriveKinematics(trackwidth=(0.381 * 2))
+    chassis_speeds = ChassisSpeeds(vx=3.0, vy=0, omega=0)
+    wheel_speeds = kinematics.toWheelSpeeds(chassis_speeds)
+
+    assert wheel_speeds.left == pytest.approx(3, abs=1e-9)
+    assert wheel_speeds.right == pytest.approx(3, abs=1e-9)
+
+
+def test_forward_kinematics_for_straight_line():
+    kinematics = DifferentialDriveKinematics(trackwidth=0.381 * 2)
+    wheel_speeds = DifferentialDriveWheelSpeeds(left=3.0, right=3.0)
+    chassis_speeds = kinematics.toChassisSpeeds(wheel_speeds)
+
+    assert chassis_speeds.vx == pytest.approx(3, abs=1e-9)
+    assert chassis_speeds.vy == pytest.approx(0, abs=1e-9)
+    assert chassis_speeds.omega == pytest.approx(0, abs=1e-9)
+
+
+def test_inverse_kinematics_for_rotate_in_place():
+    kinematics = DifferentialDriveKinematics(trackwidth=0.381 * 2)
+    chassis_speeds = ChassisSpeeds(vx=0.0, vy=0.0, omega=math.pi)
+    wheel_speeds = kinematics.toWheelSpeeds(chassis_speeds)
+
+    assert wheel_speeds.left == pytest.approx(-0.381 * math.pi, abs=1e-9)
+    assert wheel_speeds.right == pytest.approx(0.381 * math.pi, abs=1e-9)
+
+
+def test_forward_kinematics_for_rotate_in_place():
+    kinematics = DifferentialDriveKinematics(trackwidth=0.381 * 2)
+    wheel_speeds = DifferentialDriveWheelSpeeds(
+        left=0.381 * math.pi, right=-0.381 * math.pi
+    )
+    chassis_speeds = kinematics.toChassisSpeeds(wheel_speeds)
+
+    assert chassis_speeds.vx == pytest.approx(0, abs=1e-9)
+    assert chassis_speeds.vy == pytest.approx(0, abs=1e-9)
+    assert chassis_speeds.omega == pytest.approx(-math.pi, abs=1e-9)
--- a/wpimath/src/test/python/kinematics/test_differential_drive_odometry.py
+++ b/wpimath/src/test/python/kinematics/test_differential_drive_odometry.py
@@ -0,0 +1,18 @@
+import pytest
+import math
+
+from wpimath import DifferentialDriveOdometry, Rotation2d
+
+
+def test_encoder_distances():
+    odometry = DifferentialDriveOdometry(
+        gyroAngle=Rotation2d.fromDegrees(45), leftDistance=0, rightDistance=0
+    )
+
+    pose = odometry.update(
+        gyroAngle=Rotation2d.fromDegrees(135), rightDistance=0, leftDistance=5 * math.pi
+    )
+
+    assert pose.x == pytest.approx(5.0, abs=1e-9)
+    assert pose.y == pytest.approx(5.0, abs=1e-9)
+    assert pose.rotation().degrees() == pytest.approx(90.0, abs=1e-9)
--- a/wpimath/src/test/python/kinematics/test_differential_drive_odometry3d.py
+++ b/wpimath/src/test/python/kinematics/test_differential_drive_odometry3d.py
@@ -0,0 +1,33 @@
+import pytest
+import math
+
+from wpimath import DifferentialDriveOdometry3d, Rotation3d, Pose3d, Rotation2d
+
+
+def test_initialize():
+    odometry = DifferentialDriveOdometry3d(
+        gyroAngle=Rotation3d.fromDegrees(0, 0, 90),
+        leftDistance=0,
+        rightDistance=0,
+        initialPose=Pose3d(x=1, y=2, z=0, rotation=Rotation3d.fromDegrees(0, 0, 45)),
+    )
+
+    pose = odometry.getPose()
+
+    assert pose.x == pytest.approx(1, abs=1e-9)
+    assert pose.y == pytest.approx(2, abs=1e-9)
+    assert pose.z == pytest.approx(0, abs=1e-9)
+    assert pose.rotation().toRotation2d().degrees() == pytest.approx(45, abs=1e-9)
+
+
+def test_encoder_distances():
+    odometry = DifferentialDriveOdometry3d(
+        gyroAngle=Rotation3d.fromDegrees(0, 0, 45), leftDistance=0, rightDistance=0
+    )
+
+    pose = odometry.update(Rotation3d.fromDegrees(0, 0, 135), 0, 5 * math.pi)
+
+    assert pose.x == pytest.approx(5.0, abs=1e-9)
+    assert pose.y == pytest.approx(5.0, abs=1e-9)
+    assert pose.z == pytest.approx(0.0, abs=1e-9)
+    assert pose.rotation().toRotation2d().degrees() == pytest.approx(90.0, abs=1e-9)
--- a/wpimath/src/test/python/kinematics/test_differential_drive_wheel_speeds.py
+++ b/wpimath/src/test/python/kinematics/test_differential_drive_wheel_speeds.py
@@ -0,0 +1,50 @@
+import pytest
+
+from wpimath import DifferentialDriveWheelSpeeds
+
+
+def test_plus():
+    left = DifferentialDriveWheelSpeeds(left=1.0, right=0.5)
+    right = DifferentialDriveWheelSpeeds(left=2.0, right=1.5)
+
+    result = left + right
+
+    assert result.left == 3.0
+    assert result.right == 2.0
+
+
+def test_minus():
+    left = DifferentialDriveWheelSpeeds(left=1.0, right=0.5)
+    right = DifferentialDriveWheelSpeeds(left=2.0, right=0.5)
+
+    result = left - right
+
+    assert result.left == -1.0
+    assert result.right == 0.0
+
+
+def test_unary_minus():
+    speeds = DifferentialDriveWheelSpeeds(left=1.0, right=0.5)
+
+    result = -speeds
+
+    assert result.left == -1.0
+    assert result.right == -0.5
+
+
+def test_multiplication():
+    speeds = DifferentialDriveWheelSpeeds(left=1.0, right=0.5)
+
+    result = speeds * 2
+
+    assert result.left == 2.0
+    assert result.right == 1.0
+
+
+def test_division():
+    speeds = DifferentialDriveWheelSpeeds(left=1.0, right=0.5)
+
+    result = speeds / 2
+
+    assert result.left == 0.5
+    assert result.right == 0.25
--- a/wpimath/src/test/python/kinematics/test_mecanum_drive_kinematics.py
+++ b/wpimath/src/test/python/kinematics/test_mecanum_drive_kinematics.py
@@ -0,0 +1,270 @@
+import pytest
+import math
+
+from wpimath import (
+    MecanumDriveKinematics,
+    ChassisSpeeds,
+    MecanumDriveWheelSpeeds,
+    MecanumDriveWheelPositions,
+    Rotation2d,
+    Translation2d,
+)
+
+
+@pytest.fixture
+def kinematics_test():
+    class MecanumDriveKinematicsTest:
+        def __init__(self):
+            self.m_fl = Translation2d(x=12, y=12)
+            self.m_fr = Translation2d(x=12, y=-12)
+            self.m_bl = Translation2d(x=-12, y=12)
+            self.m_br = Translation2d(x=-12, y=-12)
+            self.kinematics = MecanumDriveKinematics(
+                self.m_fl, self.m_fr, self.m_bl, self.m_br
+            )
+
+    return MecanumDriveKinematicsTest()
+
+
+def test_straight_line_inverse_kinematics(kinematics_test):
+    speeds = ChassisSpeeds(vx=5, vy=0, omega=0)
+    module_states = kinematics_test.kinematics.toWheelSpeeds(speeds)
+
+    assert module_states.frontLeft == pytest.approx(5.0, abs=0.1)
+    assert module_states.frontRight == pytest.approx(5.0, abs=0.1)
+    assert module_states.rearLeft == pytest.approx(5.0, abs=0.1)
+    assert module_states.rearRight == pytest.approx(5.0, abs=0.1)
+
+
+def test_straight_line_forward_kinematics(kinematics_test):
+    wheel_speeds = MecanumDriveWheelSpeeds(
+        frontLeft=5, frontRight=5, rearLeft=5, rearRight=5
+    )
+    chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
+
+    assert chassis_speeds.vx == pytest.approx(5.0, abs=0.1)
+    assert chassis_speeds.vy == pytest.approx(0.0, abs=0.1)
+    assert chassis_speeds.omega == pytest.approx(0.0, abs=0.1)
+
+
+def test_straight_line_forward_kinematics_with_deltas(kinematics_test):
+    wheel_deltas = MecanumDriveWheelPositions(
+        frontLeft=5, frontRight=5, rearLeft=5, rearRight=5
+    )
+    twist = kinematics_test.kinematics.toTwist2d(wheel_deltas)
+
+    assert twist.dx == pytest.approx(5.0, abs=0.1)
+    assert twist.dy == pytest.approx(0.0, abs=0.1)
+    assert twist.dtheta == pytest.approx(0.0, abs=0.1)
+
+
+def test_strafe_inverse_kinematics(kinematics_test):
+    speeds = ChassisSpeeds(vx=0, vy=4, omega=0)
+    module_states = kinematics_test.kinematics.toWheelSpeeds(speeds)
+
+    assert module_states.frontLeft == pytest.approx(-4.0, abs=0.1)
+    assert module_states.frontRight == pytest.approx(4.0, abs=0.1)
+    assert module_states.rearLeft == pytest.approx(4.0, abs=0.1)
+    assert module_states.rearRight == pytest.approx(-4.0, abs=0.1)
+
+
+def test_strafe_forward_kinematics(kinematics_test):
+    wheel_speeds = MecanumDriveWheelSpeeds(
+        frontLeft=-5, frontRight=5, rearLeft=5, rearRight=-5
+    )
+    chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
+
+    assert chassis_speeds.vx == pytest.approx(0.0, abs=0.1)
+    assert chassis_speeds.vy == pytest.approx(5.0, abs=0.1)
+    assert chassis_speeds.omega == pytest.approx(0.0, abs=0.1)
+
+
+def test_strafe_forward_kinematics_with_deltas(kinematics_test):
+    wheel_deltas = MecanumDriveWheelPositions(
+        frontLeft=-5, frontRight=5, rearLeft=5, rearRight=-5
+    )
+    twist = kinematics_test.kinematics.toTwist2d(wheel_deltas)
+
+    assert twist.dx == pytest.approx(0.0, abs=0.1)
+    assert twist.dy == pytest.approx(5.0, abs=0.1)
+    assert twist.dtheta == pytest.approx(0.0, abs=0.1)
+
+
+def test_rotation_inverse_kinematics(kinematics_test):
+    speeds = ChassisSpeeds(vx=0, vy=0, omega=2 * math.pi)
+    module_states = kinematics_test.kinematics.toWheelSpeeds(speeds)
+
+    assert module_states.frontLeft == pytest.approx(-150.79644737, abs=0.1)
+    assert module_states.frontRight == pytest.approx(150.79644737, abs=0.1)
+    assert module_states.rearLeft == pytest.approx(-150.79644737, abs=0.1)
+    assert module_states.rearRight == pytest.approx(150.79644737, abs=0.1)
+
+
+def test_rotation_forward_kinematics(kinematics_test):
+    wheel_speeds = MecanumDriveWheelSpeeds(
+        frontLeft=-150.79644737,
+        frontRight=150.79644737,
+        rearLeft=-150.79644737,
+        rearRight=150.79644737,
+    )
+    chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
+
+    assert chassis_speeds.vx == pytest.approx(0.0, abs=0.1)
+    assert chassis_speeds.vy == pytest.approx(0.0, abs=0.1)
+    assert chassis_speeds.omega == pytest.approx(2 * math.pi, abs=0.1)
+
+
+def test_rotation_forward_kinematics_with_deltas(kinematics_test):
+    wheel_deltas = MecanumDriveWheelPositions(
+        frontLeft=-150.79644737,
+        frontRight=150.79644737,
+        rearLeft=-150.79644737,
+        rearRight=150.79644737,
+    )
+    twist = kinematics_test.kinematics.toTwist2d(wheel_deltas)
+
+    assert twist.dx == pytest.approx(0.0, abs=0.1)
+    assert twist.dy == pytest.approx(0.0, abs=0.1)
+    assert twist.dtheta == pytest.approx(2 * math.pi, abs=0.1)
+
+
+def test_mixed_rotation_translation_inverse_kinematics(kinematics_test):
+    speeds = ChassisSpeeds(vx=2, vy=3, omega=1)
+    module_states = kinematics_test.kinematics.toWheelSpeeds(speeds)
+
+    assert module_states.frontLeft == pytest.approx(-25.0, abs=0.1)
+    assert module_states.frontRight == pytest.approx(29.0, abs=0.1)
+    assert module_states.rearLeft == pytest.approx(-19.0, abs=0.1)
+    assert module_states.rearRight == pytest.approx(23.0, abs=0.1)
+
+
+def test_mixed_rotation_translation_forward_kinematics(kinematics_test):
+    wheel_speeds = MecanumDriveWheelSpeeds(
+        frontLeft=-17.677670,
+        frontRight=20.506097,
+        rearLeft=-13.435,
+        rearRight=16.26,
+    )
+
+    chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
+
+    assert chassis_speeds.vx == pytest.approx(1.41335, abs=0.1)
+    assert chassis_speeds.vy == pytest.approx(2.1221, abs=0.1)
+    assert chassis_speeds.omega == pytest.approx(0.707, abs=0.1)
+
+
+def test_mixed_rotation_translation_forward_kinematics_with_deltas(kinematics_test):
+    wheel_deltas = MecanumDriveWheelPositions(
+        frontLeft=-17.677670, frontRight=20.506097, rearLeft=-13.435, rearRight=16.26
+    )
+
+    twist = kinematics_test.kinematics.toTwist2d(wheel_deltas)
+
+    assert twist.dx == pytest.approx(1.41335, abs=0.1)
+    assert twist.dy == pytest.approx(2.1221, abs=0.1)
+    assert twist.dtheta == pytest.approx(0.707, abs=0.1)
+
+
+def test_off_center_rotation_inverse_kinematics(kinematics_test):
+    speeds = ChassisSpeeds(vx=0, vy=0, omega=1)
+    module_states = kinematics_test.kinematics.toWheelSpeeds(
+        speeds, kinematics_test.m_fl
+    )
+
+    assert module_states.frontLeft == pytest.approx(0, abs=0.1)
+    assert module_states.frontRight == pytest.approx(24.0, abs=0.1)
+    assert module_states.rearLeft == pytest.approx(-24.0, abs=0.1)
+    assert module_states.rearRight == pytest.approx(48.0, abs=0.1)
+
+
+def test_off_center_rotation_forward_kinematics(kinematics_test):
+    wheel_speeds = MecanumDriveWheelSpeeds(
+        frontLeft=0,
+        frontRight=16.971,
+        rearLeft=-16.971,
+        rearRight=33.941,
+    )
+    chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
+
+    assert chassis_speeds.vx == pytest.approx(8.48525, abs=0.1)
+    assert chassis_speeds.vy == pytest.approx(-8.48525, abs=0.1)
+    assert chassis_speeds.omega == pytest.approx(0.707, abs=0.1)
+
+
+def test_off_center_rotation_forward_kinematics_with_deltas(kinematics_test):
+    wheel_deltas = MecanumDriveWheelPositions(
+        frontLeft=0, frontRight=16.971, rearLeft=-16.971, rearRight=33.941
+    )
+    twist = kinematics_test.kinematics.toTwist2d(wheel_deltas)
+
+    assert twist.dx == pytest.approx(8.48525, abs=0.1)
+    assert twist.dy == pytest.approx(-8.48525, abs=0.1)
+    assert twist.dtheta == pytest.approx(0.707, abs=0.1)
+
+
+def test_off_center_translation_rotation_inverse_kinematics(kinematics_test):
+    speeds = ChassisSpeeds(vx=5, vy=2, omega=1)
+    module_states = kinematics_test.kinematics.toWheelSpeeds(
+        speeds, kinematics_test.m_fl
+    )
+
+    assert module_states.frontLeft == pytest.approx(3.0, abs=0.1)
+    assert module_states.frontRight == pytest.approx(31.0, abs=0.1)
+    assert module_states.rearLeft == pytest.approx(-17.0, abs=0.1)
+    assert module_states.rearRight == pytest.approx(51.0, abs=0.1)
+
+
+def test_off_center_translation_rotation_forward_kinematics(kinematics_test):
+    wheel_speeds = MecanumDriveWheelSpeeds(
+        frontLeft=2.12,
+        frontRight=21.92,
+        rearLeft=-12.02,
+        rearRight=36.06,
+    )
+    chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
+
+    assert chassis_speeds.vx == pytest.approx(12.02, abs=0.1)
+    assert chassis_speeds.vy == pytest.approx(-7.07, abs=0.1)
+    assert chassis_speeds.omega == pytest.approx(0.707, abs=0.1)
+
+
+def test_off_center_translation_rotation_forward_kinematics_with_deltas(
+    kinematics_test,
+):
+    wheel_deltas = MecanumDriveWheelPositions(
+        frontLeft=2.12, frontRight=21.92, rearLeft=-12.02, rearRight=36.06
+    )
+    twist = kinematics_test.kinematics.toTwist2d(wheel_deltas)
+
+    assert twist.dx == pytest.approx(12.02, abs=0.1)
+    assert twist.dy == pytest.approx(-7.07, abs=0.1)
+    assert twist.dtheta == pytest.approx(0.707, abs=0.1)
+
+
+def test_desaturate(kinematics_test):
+    wheel_speeds = MecanumDriveWheelSpeeds(
+        frontLeft=5, frontRight=6, rearLeft=4, rearRight=7
+    ).desaturate(5.5)
+
+    k_factor = 5.5 / 7.0
+
+    assert wheel_speeds.frontLeft == pytest.approx(5.0 * k_factor, abs=1e-9)
+    assert wheel_speeds.frontRight == pytest.approx(6.0 * k_factor, abs=1e-9)
+    assert wheel_speeds.rearLeft == pytest.approx(4.0 * k_factor, abs=1e-9)
+    assert wheel_speeds.rearRight == pytest.approx(7.0 * k_factor, abs=1e-9)
+
+
+def test_desaturate_negative_speeds(kinematics_test):
+    wheel_speeds = MecanumDriveWheelSpeeds(
+        frontLeft=-5,
+        frontRight=6,
+        rearLeft=4,
+        rearRight=-7,
+    ).desaturate(5.5)
+
+    k_factor = 5.5 / 7.0
+
+    assert wheel_speeds.frontLeft == pytest.approx(-5.0 * k_factor, abs=1e-9)
+    assert wheel_speeds.frontRight == pytest.approx(6.0 * k_factor, abs=1e-9)
+    assert wheel_speeds.rearLeft == pytest.approx(4.0 * k_factor, abs=1e-9)
+    assert wheel_speeds.rearRight == pytest.approx(-7.0 * k_factor, abs=1e-9)
--- a/wpimath/src/test/python/kinematics/test_mecanum_drive_odometry.py
+++ b/wpimath/src/test/python/kinematics/test_mecanum_drive_odometry.py
@@ -0,0 +1,232 @@
+import pytest
+import math
+import random
+
+from wpimath import (
+    ChassisSpeeds,
+    MecanumDriveKinematics,
+    MecanumDriveOdometry,
+    MecanumDriveWheelPositions,
+    Pose2d,
+    Rotation2d,
+    TrajectoryGenerator,
+    TrajectoryConfig,
+    Translation2d,
+)
+
+
+@pytest.fixture
+def odometry_test():
+    class MecanumDriveOdometryTest:
+        def __init__(self):
+            self.m_fl = Translation2d(x=12, y=12)
+            self.m_fr = Translation2d(x=12, y=-12)
+            self.m_bl = Translation2d(x=-12, y=12)
+            self.m_br = Translation2d(x=-12, y=-12)
+
+            self.zero = MecanumDriveWheelPositions()
+
+            self.kinematics = MecanumDriveKinematics(
+                self.m_fl, self.m_fr, self.m_bl, self.m_br
+            )
+            self.odometry = MecanumDriveOdometry(
+                self.kinematics, Rotation2d(0), self.zero
+            )
+
+    return MecanumDriveOdometryTest()
+
+
+def test_multiple_consecutive_updates(odometry_test):
+    wheel_deltas = MecanumDriveWheelPositions(
+        frontLeft=3.536, frontRight=3.536, rearLeft=3.536, rearRight=3.536
+    )
+
+    odometry_test.odometry.resetPosition(Rotation2d(0), wheel_deltas, Pose2d())
+    odometry_test.odometry.update(Rotation2d(0), wheel_deltas)
+    second_pose = odometry_test.odometry.update(Rotation2d.fromDegrees(0), wheel_deltas)
+
+    assert second_pose.x == pytest.approx(0.0, abs=0.01)
+    assert second_pose.y == pytest.approx(0.0, abs=0.01)
+    assert second_pose.rotation().radians() == pytest.approx(0.0, abs=0.01)
+
+
+def test_two_iterations(odometry_test):
+    odometry_test.odometry.resetPosition(Rotation2d(0), odometry_test.zero, Pose2d())
+    wheel_deltas = MecanumDriveWheelPositions(
+        frontLeft=0.3536, frontRight=0.3536, rearLeft=0.3536, rearRight=0.3536
+    )
+
+    odometry_test.odometry.update(Rotation2d(0), MecanumDriveWheelPositions())
+    pose = odometry_test.odometry.update(Rotation2d(0), wheel_deltas)
+
+    assert pose.x == pytest.approx(0.3536, abs=0.01)
+    assert pose.y == pytest.approx(0.0, abs=0.01)
+    assert pose.rotation().radians() == pytest.approx(0.0, abs=0.01)
+
+
+def test_90_degree_turn(odometry_test):
+    odometry_test.odometry.resetPosition(Rotation2d(0), odometry_test.zero, Pose2d())
+    wheel_deltas = MecanumDriveWheelPositions(
+        frontLeft=-13.328, frontRight=39.986, rearLeft=-13.329, rearRight=39.986
+    )
+    odometry_test.odometry.update(Rotation2d(0), MecanumDriveWheelPositions())
+    pose = odometry_test.odometry.update(Rotation2d.fromDegrees(90), wheel_deltas)
+
+    assert pose.x == pytest.approx(8.4855, abs=0.01)
+    assert pose.y == pytest.approx(8.4855, abs=0.01)
+    assert pose.rotation().degrees() == pytest.approx(90.0, abs=0.01)
+
+
+def test_gyro_angle_reset(odometry_test):
+    odometry_test.odometry.resetPosition(
+        Rotation2d.fromDegrees(90), odometry_test.zero, Pose2d()
+    )
+
+    wheel_deltas = MecanumDriveWheelPositions(
+        frontLeft=0.3536, frontRight=0.3536, rearLeft=0.3536, rearRight=0.3536
+    )
+
+    odometry_test.odometry.update(
+        Rotation2d.fromDegrees(90), MecanumDriveWheelPositions()
+    )
+    pose = odometry_test.odometry.update(Rotation2d.fromDegrees(90), wheel_deltas)
+
+    assert pose.x == pytest.approx(0.3536, abs=0.01)
+    assert pose.y == pytest.approx(0.0, abs=0.01)
+    assert pose.rotation().radians() == pytest.approx(0.0, abs=0.01)
+
+
+def test_accuracy_facing_trajectory():
+    kinematics = MecanumDriveKinematics(
+        frontLeftWheel=Translation2d(x=1, y=1),
+        frontRightWheel=Translation2d(x=1, y=-1),
+        rearLeftWheel=Translation2d(x=-1, y=-1),
+        rearRightWheel=Translation2d(x=-1, y=1),
+    )
+
+    wheel_positions = MecanumDriveWheelPositions()
+
+    odometry = MecanumDriveOdometry(kinematics, Rotation2d(), wheel_positions)
+
+    trajectory = TrajectoryGenerator.generateTrajectory(
+        [
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(45)),
+            Pose2d(x=3, y=0, rotation=Rotation2d.fromDegrees(-90)),
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(135)),
+            Pose2d(x=-3, y=0, rotation=Rotation2d.fromDegrees(-90)),
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(45)),
+        ],
+        TrajectoryConfig(maxVelocity=0.5, maxAcceleration=2.0),
+    )
+
+    random.seed(4915)
+
+    dt = 0.02
+    t = 0
+
+    max_error = -1e10
+    error_sum = 0
+
+    while t < trajectory.totalTime():
+        ground_truth_state = trajectory.sample(t)
+
+        wheel_speeds = kinematics.toWheelSpeeds(
+            ChassisSpeeds(
+                ground_truth_state.velocity,
+                0,
+                ground_truth_state.velocity * ground_truth_state.curvature,
+            )
+        )
+
+        wheel_speeds.frontLeft += random.gauss(0.0, 1.0) * 0.1
+        wheel_speeds.frontRight += random.gauss(0.0, 1.0) * 0.1
+        wheel_speeds.rearLeft += random.gauss(0.0, 1.0) * 0.1
+        wheel_speeds.rearRight += random.gauss(0.0, 1.0) * 0.1
+
+        wheel_positions.frontLeft += wheel_speeds.frontLeft * dt
+        wheel_positions.frontRight += wheel_speeds.frontRight * dt
+        wheel_positions.rearLeft += wheel_speeds.rearLeft * dt
+        wheel_positions.rearRight += wheel_speeds.rearRight * dt
+
+        xhat = odometry.update(
+            ground_truth_state.pose.rotation()
+            + Rotation2d(random.gauss(0.0, 1.0) * 0.05),
+            wheel_positions,
+        )
+
+        error = ground_truth_state.pose.translation().distance(xhat.translation())
+        if error > max_error:
+            max_error = error
+        error_sum += error
+
+        t += dt
+
+    assert error_sum / (trajectory.totalTime() / dt) < 0.35
+    assert max_error < 0.35
+
+
+def test_accuracy_facing_x_axis():
+    kinematics = MecanumDriveKinematics(
+        frontLeftWheel=Translation2d(x=1, y=1),
+        frontRightWheel=Translation2d(x=1, y=-1),
+        rearLeftWheel=Translation2d(x=-1, y=-1),
+        rearRightWheel=Translation2d(x=-1, y=1),
+    )
+
+    wheel_positions = MecanumDriveWheelPositions()
+
+    odometry = MecanumDriveOdometry(kinematics, Rotation2d(), wheel_positions)
+
+    trajectory = TrajectoryGenerator.generateTrajectory(
+        [
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(45)),
+            Pose2d(x=3, y=0, rotation=Rotation2d.fromDegrees(-90)),
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(135)),
+            Pose2d(x=-3, y=0, rotation=Rotation2d.fromDegrees(-90)),
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(45)),
+        ],
+        TrajectoryConfig(5.0, 2.0),
+    )
+
+    random.seed(4915)
+
+    dt = 0.02
+    t = 0
+
+    max_error = -1e10
+    error_sum = 0
+
+    while t < trajectory.totalTime():
+        ground_truth_state = trajectory.sample(t)
+
+        wheel_speeds = kinematics.toWheelSpeeds(
+            ChassisSpeeds(
+                ground_truth_state.velocity * ground_truth_state.pose.rotation().cos(),
+                ground_truth_state.velocity * ground_truth_state.pose.rotation().sin(),
+                0,
+            )
+        )
+
+        wheel_speeds.frontLeft += random.gauss(0.0, 1.0) * 0.1
+        wheel_speeds.frontRight += random.gauss(0.0, 1.0) * 0.1
+        wheel_speeds.rearLeft += random.gauss(0.0, 1.0) * 0.1
+        wheel_speeds.rearRight += random.gauss(0.0, 1.0) * 0.1
+
+        wheel_positions.frontLeft += wheel_speeds.frontLeft * dt
+        wheel_positions.frontRight += wheel_speeds.frontRight * dt
+        wheel_positions.rearLeft += wheel_speeds.rearLeft * dt
+        wheel_positions.rearRight += wheel_speeds.rearRight * dt
+
+        xhat = odometry.update(
+            Rotation2d(random.gauss(0.0, 1.0) * 0.05), wheel_positions
+        )
+
+        error = ground_truth_state.pose.translation().distance(xhat.translation())
+        if error > max_error:
+            max_error = error
+        error_sum += error
+
+        t += dt
+
+    assert error_sum / (trajectory.totalTime() / dt) < 0.06
+    assert max_error < 0.125
--- a/wpimath/src/test/python/kinematics/test_mecanum_drive_odometry3d.py
+++ b/wpimath/src/test/python/kinematics/test_mecanum_drive_odometry3d.py
@@ -0,0 +1,264 @@
+import pytest
+import math
+import random
+
+from wpimath import (
+    ChassisSpeeds,
+    MecanumDriveKinematics,
+    MecanumDriveOdometry3d,
+    MecanumDriveWheelPositions,
+    Pose2d,
+    Pose3d,
+    Rotation2d,
+    Rotation3d,
+    TrajectoryGenerator,
+    TrajectoryConfig,
+    Translation2d,
+)
+
+
+@pytest.fixture
+def odometry3d_test():
+    class MecanumDriveOdometry3dTest:
+        def __init__(self):
+            self.m_fl = Translation2d(x=12, y=12)
+            self.m_fr = Translation2d(x=12, y=-12)
+            self.m_bl = Translation2d(x=-12, y=12)
+            self.m_br = Translation2d(x=-12, y=-12)
+
+            self.zero = MecanumDriveWheelPositions()
+
+            self.kinematics = MecanumDriveKinematics(
+                self.m_fl, self.m_fr, self.m_bl, self.m_br
+            )
+            self.odometry = MecanumDriveOdometry3d(
+                self.kinematics, Rotation3d(), self.zero
+            )
+
+    return MecanumDriveOdometry3dTest()
+
+
+def test_initialize(odometry3d_test):
+    odometry = MecanumDriveOdometry3d(
+        odometry3d_test.kinematics,
+        gyroAngle=Rotation3d(),
+        wheelPositions=odometry3d_test.zero,
+        initialPose=Pose3d(x=1, y=2, z=0, rotation=Rotation3d.fromDegrees(0, 0, 45)),
+    )
+
+    pose = odometry.getPose()
+
+    assert pose.x == pytest.approx(1, abs=1e-9)
+    assert pose.y == pytest.approx(2, abs=1e-9)
+    assert pose.z == pytest.approx(0, abs=1e-9)
+    assert pose.rotation().toRotation2d().degrees() == pytest.approx(45, abs=1e-9)
+
+
+def test_multiple_consecutive_updates(odometry3d_test):
+    wheel_deltas = MecanumDriveWheelPositions(
+        frontLeft=3.536, frontRight=3.536, rearLeft=3.536, rearRight=3.536
+    )
+
+    odometry3d_test.odometry.resetPosition(Rotation3d(), wheel_deltas, Pose3d())
+    odometry3d_test.odometry.update(Rotation3d(), wheel_deltas)
+    second_pose = odometry3d_test.odometry.update(Rotation3d(), wheel_deltas)
+
+    assert second_pose.x == pytest.approx(0.0, abs=0.01)
+    assert second_pose.y == pytest.approx(0.0, abs=0.01)
+    assert second_pose.z == pytest.approx(0.0, abs=0.01)
+    assert second_pose.rotation().toRotation2d().radians() == pytest.approx(
+        0.0, abs=0.01
+    )
+
+
+def test_two_iterations(odometry3d_test):
+    odometry3d_test.odometry.resetPosition(Rotation3d(), odometry3d_test.zero, Pose3d())
+    wheel_deltas = MecanumDriveWheelPositions(
+        frontLeft=0.3536, frontRight=0.3536, rearLeft=0.3536, rearRight=0.3536
+    )
+
+    odometry3d_test.odometry.update(Rotation3d(), MecanumDriveWheelPositions())
+    pose = odometry3d_test.odometry.update(Rotation3d(), wheel_deltas)
+
+    assert pose.x == pytest.approx(0.3536, abs=0.01)
+    assert pose.y == pytest.approx(0.0, abs=0.01)
+    assert pose.z == pytest.approx(0.0, abs=0.01)
+    assert pose.rotation().toRotation2d().radians() == pytest.approx(0.0, abs=0.01)
+
+
+def test_90_degree_turn(odometry3d_test):
+    odometry3d_test.odometry.resetPosition(Rotation3d(), odometry3d_test.zero, Pose3d())
+    wheel_deltas = MecanumDriveWheelPositions(
+        frontLeft=-13.328, frontRight=39.986, rearLeft=-13.329, rearRight=39.986
+    )
+    odometry3d_test.odometry.update(Rotation3d(), MecanumDriveWheelPositions())
+    pose = odometry3d_test.odometry.update(
+        Rotation3d.fromDegrees(0, 0, 90), wheel_deltas
+    )
+
+    assert pose.x == pytest.approx(8.4855, abs=0.01)
+    assert pose.y == pytest.approx(8.4855, abs=0.01)
+    assert pose.z == pytest.approx(0, abs=0.01)
+    assert pose.rotation().toRotation2d().degrees() == pytest.approx(90.0, abs=0.01)
+
+
+def test_gyro_angle_reset(odometry3d_test):
+    odometry3d_test.odometry.resetPosition(
+        Rotation3d.fromDegrees(0, 0, 90), odometry3d_test.zero, Pose3d()
+    )
+
+    wheel_deltas = MecanumDriveWheelPositions(
+        frontLeft=0.3536, frontRight=0.3536, rearLeft=0.3536, rearRight=0.3536
+    )
+
+    pose = odometry3d_test.odometry.update(
+        Rotation3d.fromDegrees(0, 0, 90), wheel_deltas
+    )
+
+    assert pose.x == pytest.approx(0.3536, abs=0.01)
+    assert pose.y == pytest.approx(0.0, abs=0.01)
+    assert pose.z == pytest.approx(0.0, abs=0.01)
+    assert pose.rotation().toRotation2d().radians() == pytest.approx(0.0, abs=0.01)
+
+
+def test_accuracy_facing_trajectory():
+    kinematics = MecanumDriveKinematics(
+        frontLeftWheel=Translation2d(x=1, y=1),
+        frontRightWheel=Translation2d(x=1, y=-1),
+        rearLeftWheel=Translation2d(x=-1, y=-1),
+        rearRightWheel=Translation2d(x=-1, y=1),
+    )
+
+    wheel_positions = MecanumDriveWheelPositions()
+
+    odometry = MecanumDriveOdometry3d(kinematics, Rotation3d(), wheel_positions)
+
+    trajectory = TrajectoryGenerator.generateTrajectory(
+        [
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(45)),
+            Pose2d(x=3, y=0, rotation=Rotation2d.fromDegrees(-90)),
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(135)),
+            Pose2d(x=-3, y=0, rotation=Rotation2d.fromDegrees(-90)),
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(45)),
+        ],
+        TrajectoryConfig(maxVelocity=5.0, maxAcceleration=2.0),
+    )
+
+    random.seed(4915)
+
+    dt = 0.02
+    t = 0
+
+    max_error = -1e10
+    error_sum = 0
+
+    while t < trajectory.totalTime():
+        ground_truth_state = trajectory.sample(t)
+
+        wheel_speeds = kinematics.toWheelSpeeds(
+            ChassisSpeeds(
+                ground_truth_state.velocity,
+                0,
+                ground_truth_state.velocity * ground_truth_state.curvature,
+            )
+        )
+
+        wheel_speeds.frontLeft += random.gauss(0.0, 1.0) * 0.1
+        wheel_speeds.frontRight += random.gauss(0.0, 1.0) * 0.1
+        wheel_speeds.rearLeft += random.gauss(0.0, 1.0) * 0.1
+        wheel_speeds.rearRight += random.gauss(0.0, 1.0) * 0.1
+
+        wheel_positions.frontLeft += wheel_speeds.frontLeft * dt
+        wheel_positions.frontRight += wheel_speeds.frontRight * dt
+        wheel_positions.rearLeft += wheel_speeds.rearLeft * dt
+        wheel_positions.rearRight += wheel_speeds.rearRight * dt
+
+        xhat = odometry.update(
+            Rotation3d(
+                ground_truth_state.pose.rotation()
+                + Rotation2d(random.gauss(0.0, 1.0) * 0.05)
+            ),
+            wheel_positions,
+        )
+
+        error = ground_truth_state.pose.translation().distance(
+            xhat.translation().toTranslation2d()
+        )
+        if error > max_error:
+            max_error = error
+        error_sum += error
+
+        t += dt
+
+    assert error_sum / (trajectory.totalTime() / dt) < 0.07
+    assert max_error < 0.125
+
+
+def test_accuracy_facing_x_axis():
+    kinematics = MecanumDriveKinematics(
+        frontLeftWheel=Translation2d(x=1, y=1),
+        frontRightWheel=Translation2d(x=1, y=-1),
+        rearLeftWheel=Translation2d(x=-1, y=-1),
+        rearRightWheel=Translation2d(x=-1, y=1),
+    )
+
+    wheel_positions = MecanumDriveWheelPositions()
+
+    odometry = MecanumDriveOdometry3d(kinematics, Rotation3d(), wheel_positions)
+
+    trajectory = TrajectoryGenerator.generateTrajectory(
+        [
+            Pose2d(x=0, y=0, rotation=Rotation2d(45)),
+            Pose2d(x=3, y=0, rotation=Rotation2d(-90)),
+            Pose2d(x=0, y=0, rotation=Rotation2d(135)),
+            Pose2d(x=-3, y=0, rotation=Rotation2d(-90)),
+            Pose2d(x=0, y=0, rotation=Rotation2d(45)),
+        ],
+        TrajectoryConfig(maxVelocity=5.0, maxAcceleration=2.0),
+    )
+
+    random.seed(4915)
+
+    dt = 0.02
+    t = 0
+
+    max_error = -1e10
+    error_sum = 0
+
+    while t < trajectory.totalTime():
+        ground_truth_state = trajectory.sample(t)
+
+        wheel_speeds = kinematics.toWheelSpeeds(
+            ChassisSpeeds(
+                ground_truth_state.velocity * ground_truth_state.pose.rotation().cos(),
+                ground_truth_state.velocity * ground_truth_state.pose.rotation().sin(),
+                0,
+            )
+        )
+
+        wheel_speeds.frontLeft += random.gauss(0.0, 1.0) * 0.1
+        wheel_speeds.frontRight += random.gauss(0.0, 1.0) * 0.1
+        wheel_speeds.rearLeft += random.gauss(0.0, 1.0) * 0.1
+        wheel_speeds.rearRight += random.gauss(0.0, 1.0) * 0.1
+
+        wheel_positions.frontLeft += wheel_speeds.frontLeft * dt
+        wheel_positions.frontRight += wheel_speeds.frontRight * dt
+        wheel_positions.rearLeft += wheel_speeds.rearLeft * dt
+        wheel_positions.rearRight += wheel_speeds.rearRight * dt
+
+        xhat = odometry.update(
+            Rotation3d(0, 0, random.gauss(0.0, 1.0) * 0.05),
+            wheel_positions,
+        )
+
+        error = ground_truth_state.pose.translation().distance(
+            xhat.translation().toTranslation2d()
+        )
+        if error > max_error:
+            max_error = error
+        error_sum += error
+
+        t += dt
+
+    assert error_sum / (trajectory.totalTime() / dt) < 0.06
+    assert max_error < 0.125
--- a/wpimath/src/test/python/kinematics/test_mecanum_drive_wheel_speeds.py
+++ b/wpimath/src/test/python/kinematics/test_mecanum_drive_wheel_speeds.py
@@ -0,0 +1,95 @@
+import pytest
+
+from wpimath import MecanumDriveWheelSpeeds
+
+
+def test_plus():
+    left = MecanumDriveWheelSpeeds(
+        frontLeft=1.0,
+        frontRight=0.5,
+        rearLeft=2.0,
+        rearRight=1.5,
+    )
+    right = MecanumDriveWheelSpeeds(
+        frontLeft=2.0,
+        frontRight=1.5,
+        rearLeft=0.5,
+        rearRight=1.0,
+    )
+
+    result = left + right
+
+    assert result.frontLeft == 3.0
+    assert result.frontRight == 2.0
+    assert result.rearLeft == 2.5
+    assert result.rearRight == 2.5
+
+
+def test_minus():
+    left = MecanumDriveWheelSpeeds(
+        frontLeft=1.0,
+        frontRight=0.5,
+        rearLeft=2.0,
+        rearRight=1.5,
+    )
+    right = MecanumDriveWheelSpeeds(
+        frontLeft=2.0,
+        frontRight=1.5,
+        rearLeft=0.5,
+        rearRight=1.0,
+    )
+
+    result = left - right
+
+    assert result.frontLeft == -1.0
+    assert result.frontRight == -1.0
+    assert result.rearLeft == 1.5
+    assert result.rearRight == 0.5
+
+
+def test_unary_minus():
+    speeds = MecanumDriveWheelSpeeds(
+        frontLeft=1.0,
+        frontRight=0.5,
+        rearLeft=2.0,
+        rearRight=1.5,
+    )
+
+    result = -speeds
+
+    assert result.frontLeft == -1.0
+    assert result.frontRight == -0.5
+    assert result.rearLeft == -2.0
+    assert result.rearRight == -1.5
+
+
+def test_multiplication():
+    speeds = MecanumDriveWheelSpeeds(
+        frontLeft=1.0,
+        frontRight=0.5,
+        rearLeft=2.0,
+        rearRight=1.5,
+    )
+
+    result = speeds * 2
+
+    assert result.frontLeft == 2.0
+    assert result.frontRight == 1.0
+    assert result.rearLeft == 4.0
+    assert result.rearRight == 3.0
+
+
+def test_division():
+    speeds = MecanumDriveWheelSpeeds(
+        frontLeft=1.0,
+        frontRight=0.5,
+        rearLeft=2.0,
+        rearRight=1.5,
+    )
+
+    result = speeds / 2
+
+    assert result.frontLeft == 0.5
+    assert result.frontRight == 0.25
+    assert result.rearLeft == 1.0
+    assert result.rearRight == 0.75
--- a/wpimath/src/test/python/kinematics/test_swerve_drive_kinematics.py
+++ b/wpimath/src/test/python/kinematics/test_swerve_drive_kinematics.py
@@ -0,0 +1,330 @@
+import pytest
+import math
+
+from wpimath import (
+    SwerveDrive4Kinematics,
+    ChassisSpeeds,
+    Rotation2d,
+    SwerveModuleState,
+    SwerveModulePosition,
+    Translation2d,
+)
+
+kEpsilon = 0.1
+
+
+@pytest.fixture
+def kinematics_test():
+    class SwerveDriveKinematicsTest:
+        def __init__(self):
+            self.m_fl = Translation2d(x=12, y=12)
+            self.m_fr = Translation2d(x=12, y=-12)
+            self.m_bl = Translation2d(x=-12, y=12)
+            self.m_br = Translation2d(x=-12, y=-12)
+            self.m_kinematics = SwerveDrive4Kinematics(
+                self.m_fl, self.m_fr, self.m_bl, self.m_br
+            )
+
+    return SwerveDriveKinematicsTest()
+
+
+def test_straight_line_inverse_kinematics(kinematics_test):
+    speeds = ChassisSpeeds(vx=5.0, vy=0.0, omega=0.0)
+    states = kinematics_test.m_kinematics.toSwerveModuleStates(speeds)
+
+    fl, fr, bl, br = states
+
+    assert fl.speed == pytest.approx(5.0, abs=kEpsilon)
+    assert fr.speed == pytest.approx(5.0, abs=kEpsilon)
+    assert bl.speed == pytest.approx(5.0, abs=kEpsilon)
+    assert br.speed == pytest.approx(5.0, abs=kEpsilon)
+
+    assert fl.angle.radians() == pytest.approx(0.0, abs=kEpsilon)
+    assert fr.angle.radians() == pytest.approx(0.0, abs=kEpsilon)
+    assert bl.angle.radians() == pytest.approx(0.0, abs=kEpsilon)
+    assert br.angle.radians() == pytest.approx(0.0, abs=kEpsilon)
+
+
+def test_straight_line_forward_kinematics(kinematics_test):
+    state = SwerveModuleState(speed=5.0, angle=Rotation2d.fromDegrees(0))
+    chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds(
+        (state, state, state, state)
+    )
+
+    assert chassis_speeds.vx == pytest.approx(5.0, abs=kEpsilon)
+    assert chassis_speeds.vy == pytest.approx(0.0, abs=kEpsilon)
+    assert chassis_speeds.omega == pytest.approx(0.0, abs=kEpsilon)
+
+
+def test_straight_line_forward_kinematics_with_deltas(kinematics_test):
+    delta = SwerveModulePosition(distance=5.0, angle=Rotation2d.fromDegrees(0))
+    twist = kinematics_test.m_kinematics.toTwist2d((delta, delta, delta, delta))
+
+    assert twist.dx == pytest.approx(5.0, abs=kEpsilon)
+    assert twist.dy == pytest.approx(0.0, abs=kEpsilon)
+    assert twist.dtheta == pytest.approx(0.0, abs=kEpsilon)
+
+
+def test_straight_strafe_inverse_kinematics(kinematics_test):
+    speeds = ChassisSpeeds(vx=0, vy=5, omega=0)
+    states = kinematics_test.m_kinematics.toSwerveModuleStates(speeds)
+
+    fl, fr, bl, br = states
+
+    assert fl.speed == pytest.approx(5.0, abs=kEpsilon)
+    assert fr.speed == pytest.approx(5.0, abs=kEpsilon)
+    assert bl.speed == pytest.approx(5.0, abs=kEpsilon)
+    assert br.speed == pytest.approx(5.0, abs=kEpsilon)
+
+    assert fl.angle.degrees() == pytest.approx(90.0, abs=kEpsilon)
+    assert fr.angle.degrees() == pytest.approx(90.0, abs=kEpsilon)
+    assert bl.angle.degrees() == pytest.approx(90.0, abs=kEpsilon)
+    assert br.angle.degrees() == pytest.approx(90.0, abs=kEpsilon)
+
+
+def test_straight_strafe_forward_kinematics(kinematics_test):
+    state = SwerveModuleState(speed=5, angle=Rotation2d.fromDegrees(90))
+    chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds(
+        (state, state, state, state)
+    )
+
+    assert chassis_speeds.vx == pytest.approx(0.0, abs=kEpsilon)
+    assert chassis_speeds.vy == pytest.approx(5.0, abs=kEpsilon)
+    assert chassis_speeds.omega == pytest.approx(0.0, abs=kEpsilon)
+
+
+def test_straight_strafe_forward_kinematics_with_deltas(kinematics_test):
+    delta = SwerveModulePosition(distance=5, angle=Rotation2d.fromDegrees(90))
+    twist = kinematics_test.m_kinematics.toTwist2d((delta, delta, delta, delta))
+
+    assert twist.dx == pytest.approx(0.0, abs=kEpsilon)
+    assert twist.dy == pytest.approx(5.0, abs=kEpsilon)
+    assert twist.dtheta == pytest.approx(0.0, abs=kEpsilon)
+
+
+def test_turn_in_place_inverse_kinematics(kinematics_test):
+    speeds = ChassisSpeeds(vx=0, vy=0, omega=2 * math.pi)
+    states = kinematics_test.m_kinematics.toSwerveModuleStates(speeds)
+
+    fl, fr, bl, br = states
+
+    assert fl.speed == pytest.approx(106.63, abs=kEpsilon)
+    assert fr.speed == pytest.approx(106.63, abs=kEpsilon)
+    assert bl.speed == pytest.approx(106.63, abs=kEpsilon)
+    assert br.speed == pytest.approx(106.63, abs=kEpsilon)
+
+    assert fl.angle.degrees() == pytest.approx(135.0, abs=kEpsilon)
+    assert fr.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert bl.angle.degrees() == pytest.approx(-135.0, abs=kEpsilon)
+    assert br.angle.degrees() == pytest.approx(-45.0, abs=kEpsilon)
+
+
+def test_conserve_wheel_angle(kinematics_test):
+    speeds = ChassisSpeeds(vx=0, vy=0, omega=2 * math.pi)
+    kinematics_test.m_kinematics.toSwerveModuleStates(speeds)
+    states = kinematics_test.m_kinematics.toSwerveModuleStates(ChassisSpeeds())
+
+    fl, fr, bl, br = states
+
+    assert fl.speed == pytest.approx(0.0, abs=kEpsilon)
+    assert fr.speed == pytest.approx(0.0, abs=kEpsilon)
+    assert bl.speed == pytest.approx(0.0, abs=kEpsilon)
+    assert br.speed == pytest.approx(0.0, abs=kEpsilon)
+
+    assert fl.angle.degrees() == pytest.approx(135.0, abs=kEpsilon)
+    assert fr.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert bl.angle.degrees() == pytest.approx(-135.0, abs=kEpsilon)
+    assert br.angle.degrees() == pytest.approx(-45.0, abs=kEpsilon)
+
+
+def test_reset_wheel_angle(kinematics_test):
+    fl_angle = Rotation2d.fromDegrees(0)
+    fr_angle = Rotation2d.fromDegrees(90)
+    bl_angle = Rotation2d.fromDegrees(180)
+    br_angle = Rotation2d.fromDegrees(270)
+    kinematics_test.m_kinematics.resetHeadings((fl_angle, fr_angle, bl_angle, br_angle))
+    states = kinematics_test.m_kinematics.toSwerveModuleStates(ChassisSpeeds())
+
+    fl_mod, fr_mod, bl_mod, br_mod = states
+
+    assert fl_mod.speed == pytest.approx(0.0, abs=kEpsilon)
+    assert fr_mod.speed == pytest.approx(0.0, abs=kEpsilon)
+    assert bl_mod.speed == pytest.approx(0.0, abs=kEpsilon)
+    assert br_mod.speed == pytest.approx(0.0, abs=kEpsilon)
+
+    assert fl_mod.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
+    assert fr_mod.angle.degrees() == pytest.approx(90.0, abs=kEpsilon)
+    assert bl_mod.angle.degrees() == pytest.approx(180.0, abs=kEpsilon)
+    assert br_mod.angle.degrees() == pytest.approx(-90.0, abs=kEpsilon)
+
+
+def test_turn_in_place_forward_kinematics(kinematics_test):
+    fl = SwerveModuleState(speed=106.629, angle=Rotation2d.fromDegrees(135))
+    fr = SwerveModuleState(speed=106.629, angle=Rotation2d.fromDegrees(45))
+    bl = SwerveModuleState(speed=106.629, angle=Rotation2d.fromDegrees(-135))
+    br = SwerveModuleState(speed=106.629, angle=Rotation2d.fromDegrees(-45))
+
+    chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds((fl, fr, bl, br))
+
+    assert chassis_speeds.vx == pytest.approx(0.0, abs=kEpsilon)
+    assert chassis_speeds.vy == pytest.approx(0.0, abs=kEpsilon)
+    assert chassis_speeds.omega == pytest.approx(2 * math.pi, abs=kEpsilon)
+
+
+def test_turn_in_place_forward_kinematics_with_deltas(kinematics_test):
+    fl = SwerveModulePosition(distance=106.629, angle=Rotation2d.fromDegrees(135))
+    fr = SwerveModulePosition(distance=106.629, angle=Rotation2d.fromDegrees(45))
+    bl = SwerveModulePosition(distance=106.629, angle=Rotation2d.fromDegrees(-135))
+    br = SwerveModulePosition(distance=106.629, angle=Rotation2d.fromDegrees(-45))
+
+    twist = kinematics_test.m_kinematics.toTwist2d((fl, fr, bl, br))
+
+    assert twist.dx == pytest.approx(0.0, abs=kEpsilon)
+    assert twist.dy == pytest.approx(0.0, abs=kEpsilon)
+    assert twist.dtheta == pytest.approx(2 * math.pi, abs=kEpsilon)
+
+
+def test_off_center_cor_rotation_inverse_kinematics(kinematics_test):
+    speeds = ChassisSpeeds(0, 0, 2 * math.pi)
+    states = kinematics_test.m_kinematics.toSwerveModuleStates(
+        speeds, kinematics_test.m_fl
+    )
+
+    fl, fr, bl, br = states
+
+    assert fl.speed == pytest.approx(0.0, abs=kEpsilon)
+    assert fr.speed == pytest.approx(150.796, abs=kEpsilon)
+    assert bl.speed == pytest.approx(150.796, abs=kEpsilon)
+    assert br.speed == pytest.approx(213.258, abs=kEpsilon)
+
+    assert fl.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
+    assert fr.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
+    assert bl.angle.degrees() == pytest.approx(-90.0, abs=kEpsilon)
+    assert br.angle.degrees() == pytest.approx(-45.0, abs=kEpsilon)
+
+
+def test_off_center_cor_rotation_forward_kinematics(kinematics_test):
+    fl = SwerveModuleState(speed=0.0, angle=Rotation2d.fromDegrees(0))
+    fr = SwerveModuleState(speed=150.796, angle=Rotation2d.fromDegrees(0))
+    bl = SwerveModuleState(speed=150.796, angle=Rotation2d.fromDegrees(-90))
+    br = SwerveModuleState(speed=213.258, angle=Rotation2d.fromDegrees(-45))
+
+    chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds((fl, fr, bl, br))
+
+    assert chassis_speeds.vx == pytest.approx(75.398, abs=kEpsilon)
+    assert chassis_speeds.vy == pytest.approx(-75.398, abs=kEpsilon)
+    assert chassis_speeds.omega == pytest.approx(2 * math.pi, abs=kEpsilon)
+
+
+def test_off_center_cor_rotation_forward_kinematics_with_deltas(kinematics_test):
+    fl = SwerveModulePosition(distance=0.0, angle=Rotation2d.fromDegrees(0))
+    fr = SwerveModulePosition(distance=150.796, angle=Rotation2d.fromDegrees(0))
+    bl = SwerveModulePosition(distance=150.796, angle=Rotation2d.fromDegrees(-90))
+    br = SwerveModulePosition(distance=213.258, angle=Rotation2d.fromDegrees(-45))
+
+    twist = kinematics_test.m_kinematics.toTwist2d((fl, fr, bl, br))
+
+    assert twist.dx == pytest.approx(75.398, abs=kEpsilon)
+    assert twist.dy == pytest.approx(-75.398, abs=kEpsilon)
+    assert twist.dtheta == pytest.approx(2 * math.pi, abs=kEpsilon)
+
+
+def test_off_center_cor_rotation_and_translation_inverse_kinematics(kinematics_test):
+    speeds = ChassisSpeeds(0, 3.0, 1.5)
+    states = kinematics_test.m_kinematics.toSwerveModuleStates(
+        speeds, Translation2d(x=24, y=0)
+    )
+
+    fl, fr, bl, br = states
+
+    assert fl.speed == pytest.approx(23.43, abs=kEpsilon)
+    assert fr.speed == pytest.approx(23.43, abs=kEpsilon)
+    assert bl.speed == pytest.approx(54.08, abs=kEpsilon)
+    assert br.speed == pytest.approx(54.08, abs=kEpsilon)
+
+    assert fl.angle.degrees() == pytest.approx(-140.19, abs=kEpsilon)
+    assert fr.angle.degrees() == pytest.approx(-39.81, abs=kEpsilon)
+    assert bl.angle.degrees() == pytest.approx(-109.44, abs=kEpsilon)
+    assert br.angle.degrees() == pytest.approx(-70.56, abs=kEpsilon)
+
+
+def test_off_center_cor_rotation_and_translation_forward_kinematics(kinematics_test):
+    fl = SwerveModuleState(speed=23.43, angle=Rotation2d.fromDegrees(-140.19))
+    fr = SwerveModuleState(speed=23.43, angle=Rotation2d.fromDegrees(-39.81))
+    bl = SwerveModuleState(speed=54.08, angle=Rotation2d.fromDegrees(-109.44))
+    br = SwerveModuleState(speed=54.08, angle=Rotation2d.fromDegrees(-70.56))
+
+    chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds((fl, fr, bl, br))
+
+    assert chassis_speeds.vx == pytest.approx(0.0, abs=kEpsilon)
+    assert chassis_speeds.vy == pytest.approx(-33.0, abs=kEpsilon)
+    assert chassis_speeds.omega == pytest.approx(1.5, abs=kEpsilon)
+
+
+def test_off_center_cor_rotation_and_translation_forward_kinematics_with_deltas(
+    kinematics_test,
+):
+    fl = SwerveModulePosition(distance=23.43, angle=Rotation2d.fromDegrees(-140.19))
+    fr = SwerveModulePosition(distance=23.43, angle=Rotation2d.fromDegrees(-39.81))
+    bl = SwerveModulePosition(distance=54.08, angle=Rotation2d.fromDegrees(-109.44))
+    br = SwerveModulePosition(distance=54.08, angle=Rotation2d.fromDegrees(-70.56))
+
+    twist = kinematics_test.m_kinematics.toTwist2d((fl, fr, bl, br))
+
+    assert twist.dx == pytest.approx(0.0, abs=kEpsilon)
+    assert twist.dy == pytest.approx(-33.0, abs=kEpsilon)
+    assert twist.dtheta == pytest.approx(1.5, abs=kEpsilon)
+
+
+def test_desaturate(kinematics_test):
+    state1 = SwerveModuleState(speed=5.0, angle=Rotation2d.fromDegrees(0))
+    state2 = SwerveModuleState(speed=6.0, angle=Rotation2d.fromDegrees(0))
+    state3 = SwerveModuleState(speed=4.0, angle=Rotation2d.fromDegrees(0))
+    state4 = SwerveModuleState(speed=7.0, angle=Rotation2d.fromDegrees(0))
+
+    arr = [state1, state2, state3, state4]
+    arr = kinematics_test.m_kinematics.desaturateWheelSpeeds(arr, 5.5)
+
+    k_factor = 5.5 / 7.0
+
+    assert arr[0].speed == pytest.approx(5.0 * k_factor, abs=kEpsilon)
+    assert arr[1].speed == pytest.approx(6.0 * k_factor, abs=kEpsilon)
+    assert arr[2].speed == pytest.approx(4.0 * k_factor, abs=kEpsilon)
+    assert arr[3].speed == pytest.approx(7.0 * k_factor, abs=kEpsilon)
+
+
+def test_desaturate_smooth(kinematics_test):
+    state1 = SwerveModuleState(speed=5.0, angle=Rotation2d(0))
+    state2 = SwerveModuleState(speed=6.0, angle=Rotation2d(0))
+    state3 = SwerveModuleState(speed=4.0, angle=Rotation2d(0))
+    state4 = SwerveModuleState(speed=7.0, angle=Rotation2d(0))
+
+    arr = [state1, state2, state3, state4]
+    chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds(
+        (arr[0], arr[1], arr[2], arr[3])
+    )
+    arr = kinematics_test.m_kinematics.desaturateWheelSpeeds(
+        arr, chassis_speeds, 5.5, 5.5, 3.5
+    )
+
+    k_factor = 5.5 / 7.0
+
+    assert arr[0].speed == pytest.approx(5.0 * k_factor, abs=kEpsilon)
+    assert arr[1].speed == pytest.approx(6.0 * k_factor, abs=kEpsilon)
+    assert arr[2].speed == pytest.approx(4.0 * k_factor, abs=kEpsilon)
+    assert arr[3].speed == pytest.approx(7.0 * k_factor, abs=kEpsilon)
+
+
+def test_desaturate_negative_speed(kinematics_test):
+    state1 = SwerveModuleState(speed=1.0, angle=Rotation2d(0))
+    state2 = SwerveModuleState(speed=1.0, angle=Rotation2d(0))
+    state3 = SwerveModuleState(speed=-2.0, angle=Rotation2d(0))
+    state4 = SwerveModuleState(speed=-2.0, angle=Rotation2d(0))
+
+    arr = [state1, state2, state3, state4]
+    arr = kinematics_test.m_kinematics.desaturateWheelSpeeds(arr, 1.0)
+
+    assert arr[0].speed == pytest.approx(0.5, abs=kEpsilon)
+    assert arr[1].speed == pytest.approx(0.5, abs=kEpsilon)
+    assert arr[2].speed == pytest.approx(-1.0, abs=kEpsilon)
+    assert arr[3].speed == pytest.approx(-1.0, abs=kEpsilon)
--- a/wpimath/src/test/python/kinematics/test_swerve_drive_odometry.py
+++ b/wpimath/src/test/python/kinematics/test_swerve_drive_odometry.py
@@ -0,0 +1,262 @@
+import pytest
+import math
+import random
+
+from wpimath import (
+    ChassisSpeeds,
+    Pose2d,
+    Rotation2d,
+    SwerveDrive4Kinematics,
+    SwerveDrive4Odometry,
+    SwerveModuleState,
+    SwerveModulePosition,
+    TrajectoryGenerator,
+    TrajectoryConfig,
+    Translation2d,
+)
+
+kEpsilon = 0.01
+
+
+@pytest.fixture
+def odometry_test():
+    class SwerveDriveOdometryTest:
+        def __init__(self):
+            self.m_fl = Translation2d(x=12, y=12)
+            self.m_fr = Translation2d(x=12, y=-12)
+            self.m_bl = Translation2d(x=-12, y=12)
+            self.m_br = Translation2d(x=-12, y=-12)
+
+            self.m_kinematics = SwerveDrive4Kinematics(
+                self.m_fl, self.m_fr, self.m_bl, self.m_br
+            )
+            self.zero = SwerveModulePosition()
+            self.m_odometry = SwerveDrive4Odometry(
+                self.m_kinematics,
+                Rotation2d(0),
+                [self.zero, self.zero, self.zero, self.zero],
+            )
+
+    return SwerveDriveOdometryTest()
+
+
+def test_two_iterations(odometry_test):
+    position = SwerveModulePosition(distance=0.5, angle=Rotation2d.fromDegrees(0))
+    odometry_test.m_odometry.resetPosition(
+        Rotation2d(0),
+        [
+            odometry_test.zero,
+            odometry_test.zero,
+            odometry_test.zero,
+            odometry_test.zero,
+        ],
+        Pose2d(),
+    )
+
+    odometry_test.m_odometry.update(
+        Rotation2d(0),
+        [
+            odometry_test.zero,
+            odometry_test.zero,
+            odometry_test.zero,
+            odometry_test.zero,
+        ],
+    )
+
+    pose = odometry_test.m_odometry.update(
+        Rotation2d(0), [position, position, position, position]
+    )
+
+    assert pose.x == pytest.approx(0.5, abs=kEpsilon)
+    assert pose.y == pytest.approx(0.0, abs=kEpsilon)
+    assert pose.rotation().degrees() == pytest.approx(0.0, abs=kEpsilon)
+
+
+def test_90_degree_turn(odometry_test):
+    fl = SwerveModulePosition(distance=18.85, angle=Rotation2d.fromDegrees(90))
+    fr = SwerveModulePosition(distance=42.15, angle=Rotation2d.fromDegrees(26.565))
+    bl = SwerveModulePosition(distance=18.85, angle=Rotation2d.fromDegrees(-90))
+    br = SwerveModulePosition(distance=42.15, angle=Rotation2d.fromDegrees(-26.565))
+
+    odometry_test.m_odometry.resetPosition(
+        Rotation2d(0),
+        [
+            odometry_test.zero,
+            odometry_test.zero,
+            odometry_test.zero,
+            odometry_test.zero,
+        ],
+        Pose2d(),
+    )
+    pose = odometry_test.m_odometry.update(Rotation2d.fromDegrees(90), [fl, fr, bl, br])
+
+    assert pose.x == pytest.approx(12.0, abs=kEpsilon)
+    assert pose.y == pytest.approx(12.0, abs=kEpsilon)
+    assert pose.rotation().degrees() == pytest.approx(90.0, abs=kEpsilon)
+
+
+def test_gyro_angle_reset(odometry_test):
+    odometry_test.m_odometry.resetPosition(
+        Rotation2d.fromDegrees(90),
+        [
+            odometry_test.zero,
+            odometry_test.zero,
+            odometry_test.zero,
+            odometry_test.zero,
+        ],
+        Pose2d(),
+    )
+
+    position = SwerveModulePosition(distance=0.5, angle=Rotation2d.fromDegrees(0))
+    pose = odometry_test.m_odometry.update(
+        Rotation2d.fromDegrees(90), [position, position, position, position]
+    )
+
+    assert pose.x == pytest.approx(0.5, abs=kEpsilon)
+    assert pose.y == pytest.approx(0.0, abs=kEpsilon)
+    assert pose.rotation().degrees() == pytest.approx(0.0, abs=kEpsilon)
+
+
+def test_accuracy_facing_trajectory():
+    kinematics = SwerveDrive4Kinematics(
+        Translation2d(x=1, y=1),
+        Translation2d(x=1, y=-1),
+        Translation2d(x=-1, y=-1),
+        Translation2d(x=-1, y=1),
+    )
+    zero = SwerveModulePosition()
+    odometry = SwerveDrive4Odometry(kinematics, Rotation2d(), [zero, zero, zero, zero])
+
+    fl = SwerveModulePosition()
+    fr = SwerveModulePosition()
+    bl = SwerveModulePosition()
+    br = SwerveModulePosition()
+
+    trajectory = TrajectoryGenerator.generateTrajectory(
+        [
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(45)),
+            Pose2d(x=3, y=0, rotation=Rotation2d.fromDegrees(-90)),
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(135)),
+            Pose2d(x=-3, y=0, rotation=Rotation2d.fromDegrees(-90)),
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(45)),
+        ],
+        TrajectoryConfig(maxVelocity=5.0, maxAcceleration=2.0),
+    )
+
+    random.seed(4915)
+
+    dt = 0.02
+    t = 0
+
+    max_error = -float("inf")
+    error_sum = 0
+
+    while t < trajectory.totalTime():
+        ground_truth_state = trajectory.sample(t)
+
+        module_states = kinematics.toSwerveModuleStates(
+            ChassisSpeeds(
+                vx=ground_truth_state.velocity,
+                vy=0,
+                omega=ground_truth_state.velocity * ground_truth_state.curvature,
+            )
+        )
+
+        fl.distance += module_states[0].speed * dt
+        fr.distance += module_states[1].speed * dt
+        bl.distance += module_states[2].speed * dt
+        br.distance += module_states[3].speed * dt
+
+        fl.angle = module_states[0].angle
+        fr.angle = module_states[1].angle
+        bl.angle = module_states[2].angle
+        br.angle = module_states[3].angle
+
+        xhat = odometry.update(
+            ground_truth_state.pose.rotation()
+            + Rotation2d(random.gauss(0.0, 1.0) * 0.05),
+            [fl, fr, bl, br],
+        )
+
+        error = ground_truth_state.pose.translation().distance(xhat.translation())
+        if error > max_error:
+            max_error = error
+        error_sum += error
+
+        t += dt
+
+    assert error_sum / (trajectory.totalTime() / dt) < 0.05
+    assert max_error < 0.125
+
+
+def test_accuracy_facing_x_axis():
+    kinematics = SwerveDrive4Kinematics(
+        Translation2d(x=1, y=1),
+        Translation2d(x=1, y=-1),
+        Translation2d(x=-1, y=-1),
+        Translation2d(x=-1, y=1),
+    )
+    zero = SwerveModulePosition()
+    odometry = SwerveDrive4Odometry(kinematics, Rotation2d(), [zero, zero, zero, zero])
+
+    fl = SwerveModulePosition()
+    fr = SwerveModulePosition()
+    bl = SwerveModulePosition()
+    br = SwerveModulePosition()
+
+    trajectory = TrajectoryGenerator.generateTrajectory(
+        [
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(45)),
+            Pose2d(x=3, y=0, rotation=Rotation2d.fromDegrees(-90)),
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(135)),
+            Pose2d(x=-3, y=0, rotation=Rotation2d.fromDegrees(-90)),
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(45)),
+        ],
+        TrajectoryConfig(maxVelocity=5.0, maxAcceleration=2.0),
+    )
+
+    random.seed(4915)
+
+    dt = 0.02
+    t = 0
+
+    max_error = -float("inf")
+    error_sum = 0
+
+    while t < trajectory.totalTime():
+        ground_truth_state = trajectory.sample(t)
+
+        fl.distance += (
+            ground_truth_state.velocity * dt
+            + 0.5 * ground_truth_state.acceleration * dt * dt
+        )
+        fr.distance += (
+            ground_truth_state.velocity * dt
+            + 0.5 * ground_truth_state.acceleration * dt * dt
+        )
+        bl.distance += (
+            ground_truth_state.velocity * dt
+            + 0.5 * ground_truth_state.acceleration * dt * dt
+        )
+        br.distance += (
+            ground_truth_state.velocity * dt
+            + 0.5 * ground_truth_state.acceleration * dt * dt
+        )
+
+        fl.angle = ground_truth_state.pose.rotation()
+        fr.angle = ground_truth_state.pose.rotation()
+        bl.angle = ground_truth_state.pose.rotation()
+        br.angle = ground_truth_state.pose.rotation()
+
+        xhat = odometry.update(
+            Rotation2d(random.gauss(0.0, 1.0) * 0.05), [fl, fr, bl, br]
+        )
+        error = ground_truth_state.pose.translation().distance(xhat.translation())
+        if error > max_error:
+            max_error = error
+        error_sum += error
+
+        t += dt
+
+    assert error_sum / (trajectory.totalTime() / dt) < 0.06
+    assert max_error < 0.125
--- a/wpimath/src/test/python/kinematics/test_swerve_drive_odometry3d.py
+++ b/wpimath/src/test/python/kinematics/test_swerve_drive_odometry3d.py
@@ -0,0 +1,226 @@
+import pytest
+import math
+import random
+
+from wpimath import (
+    ChassisSpeeds,
+    Pose3d,
+    Pose2d,
+    Rotation2d,
+    Rotation3d,
+    SwerveDrive4Kinematics,
+    SwerveDrive4Odometry3d,
+    SwerveModulePosition,
+    TrajectoryGenerator,
+    TrajectoryConfig,
+    Translation2d,
+)
+
+kEpsilon = 0.01
+
+
+@pytest.fixture
+def odometry3d_test():
+    class SwerveDriveOdometry3dTest:
+        def __init__(self):
+            self.m_fl = Translation2d(x=12, y=12)
+            self.m_fr = Translation2d(x=12, y=-12)
+            self.m_bl = Translation2d(x=-12, y=12)
+            self.m_br = Translation2d(x=-12, y=-12)
+            self.m_kinematics = SwerveDrive4Kinematics(
+                self.m_fl, self.m_fr, self.m_bl, self.m_br
+            )
+            self.zero = SwerveModulePosition()
+            self.m_odometry = SwerveDrive4Odometry3d(
+                self.m_kinematics,
+                Rotation3d(),
+                [self.zero, self.zero, self.zero, self.zero],
+            )
+
+    return SwerveDriveOdometry3dTest()
+
+
+def test_initialize(odometry3d_test):
+    odometry = SwerveDrive4Odometry3d(
+        odometry3d_test.m_kinematics,
+        Rotation3d(),
+        [
+            odometry3d_test.zero,
+            odometry3d_test.zero,
+            odometry3d_test.zero,
+            odometry3d_test.zero,
+        ],
+        Pose3d(x=1, y=2, z=0, rotation=Rotation3d.fromDegrees(0, 0, 45)),
+    )
+
+    pose = odometry.getPose()
+
+    assert pose.x == pytest.approx(1, abs=kEpsilon)
+    assert pose.y == pytest.approx(2, abs=kEpsilon)
+    assert pose.z == pytest.approx(0, abs=kEpsilon)
+    assert pose.rotation().toRotation2d().degrees() == pytest.approx(45, abs=kEpsilon)
+
+
+def test_two_iterations(odometry3d_test):
+    position = SwerveModulePosition(distance=0.5, angle=Rotation2d(0))
+
+    odometry3d_test.m_odometry.resetPosition(
+        Rotation3d(),
+        [
+            odometry3d_test.zero,
+            odometry3d_test.zero,
+            odometry3d_test.zero,
+            odometry3d_test.zero,
+        ],
+        Pose3d(),
+    )
+
+    odometry3d_test.m_odometry.update(
+        Rotation3d(),
+        [
+            odometry3d_test.zero,
+            odometry3d_test.zero,
+            odometry3d_test.zero,
+            odometry3d_test.zero,
+        ],
+    )
+
+    pose = odometry3d_test.m_odometry.update(
+        Rotation3d(), [position, position, position, position]
+    )
+
+    assert pose.x == pytest.approx(0.5, abs=kEpsilon)
+    assert pose.y == pytest.approx(0.0, abs=kEpsilon)
+    assert pose.z == pytest.approx(0.0, abs=kEpsilon)
+    assert pose.rotation().toRotation2d().degrees() == pytest.approx(0.0, abs=kEpsilon)
+
+
+def test_90_degree_turn(odometry3d_test):
+    fl = SwerveModulePosition(distance=18.85, angle=Rotation2d.fromDegrees(90))
+    fr = SwerveModulePosition(distance=42.15, angle=Rotation2d.fromDegrees(26.565))
+    bl = SwerveModulePosition(distance=18.85, angle=Rotation2d.fromDegrees(-90))
+    br = SwerveModulePosition(distance=42.15, angle=Rotation2d.fromDegrees(-26.565))
+
+    odometry3d_test.m_odometry.resetPosition(
+        Rotation3d(),
+        [
+            odometry3d_test.zero,
+            odometry3d_test.zero,
+            odometry3d_test.zero,
+            odometry3d_test.zero,
+        ],
+        Pose3d(),
+    )
+    pose = odometry3d_test.m_odometry.update(
+        Rotation3d.fromDegrees(0, 0, 90), [fl, fr, bl, br]
+    )
+
+    assert pose.x == pytest.approx(12.0, abs=kEpsilon)
+    assert pose.y == pytest.approx(12.0, abs=kEpsilon)
+    assert pose.z == pytest.approx(0.0, abs=kEpsilon)
+    assert pose.rotation().toRotation2d().degrees() == pytest.approx(90.0, abs=kEpsilon)
+
+
+def test_gyro_angle_reset(odometry3d_test):
+    odometry3d_test.m_odometry.resetPosition(
+        Rotation3d.fromDegrees(0, 0, 90),
+        [
+            odometry3d_test.zero,
+            odometry3d_test.zero,
+            odometry3d_test.zero,
+            odometry3d_test.zero,
+        ],
+        Pose3d(),
+    )
+
+    position = SwerveModulePosition(distance=0.5, angle=Rotation2d.fromDegrees(0))
+
+    pose = odometry3d_test.m_odometry.update(
+        Rotation3d.fromDegrees(0, 0, 90),
+        [position, position, position, position],
+    )
+
+    assert pose.x == pytest.approx(0.5, abs=kEpsilon)
+    assert pose.y == pytest.approx(0.0, abs=kEpsilon)
+    assert pose.z == pytest.approx(0.0, abs=kEpsilon)
+    assert pose.rotation().toRotation2d().degrees() == pytest.approx(0.0, abs=kEpsilon)
+
+
+def test_accuracy_facing_x_axis():
+    kinematics = SwerveDrive4Kinematics(
+        Translation2d(x=1, y=1),
+        Translation2d(x=1, y=-1),
+        Translation2d(x=-1, y=-1),
+        Translation2d(x=-1, y=1),
+    )
+
+    zero = SwerveModulePosition()
+    odometry = SwerveDrive4Odometry3d(
+        kinematics, Rotation3d(), [zero, zero, zero, zero]
+    )
+
+    fl = SwerveModulePosition()
+    fr = SwerveModulePosition()
+    bl = SwerveModulePosition()
+    br = SwerveModulePosition()
+
+    trajectory = TrajectoryGenerator.generateTrajectory(
+        [
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(45)),
+            Pose2d(x=3, y=0, rotation=Rotation2d.fromDegrees(-90)),
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(135)),
+            Pose2d(x=-3, y=0, rotation=Rotation2d.fromDegrees(-90)),
+            Pose2d(x=0, y=0, rotation=Rotation2d.fromDegrees(45)),
+        ],
+        TrajectoryConfig(maxVelocity=5.0, maxAcceleration=2.0),
+    )
+
+    random.seed(4915)
+
+    dt = 0.02
+    t = 0
+
+    max_error = -float("inf")
+    error_sum = 0
+
+    while t < trajectory.totalTime():
+        ground_truth_state = trajectory.sample(t)
+
+        fl.distance += (
+            ground_truth_state.velocity * dt
+            + 0.5 * ground_truth_state.acceleration * dt * dt
+        )
+        fr.distance += (
+            ground_truth_state.velocity * dt
+            + 0.5 * ground_truth_state.acceleration * dt * dt
+        )
+        bl.distance += (
+            ground_truth_state.velocity * dt
+            + 0.5 * ground_truth_state.acceleration * dt * dt
+        )
+        br.distance += (
+            ground_truth_state.velocity * dt
+            + 0.5 * ground_truth_state.acceleration * dt * dt
+        )
+
+        fl.angle = ground_truth_state.pose.rotation()
+        fr.angle = ground_truth_state.pose.rotation()
+        bl.angle = ground_truth_state.pose.rotation()
+        br.angle = ground_truth_state.pose.rotation()
+
+        xhat = odometry.update(
+            Rotation3d(0, 0, random.gauss(0.0, 1.0) * 0.05),
+            [fl, fr, bl, br],
+        )
+        error = ground_truth_state.pose.translation().distance(
+            xhat.translation().toTranslation2d()
+        )
+
+        if error > max_error:
+            max_error = error
+        error_sum += error
+
+        t += dt
+
+    assert error_sum / (trajectory.totalTime() / dt) < 0.06
+    assert max_error < 0.125
--- a/wpimath/src/test/python/kinematics/test_swerve_module_position.py
+++ b/wpimath/src/test/python/kinematics/test_swerve_module_position.py
@@ -0,0 +1,19 @@
+import pytest
+
+from wpimath import Rotation2d, SwerveModulePosition
+
+
+def test_equality():
+    position1 = SwerveModulePosition(distance=2, angle=Rotation2d.fromDegrees(90))
+    position2 = SwerveModulePosition(distance=2, angle=Rotation2d.fromDegrees(90))
+
+    assert position1 == position2
+
+
+def test_inequality():
+    position1 = SwerveModulePosition(distance=1, angle=Rotation2d.fromDegrees(90))
+    position2 = SwerveModulePosition(distance=2, angle=Rotation2d.fromDegrees(90))
+    position3 = SwerveModulePosition(distance=1, angle=Rotation2d.fromDegrees(89))
+
+    assert position1 != position2
+    assert position1 != position3
--- a/wpimath/src/test/python/kinematics/test_swerve_module_state.py
+++ b/wpimath/src/test/python/kinematics/test_swerve_module_state.py
@@ -0,0 +1,141 @@
+import pytest
+import math
+
+from wpimath import Rotation2d, SwerveModuleState
+
+
+kEpsilon = 1e-9
+
+
+def test_optimize():
+    angle_a = Rotation2d.fromDegrees(45)
+    ref_a = SwerveModuleState(-2, Rotation2d.fromDegrees(180))
+    ref_a.optimize(angle_a)
+
+    assert ref_a.speed == pytest.approx(2.0, abs=kEpsilon)
+    assert ref_a.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
+
+    angle_b = Rotation2d.fromDegrees(-50)
+    ref_b = SwerveModuleState(4.7, Rotation2d.fromDegrees(41))
+    ref_b.optimize(angle_b)
+
+    assert ref_b.speed == pytest.approx(-4.7, abs=kEpsilon)
+    assert ref_b.angle.degrees() == pytest.approx(-139.0, abs=kEpsilon)
+
+
+def test_no_optimize():
+    angle_a = Rotation2d.fromDegrees(0)
+    ref_a = SwerveModuleState(2, Rotation2d.fromDegrees(89))
+    ref_a.optimize(angle_a)
+
+    assert ref_a.speed == pytest.approx(2.0, abs=kEpsilon)
+    assert ref_a.angle.degrees() == pytest.approx(89.0, abs=kEpsilon)
+
+    angle_b = Rotation2d.fromDegrees(0)
+    ref_b = SwerveModuleState(-2, Rotation2d.fromDegrees(-2))
+    ref_b.optimize(angle_b)
+
+    assert ref_b.speed == pytest.approx(-2.0, abs=kEpsilon)
+    assert ref_b.angle.degrees() == pytest.approx(-2.0, abs=kEpsilon)
+
+
+def test_cosine_scaling():
+    angle_a = Rotation2d.fromDegrees(0)
+    ref_a = SwerveModuleState(2, Rotation2d.fromDegrees(45))
+    ref_a.cosineScale(angle_a)
+
+    assert ref_a.speed == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
+    assert ref_a.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+
+    angle_b = Rotation2d.fromDegrees(45)
+    ref_b = SwerveModuleState(2, Rotation2d.fromDegrees(45))
+    ref_b.cosineScale(angle_b)
+
+    assert ref_b.speed == pytest.approx(2.0, abs=kEpsilon)
+    assert ref_b.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+
+    angle_c = Rotation2d.fromDegrees(-45)
+    ref_c = SwerveModuleState(2, Rotation2d.fromDegrees(45))
+    ref_c.cosineScale(angle_c)
+
+    assert ref_c.speed == pytest.approx(0.0, abs=kEpsilon)
+    assert ref_c.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+
+    angle_d = Rotation2d.fromDegrees(135)
+    ref_d = SwerveModuleState(2, Rotation2d.fromDegrees(45))
+    ref_d.cosineScale(angle_d)
+
+    assert ref_d.speed == pytest.approx(0.0, abs=kEpsilon)
+    assert ref_d.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+
+    angle_e = Rotation2d.fromDegrees(-135)
+    ref_e = SwerveModuleState(2, Rotation2d.fromDegrees(45))
+    ref_e.cosineScale(angle_e)
+
+    assert ref_e.speed == pytest.approx(-2.0, abs=kEpsilon)
+    assert ref_e.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+
+    angle_f = Rotation2d.fromDegrees(180)
+    ref_f = SwerveModuleState(2, Rotation2d.fromDegrees(45))
+    ref_f.cosineScale(angle_f)
+
+    assert ref_f.speed == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
+    assert ref_f.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+
+    angle_g = Rotation2d.fromDegrees(0)
+    ref_g = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
+    ref_g.cosineScale(angle_g)
+
+    assert ref_g.speed == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
+    assert ref_g.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+
+    angle_h = Rotation2d.fromDegrees(45)
+    ref_h = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
+    ref_h.cosineScale(angle_h)
+
+    assert ref_h.speed == pytest.approx(-2.0, abs=kEpsilon)
+    assert ref_h.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+
+    angle_i = Rotation2d.fromDegrees(-45)
+    ref_i = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
+    ref_i.cosineScale(angle_i)
+
+    assert ref_i.speed == pytest.approx(0.0, abs=kEpsilon)
+    assert ref_i.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+
+    angle_j = Rotation2d.fromDegrees(135)
+    ref_j = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
+    ref_j.cosineScale(angle_j)
+
+    assert ref_j.speed == pytest.approx(0.0, abs=kEpsilon)
+    assert ref_j.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+
+    angle_k = Rotation2d.fromDegrees(-135)
+    ref_k = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
+    ref_k.cosineScale(angle_k)
+
+    assert ref_k.speed == pytest.approx(2.0, abs=kEpsilon)
+    assert ref_k.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+
+    angle_l = Rotation2d.fromDegrees(180)
+    ref_l = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
+    ref_l.cosineScale(angle_l)
+
+    assert ref_l.speed == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
+    assert ref_l.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+
+
+def test_equality():
+    state1 = SwerveModuleState(2, Rotation2d.fromDegrees(90))
+    state2 = SwerveModuleState(2, Rotation2d.fromDegrees(90))
+
+    assert state1 == state2
+
+
+def test_inequality():
+    state1 = SwerveModuleState(1, Rotation2d.fromDegrees(90))
+    state2 = SwerveModuleState(2, Rotation2d.fromDegrees(90))
+    state3 = SwerveModuleState(1, Rotation2d.fromDegrees(89))
+
+    assert state1 != state2
+    assert state1 != state3