diff --git a/frc971/math/interpolate_test.cc b/frc971/math/interpolate_test.cc
new file mode 100644
index 0000000..4d52e64
--- /dev/null
+++ b/frc971/math/interpolate_test.cc
@@ -0,0 +1,20 @@
+#include "frc971/math/interpolate.h"
+
+#include "gtest/gtest.h"
+#include <Eigen/Core>
+
+namespace frc971::math::testing {
+// Tests that when we "interpolate" exactly to the ends of the interpolation
+// that we get the correct result.
+TEST(InterpolateTest, ExactEnds) {
+  const Eigen::Vector3d a(1.0, 2.0, 3.0), b(4.0, 5.0, 6.0);
+  ASSERT_EQ(a, lerp(a, b, 0.0));
+  ASSERT_EQ(Eigen::Vector3d(2.5, 3.5, 4.5), lerp(a, b, 0.5));
+  ASSERT_EQ(b, lerp(a, b, 1.0));
+
+  ASSERT_EQ(a, Interpolate(10.0, 20.0, a, b, 10.0));
+  ASSERT_EQ(b, Interpolate(10.0, 20.0, a, b, 20.0));
+  ASSERT_EQ(Eigen::Vector3d(2.5, 3.5, 4.5),
+            Interpolate(10.0, 20.0, a, b, 15.0));
+}
+}  // namespace frc971::math::testing