diff --git a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
index 5c8e3f15fa..e05720455d 100644
--- a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
@@ -2,8 +2,6 @@
 // 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 <cmath>
-
 #include <gtest/gtest.h>
 
 #include "frc/EigenCore.h"
@@ -22,10 +20,7 @@ Vectord<2> StateDynamics(const Vectord<2>& x) {
 }
 
 TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
-  std::function<Vectord<2>(const Vectord<2>&, const Vectord<1>&)>
-      modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); };
-
-  frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{modelDynamics,
+  frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{&Dynamics,
                                                                 20_ms};
 
   Vectord<2> r{2, 2};
@@ -35,14 +30,8 @@ TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
 }
 
 TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
-  std::function<Vectord<2>(const Vectord<2>&)> modelDynamics = [](auto& x) {
-    return StateDynamics(x);
-  };
-
-  Matrixd<2, 1> B{0, 1};
-
-  frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{modelDynamics,
-                                                                B, 20_ms};
+  frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
+      &StateDynamics, Matrixd<2, 1>{{0.0}, {1.0}}, 20_ms};
 
   Vectord<2> r{2, 2};
   Vectord<2> nextR{3, 3};