diff --git a/aos/util/math_test.cc b/aos/util/math_test.cc
new file mode 100644
index 0000000..a243e53
--- /dev/null
+++ b/aos/util/math_test.cc
@@ -0,0 +1,89 @@
+#include "aos/util/math.h"
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace math {
+namespace testing {
+
+bool AngleEqual(double a1, double a2) {
+  double diff = a1 - a2;
+  return ::std::fmod(diff, 2 * M_PI) == 0.0;
+}
+
+bool AngleInBounds(double a) { return a <= M_PI && a > -M_PI; }
+
+// Check that something
+void ExpectNormalizesCorrectly(double a) {
+  double b = NormalizeAngle(a);
+  EXPECT_PRED2(AngleEqual, a, b);
+  EXPECT_PRED1(AngleInBounds, b);
+}
+
+void ExpectDiffsCorrectly(double a, double b) {
+  double diff = DiffAngle(a, b);
+
+  EXPECT_PRED1(AngleInBounds, diff) << "a: " << a << " b: " << b;
+  EXPECT_PRED2(AngleEqual, a, b + diff);
+}
+
+// Checks that normalizing positive and negative angles in and out of bounds
+// works correctly.
+TEST(MathTest, NormalizeAngleTest) {
+  ExpectNormalizesCorrectly(0.0);
+  ExpectNormalizesCorrectly(1.0);
+  ExpectNormalizesCorrectly(-1.0);
+  ExpectNormalizesCorrectly(M_PI);
+  ExpectNormalizesCorrectly(2.0 * M_PI);
+  ExpectNormalizesCorrectly(-2.0 * M_PI);
+  ExpectNormalizesCorrectly(100.0);
+  ExpectNormalizesCorrectly(-100.0);
+}
+
+// Checks that subtracting one angle from another works for various combinations
+// of angles.
+TEST(MathTest, DiffAngleTest) {
+  ExpectDiffsCorrectly(0.0, 0.0);
+  ExpectDiffsCorrectly(1.0, 0.0);
+  ExpectDiffsCorrectly(0.0, 1.0);
+  ExpectDiffsCorrectly(-1.0, 1.0);
+  ExpectDiffsCorrectly(100.0, 1.0);
+  ExpectDiffsCorrectly(-100.0, 1.0);
+  ExpectDiffsCorrectly(M_PI, M_PI);
+}
+
+// Check that points that are really counter-clockwise show up as
+// counter-clockwise, and that invalid orderings don't show up as CCW.
+TEST(MathTest, PointsAreCCWTest) {
+  Eigen::Vector2d a, b, c;
+  a << 0.0, 0.0;
+  b << 1.0, 0.0;
+  c << 0.0, 1.0;
+  // Because there are only six orderings we can just enumerate them all.
+  EXPECT_TRUE(PointsAreCCW<double>(a, b, c));
+  EXPECT_FALSE(PointsAreCCW<double>(a, c, b));
+  EXPECT_FALSE(PointsAreCCW<double>(b, a, c));
+  EXPECT_TRUE(PointsAreCCW<double>(b, c, a));
+  EXPECT_TRUE(PointsAreCCW<double>(c, a, b));
+  EXPECT_FALSE(PointsAreCCW<double>(c, b, a));
+}
+
+// Collinear points should always appear as non-counter-clockwise.
+TEST(MathTest, CollinearPointsAreCCWTest) {
+  // Create three collinear points along the X-axis and check that every
+  // combination is not CCW.
+  Eigen::Vector2d a, b, c;
+  a << 0.0, 0.0;
+  b << 1.0, 0.0;
+  c << 2.0, 0.0;
+  EXPECT_FALSE(PointsAreCCW<double>(a, b, c));
+  EXPECT_FALSE(PointsAreCCW<double>(a, c, b));
+  EXPECT_FALSE(PointsAreCCW<double>(b, a, c));
+  EXPECT_FALSE(PointsAreCCW<double>(b, c, a));
+  EXPECT_FALSE(PointsAreCCW<double>(c, a, b));
+  EXPECT_FALSE(PointsAreCCW<double>(c, b, a));
+}
+
+}  // namespace testing
+}  // namespace math
+}  // namespace aos