diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleStateTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleStateTest.java
new file mode 100644
index 0000000000..49ca2f642e
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleStateTest.java
@@ -0,0 +1,53 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import org.junit.jupiter.api.Test;
+
+class SwerveModuleStateTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testOptimize() {
+    var angleA = Rotation2d.fromDegrees(45);
+    var refA = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(180));
+    var optimizedA = SwerveModuleState.optimize(refA, angleA);
+
+    assertAll(
+        () -> assertEquals(2.0, optimizedA.speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, optimizedA.angle.getDegrees(), kEpsilon));
+
+    var angleB = Rotation2d.fromDegrees(-50);
+    var refB = new SwerveModuleState(4.7, Rotation2d.fromDegrees(41));
+    var optimizedB = SwerveModuleState.optimize(refB, angleB);
+
+    assertAll(
+        () -> assertEquals(-4.7, optimizedB.speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(-139.0, optimizedB.angle.getDegrees(), kEpsilon));
+  }
+
+  @Test
+  void testNoOptimize() {
+    var angleA = Rotation2d.fromDegrees(0);
+    var refA = new SwerveModuleState(2.0, Rotation2d.fromDegrees(89));
+    var optimizedA = SwerveModuleState.optimize(refA, angleA);
+
+    assertAll(
+        () -> assertEquals(2.0, optimizedA.speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(89.0, optimizedA.angle.getDegrees(), kEpsilon));
+
+    var angleB = Rotation2d.fromDegrees(0);
+    var refB = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(-2));
+    var optimizedB = SwerveModuleState.optimize(refB, angleB);
+
+    assertAll(
+        () -> assertEquals(-2.0, optimizedB.speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(-2.0, optimizedB.angle.getDegrees(), kEpsilon));
+  }
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
new file mode 100644
index 0000000000..13223db287
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/SwerveModuleState.h"
+#include "gtest/gtest.h"
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(SwerveModuleState, Optimize) {
+  frc::Rotation2d angleA{45_deg};
+  frc::SwerveModuleState refA{-2_mps, 180_deg};
+  auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA);
+
+  EXPECT_NEAR(optimizedA.speed.to<double>(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.angle.Degrees().to<double>(), 0.0, kEpsilon);
+
+  frc::Rotation2d angleB{-50_deg};
+  frc::SwerveModuleState refB{4.7_mps, 41_deg};
+  auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB);
+
+  EXPECT_NEAR(optimizedB.speed.to<double>(), -4.7, kEpsilon);
+  EXPECT_NEAR(optimizedB.angle.Degrees().to<double>(), -139.0, kEpsilon);
+}
+
+TEST(SwerveModuleState, NoOptimize) {
+  frc::Rotation2d angleA{0_deg};
+  frc::SwerveModuleState refA{2_mps, 89_deg};
+  auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA);
+
+  EXPECT_NEAR(optimizedA.speed.to<double>(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.angle.Degrees().to<double>(), 89.0, kEpsilon);
+
+  frc::Rotation2d angleB{0_deg};
+  frc::SwerveModuleState refB{-2_mps, -2_deg};
+  auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB);
+
+  EXPECT_NEAR(optimizedB.speed.to<double>(), -2.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.angle.Degrees().to<double>(), -2.0, kEpsilon);
+}