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 dd91f123ec..aa5338e6eb 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
@@ -99,7 +99,7 @@ public class SwerveDriveKinematics
           "Number of headings is not consistent with number of module locations provided in "
               + "constructor");
     }
-    m_moduleHeadings = Arrays.copyOf(moduleHeadings, 4);
+    m_moduleHeadings = Arrays.copyOf(moduleHeadings, m_numModules);
   }
 
   /**