diff --git a/libraries/AP_Math/tests/test_control.cpp b/libraries/AP_Math/tests/test_control.cpp
new file mode 100644
index 0000000000..ec60a4ffeb
--- /dev/null
+++ b/libraries/AP_Math/tests/test_control.cpp
@@ -0,0 +1,37 @@
+#include <AP_gtest.h>
+
+#include <AP_Math/AP_Math.h>
+#include <AP_Math/vector2.h>
+#include <AP_Math/vector3.h>
+#include <AP_Math/control.h>
+
+TEST(Control, test_control)
+{
+    postype_t pos_start = 17;
+    float vel_start = 20;
+
+    float vel = vel_start;
+    postype_t pos = pos_start;
+    const float dt = 0.01;
+    const float accel = 1.0;
+
+    update_pos_vel_accel(pos, vel, accel, dt, 0, 0, 0);
+    EXPECT_FLOAT_EQ(vel, vel_start + accel*dt);
+    EXPECT_FLOAT_EQ(pos, pos_start + 0.5*(vel+vel_start)*dt);
+
+    vel = vel_start;
+    pos = pos_start;
+    update_pos_vel_accel(pos, vel, accel, dt, 1.0, 1.0, 1.0);
+    EXPECT_FLOAT_EQ(vel, vel_start);
+    EXPECT_FLOAT_EQ(pos, pos_start);
+
+    vel = vel_start;
+    pos = pos_start;
+    update_pos_vel_accel(pos, vel, accel, dt, -1.0, 1.0, 1.0);
+    EXPECT_FLOAT_EQ(vel, vel_start + accel*dt);
+    EXPECT_FLOAT_EQ(pos, pos_start + 0.5*(vel+vel_start)*dt);
+}
+
+
+AP_GTEST_MAIN()
+int hal = 0;