diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
index a5205dcfc7..45c3f3bb23 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
@@ -167,7 +167,7 @@ public class SwerveDriveKinematics
       double y = moduleStatesMatrix.get(i * 2 + 1, 0);
 
       double speed = Math.hypot(x, y);
-      Rotation2d angle = new Rotation2d(x, y);
+      Rotation2d angle = speed > 1e-6 ? new Rotation2d(x, y) : m_moduleHeadings[i];
 
       moduleStates[i] = new SwerveModuleState(speed, angle);
       m_moduleHeadings[i] = angle;
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
index b9bf34016e..8c5e18b359 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -66,7 +66,8 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
     units::meters_per_second_t y{moduleStateMatrix(i * 2 + 1, 0)};
 
     auto speed = units::math::hypot(x, y);
-    Rotation2d rotation{x.value(), y.value()};
+    auto rotation = speed > 1e-6_mps ? Rotation2d{x.value(), y.value()}
+                                     : m_moduleHeadings[i];
 
     moduleStates[i] = {speed, rotation};
     m_moduleHeadings[i] = rotation;