SCRIPT: wpiformat

wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp

@@ -7,13 +7,16 @@
 #include "wpi/math/util/ComputerVisionUtil.hpp"
 
 TEST(ComputerVisionUtilTest, ObjectToRobotPose) {
-  wpi::math::Pose3d robot{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 30_deg}};
-  wpi::math::Transform3d cameraToObject{wpi::math::Translation3d{1_m, 1_m, 1_m},
-                                  wpi::math::Rotation3d{0_deg, -20_deg, 45_deg}};
-  wpi::math::Transform3d robotToCamera{wpi::math::Translation3d{1_m, 0_m, 2_m},
-                                 wpi::math::Rotation3d{0_deg, 0_deg, 25_deg}};
+  wpi::math::Pose3d robot{1_m, 2_m, 0_m,
+                          wpi::math::Rotation3d{0_deg, 0_deg, 30_deg}};
+  wpi::math::Transform3d cameraToObject{
+      wpi::math::Translation3d{1_m, 1_m, 1_m},
+      wpi::math::Rotation3d{0_deg, -20_deg, 45_deg}};
+  wpi::math::Transform3d robotToCamera{
+      wpi::math::Translation3d{1_m, 0_m, 2_m},
+      wpi::math::Rotation3d{0_deg, 0_deg, 25_deg}};
   wpi::math::Pose3d object = robot + robotToCamera + cameraToObject;
 
-  EXPECT_EQ(robot,
-            wpi::math::ObjectToRobotPose(object, cameraToObject, robotToCamera));
+  EXPECT_EQ(robot, wpi::math::ObjectToRobotPose(object, cameraToObject,
+                                                robotToCamera));
 }

wpimath/src/test/native/cpp/DARETest.cpp

@@ -15,56 +15,72 @@
 #include "wpi/util/print.hpp"
 
 // 2x1
-extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
+extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>,
+                                    wpi::math::DAREError>
 wpi::math::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
-                const Eigen::Matrix<double, 2, 1>& B,
-                const Eigen::Matrix<double, 2, 2>& Q,
-                const Eigen::Matrix<double, 1, 1>& R, bool checkPreconditions);
-extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
+                      const Eigen::Matrix<double, 2, 1>& B,
+                      const Eigen::Matrix<double, 2, 2>& Q,
+                      const Eigen::Matrix<double, 1, 1>& R,
+                      bool checkPreconditions);
+extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>,
+                                    wpi::math::DAREError>
 wpi::math::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
-                const Eigen::Matrix<double, 2, 1>& B,
-                const Eigen::Matrix<double, 2, 2>& Q,
-                const Eigen::Matrix<double, 1, 1>& R,
-                const Eigen::Matrix<double, 2, 1>& N, bool checkPreconditions);
+                      const Eigen::Matrix<double, 2, 1>& B,
+                      const Eigen::Matrix<double, 2, 2>& Q,
+                      const Eigen::Matrix<double, 1, 1>& R,
+                      const Eigen::Matrix<double, 2, 1>& N,
+                      bool checkPreconditions);
 
 // 4x1
-extern template wpi::util::expected<Eigen::Matrix<double, 4, 4>, wpi::math::DAREError>
+extern template wpi::util::expected<Eigen::Matrix<double, 4, 4>,
+                                    wpi::math::DAREError>
 wpi::math::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
-                const Eigen::Matrix<double, 4, 1>& B,
-                const Eigen::Matrix<double, 4, 4>& Q,
-                const Eigen::Matrix<double, 1, 1>& R, bool checkPreconditions);
-extern template wpi::util::expected<Eigen::Matrix<double, 4, 4>, wpi::math::DAREError>
+                      const Eigen::Matrix<double, 4, 1>& B,
+                      const Eigen::Matrix<double, 4, 4>& Q,
+                      const Eigen::Matrix<double, 1, 1>& R,
+                      bool checkPreconditions);
+extern template wpi::util::expected<Eigen::Matrix<double, 4, 4>,
+                                    wpi::math::DAREError>
 wpi::math::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
-                const Eigen::Matrix<double, 4, 1>& B,
-                const Eigen::Matrix<double, 4, 4>& Q,
-                const Eigen::Matrix<double, 1, 1>& R,
-                const Eigen::Matrix<double, 4, 1>& N, bool checkPreconditions);
+                      const Eigen::Matrix<double, 4, 1>& B,
+                      const Eigen::Matrix<double, 4, 4>& Q,
+                      const Eigen::Matrix<double, 1, 1>& R,
+                      const Eigen::Matrix<double, 4, 1>& N,
+                      bool checkPreconditions);
 
 // 2x2
-extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
+extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>,
+                                    wpi::math::DAREError>
 wpi::math::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
-                const Eigen::Matrix<double, 2, 2>& B,
-                const Eigen::Matrix<double, 2, 2>& Q,
-                const Eigen::Matrix<double, 2, 2>& R, bool checkPreconditions);
-extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
+                      const Eigen::Matrix<double, 2, 2>& B,
+                      const Eigen::Matrix<double, 2, 2>& Q,
+                      const Eigen::Matrix<double, 2, 2>& R,
+                      bool checkPreconditions);
+extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>,
+                                    wpi::math::DAREError>
 wpi::math::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
-                const Eigen::Matrix<double, 2, 2>& B,
-                const Eigen::Matrix<double, 2, 2>& Q,
-                const Eigen::Matrix<double, 2, 2>& R,
-                const Eigen::Matrix<double, 2, 2>& N, bool checkPreconditions);
+                      const Eigen::Matrix<double, 2, 2>& B,
+                      const Eigen::Matrix<double, 2, 2>& Q,
+                      const Eigen::Matrix<double, 2, 2>& R,
+                      const Eigen::Matrix<double, 2, 2>& N,
+                      bool checkPreconditions);
 
 // 2x3
-extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
+extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>,
+                                    wpi::math::DAREError>
 wpi::math::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
-                const Eigen::Matrix<double, 2, 3>& B,
-                const Eigen::Matrix<double, 2, 2>& Q,
-                const Eigen::Matrix<double, 3, 3>& R, bool checkPreconditions);
-extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
+                      const Eigen::Matrix<double, 2, 3>& B,
+                      const Eigen::Matrix<double, 2, 2>& Q,
+                      const Eigen::Matrix<double, 3, 3>& R,
+                      bool checkPreconditions);
+extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>,
+                                    wpi::math::DAREError>
 wpi::math::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
-                const Eigen::Matrix<double, 2, 3>& B,
-                const Eigen::Matrix<double, 2, 2>& Q,
-                const Eigen::Matrix<double, 3, 3>& R,
-                const Eigen::Matrix<double, 2, 3>& N, bool checkPreconditions);
+                      const Eigen::Matrix<double, 2, 3>& B,
+                      const Eigen::Matrix<double, 2, 2>& Q,
+                      const Eigen::Matrix<double, 3, 3>& R,
+                      const Eigen::Matrix<double, 2, 3>& N,
+                      bool checkPreconditions);
 
 void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs,
                        double tolerance) {
@@ -134,7 +150,8 @@ TEST(DARETest, NonInvertibleA_ABQR) {
   wpi::math::Matrixd<4, 4> A{
       {0.5, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}};
   wpi::math::Matrixd<4, 1> B{{0}, {0}, {0}, {1}};
-  wpi::math::Matrixd<4, 4> Q{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
+  wpi::math::Matrixd<4, 4> Q{
+      {1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
   wpi::math::Matrixd<1, 1> R{0.25};
 
   auto ret = wpi::math::DARE<4, 1>(A, B, Q, R);
@@ -153,7 +170,8 @@ TEST(DARETest, NonInvertibleA_ABQRN) {
   wpi::math::Matrixd<4, 4> A{
       {0.5, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}};
   wpi::math::Matrixd<4, 1> B{{0}, {0}, {0}, {1}};
-  wpi::math::Matrixd<4, 4> Q{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
+  wpi::math::Matrixd<4, 4> Q{
+      {1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
   wpi::math::Matrixd<1, 1> R{0.25};
 
   wpi::math::Matrixd<4, 4> Aref{

wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp

@@ -6,12 +6,14 @@
 
 template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
 wpi::math::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
-                const Eigen::Matrix<double, 2, 1>& B,
-                const Eigen::Matrix<double, 2, 2>& Q,
-                const Eigen::Matrix<double, 1, 1>& R, bool checkPreconditions);
+                      const Eigen::Matrix<double, 2, 1>& B,
+                      const Eigen::Matrix<double, 2, 2>& Q,
+                      const Eigen::Matrix<double, 1, 1>& R,
+                      bool checkPreconditions);
 template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
 wpi::math::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
-                const Eigen::Matrix<double, 2, 1>& B,
-                const Eigen::Matrix<double, 2, 2>& Q,
-                const Eigen::Matrix<double, 1, 1>& R,
-                const Eigen::Matrix<double, 2, 1>& N, bool checkPreconditions);
+                      const Eigen::Matrix<double, 2, 1>& B,
+                      const Eigen::Matrix<double, 2, 2>& Q,
+                      const Eigen::Matrix<double, 1, 1>& R,
+                      const Eigen::Matrix<double, 2, 1>& N,
+                      bool checkPreconditions);

wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp

@@ -6,12 +6,14 @@
 
 template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
 wpi::math::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
-                const Eigen::Matrix<double, 2, 2>& B,
-                const Eigen::Matrix<double, 2, 2>& Q,
-                const Eigen::Matrix<double, 2, 2>& R, bool checkPreconditions);
+                      const Eigen::Matrix<double, 2, 2>& B,
+                      const Eigen::Matrix<double, 2, 2>& Q,
+                      const Eigen::Matrix<double, 2, 2>& R,
+                      bool checkPreconditions);
 template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
 wpi::math::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
-                const Eigen::Matrix<double, 2, 2>& B,
-                const Eigen::Matrix<double, 2, 2>& Q,
-                const Eigen::Matrix<double, 2, 2>& R,
-                const Eigen::Matrix<double, 2, 2>& N, bool checkPreconditions);
+                      const Eigen::Matrix<double, 2, 2>& B,
+                      const Eigen::Matrix<double, 2, 2>& Q,
+                      const Eigen::Matrix<double, 2, 2>& R,
+                      const Eigen::Matrix<double, 2, 2>& N,
+                      bool checkPreconditions);

wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp

@@ -6,12 +6,14 @@
 
 template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
 wpi::math::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
-                const Eigen::Matrix<double, 2, 3>& B,
-                const Eigen::Matrix<double, 2, 2>& Q,
-                const Eigen::Matrix<double, 3, 3>& R, bool checkPreconditions);
+                      const Eigen::Matrix<double, 2, 3>& B,
+                      const Eigen::Matrix<double, 2, 2>& Q,
+                      const Eigen::Matrix<double, 3, 3>& R,
+                      bool checkPreconditions);
 template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
 wpi::math::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
-                const Eigen::Matrix<double, 2, 3>& B,
-                const Eigen::Matrix<double, 2, 2>& Q,
-                const Eigen::Matrix<double, 3, 3>& R,
-                const Eigen::Matrix<double, 2, 3>& N, bool checkPreconditions);
+                      const Eigen::Matrix<double, 2, 3>& B,
+                      const Eigen::Matrix<double, 2, 2>& Q,
+                      const Eigen::Matrix<double, 3, 3>& R,
+                      const Eigen::Matrix<double, 2, 3>& N,
+                      bool checkPreconditions);

wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp

@@ -7,12 +7,14 @@
 
 template wpi::util::expected<Eigen::Matrix<double, 4, 4>, wpi::math::DAREError>
 wpi::math::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
-                const Eigen::Matrix<double, 4, 1>& B,
-                const Eigen::Matrix<double, 4, 4>& Q,
-                const Eigen::Matrix<double, 1, 1>& R, bool checkPreconditions);
+                      const Eigen::Matrix<double, 4, 1>& B,
+                      const Eigen::Matrix<double, 4, 4>& Q,
+                      const Eigen::Matrix<double, 1, 1>& R,
+                      bool checkPreconditions);
 template wpi::util::expected<Eigen::Matrix<double, 4, 4>, wpi::math::DAREError>
 wpi::math::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
-                const Eigen::Matrix<double, 4, 1>& B,
-                const Eigen::Matrix<double, 4, 4>& Q,
-                const Eigen::Matrix<double, 1, 1>& R,
-                const Eigen::Matrix<double, 4, 1>& N, bool checkPreconditions);
+                      const Eigen::Matrix<double, 4, 1>& B,
+                      const Eigen::Matrix<double, 4, 4>& Q,
+                      const Eigen::Matrix<double, 1, 1>& R,
+                      const Eigen::Matrix<double, 4, 1>& N,
+                      bool checkPreconditions);

wpimath/src/test/native/cpp/EigenTest.cpp

@@ -24,7 +24,7 @@ TEST(EigenTest, Multiplication) {
   const auto result2 = m3 * m4;
 
   wpi::math::Matrixd<2, 4> expectedResult2{{12.5, 5.55, 7.8, 14.3},
-                                     {22.13, 9.82, 13.28, 23.53}};
+                                           {22.13, 9.82, 13.28, 23.53}};
 
   EXPECT_TRUE(expectedResult2.isApprox(result2));
 }

wpimath/src/test/native/cpp/MathUtilTest.cpp

@@ -55,14 +55,14 @@ TEST(MathUtilTest, ApplyDeadbandArbitraryScale) {
 
 TEST(MathUtilTest, ApplyDeadbandUnits) {
   // < 0
-  EXPECT_UNITS_EQ(-20_rad,
-                  wpi::math::ApplyDeadband<wpi::units::radian_t>(-20_rad, 1_rad, 20_rad));
+  EXPECT_UNITS_EQ(-20_rad, wpi::math::ApplyDeadband<wpi::units::radian_t>(
+                               -20_rad, 1_rad, 20_rad));
 }
 
 TEST(MathUtilTest, ApplyDeadbandLargeMaxMagnitude) {
   EXPECT_DOUBLE_EQ(
-      80.0,
-      wpi::math::ApplyDeadband(100.0, 20.0, std::numeric_limits<double>::infinity()));
+      80.0, wpi::math::ApplyDeadband(100.0, 20.0,
+                                     std::numeric_limits<double>::infinity()));
 }
 
 TEST(MathUtilTest, ApplyDeadband2dUnityScale) {
@@ -91,35 +91,41 @@ TEST(MathUtilTest, ApplyDeadband2dUnityScale) {
 TEST(MathUtilTest, ApplyDeadband2dArbitraryScale) {
   EXPECT_EQ((Eigen::Vector2d{{0.0}, {2.5}}),
             wpi::math::ApplyDeadband(Eigen::Vector2d{{0.0}, {2.5}}, 0.02, 2.5));
-  EXPECT_EQ((Eigen::Vector2d{{0.0}, {-2.5}}),
-            wpi::math::ApplyDeadband(Eigen::Vector2d{{0.0}, {-2.5}}, 0.02, 2.5));
-  EXPECT_EQ((Eigen::Vector2d{{-2.5}, {0.0}}),
-            wpi::math::ApplyDeadband(Eigen::Vector2d{{-2.5}, {0.0}}, 0.02, 2.5));
+  EXPECT_EQ(
+      (Eigen::Vector2d{{0.0}, {-2.5}}),
+      wpi::math::ApplyDeadband(Eigen::Vector2d{{0.0}, {-2.5}}, 0.02, 2.5));
+  EXPECT_EQ(
+      (Eigen::Vector2d{{-2.5}, {0.0}}),
+      wpi::math::ApplyDeadband(Eigen::Vector2d{{-2.5}, {0.0}}, 0.02, 2.5));
 
   // == 0
   EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
             wpi::math::ApplyDeadband(Eigen::Vector2d{{0.0}, {0.0}}, 0.02, 2.5));
 
   // > 0
-  EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
-            wpi::math::ApplyDeadband(Eigen::Vector2d{{0.01}, {0.0}}, 0.02, 2.5));
-  EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
-            wpi::math::ApplyDeadband(Eigen::Vector2d{{0.02}, {0.0}}, 0.02, 2.5));
+  EXPECT_EQ(
+      (Eigen::Vector2d{{0.0}, {0.0}}),
+      wpi::math::ApplyDeadband(Eigen::Vector2d{{0.01}, {0.0}}, 0.02, 2.5));
+  EXPECT_EQ(
+      (Eigen::Vector2d{{0.0}, {0.0}}),
+      wpi::math::ApplyDeadband(Eigen::Vector2d{{0.02}, {0.0}}, 0.02, 2.5));
   EXPECT_EQ((Eigen::Vector2d{{2.5}, {0.0}}),
             wpi::math::ApplyDeadband(Eigen::Vector2d{{2.5}, {0.0}}, 0.02, 2.5));
 }
 
 TEST(MathUtilTest, ApplyDeadband2dLargeMaxMagnitude) {
-  EXPECT_EQ((Eigen::Vector2d{{80.0}, {0.0}}),
-            (wpi::math::ApplyDeadband(Eigen::Vector2d{{100.0}, {0.0}}, 20.0,
+  EXPECT_EQ(
+      (Eigen::Vector2d{{80.0}, {0.0}}),
+      (wpi::math::ApplyDeadband(Eigen::Vector2d{{100.0}, {0.0}}, 20.0,
                                 std::numeric_limits<double>::infinity())));
 }
 
 TEST(MathUtilTest, ApplyDeadband2dUnits) {
-  EXPECT_EQ((Eigen::Vector<wpi::units::meters_per_second_t, 2>{0_mps, 2.5_mps}),
-            wpi::math::ApplyDeadband(
-                Eigen::Vector<wpi::units::meters_per_second_t, 2>{0_mps, 2.5_mps},
-                0.02_mps, 2.5_mps));
+  EXPECT_EQ(
+      (Eigen::Vector<wpi::units::meters_per_second_t, 2>{0_mps, 2.5_mps}),
+      wpi::math::ApplyDeadband(
+          Eigen::Vector<wpi::units::meters_per_second_t, 2>{0_mps, 2.5_mps},
+          0.02_mps, 2.5_mps));
   EXPECT_EQ((Eigen::Vector<wpi::units::meters_per_second_t, 2>{1_mps, 0_mps}),
             wpi::math::ApplyDeadband(
                 Eigen::Vector<wpi::units::meters_per_second_t, 2>{1_mps, 0_mps},
@@ -154,9 +160,11 @@ TEST(MathUtilTest, CopyDirectionPowWithMaxMagnitude) {
   EXPECT_DOUBLE_EQ(-5.0, wpi::math::CopyDirectionPow(-5.0, 1.0, 10.0));
 
   EXPECT_DOUBLE_EQ(0.5 * 0.5 * 10, wpi::math::CopyDirectionPow(5.0, 2.0, 10.0));
-  EXPECT_DOUBLE_EQ(-0.5 * 0.5 * 10, wpi::math::CopyDirectionPow(-5.0, 2.0, 10.0));
+  EXPECT_DOUBLE_EQ(-0.5 * 0.5 * 10,
+                   wpi::math::CopyDirectionPow(-5.0, 2.0, 10.0));
 
-  EXPECT_DOUBLE_EQ(std::sqrt(0.5) * 10, wpi::math::CopyDirectionPow(5.0, 0.5, 10.0));
+  EXPECT_DOUBLE_EQ(std::sqrt(0.5) * 10,
+                   wpi::math::CopyDirectionPow(5.0, 0.5, 10.0));
   EXPECT_DOUBLE_EQ(-std::sqrt(0.5) * 10,
                    wpi::math::CopyDirectionPow(-5.0, 0.5, 10.0));
 
@@ -172,18 +180,21 @@ TEST(MathUtilTest, CopyDirectionPowWithMaxMagnitude) {
 
 TEST(MathUtilTest, CopyDirectionPowWithUnits) {
   EXPECT_UNITS_EQ(
-      0_mps, wpi::math::CopyDirectionPow<wpi::units::meters_per_second_t>(0_mps, 2.0));
+      0_mps,
+      wpi::math::CopyDirectionPow<wpi::units::meters_per_second_t>(0_mps, 2.0));
   EXPECT_UNITS_EQ(
-      1_mps, wpi::math::CopyDirectionPow<wpi::units::meters_per_second_t>(1_mps, 2.0));
-  EXPECT_UNITS_EQ(
-      -1_mps, wpi::math::CopyDirectionPow<wpi::units::meters_per_second_t>(-1_mps, 2.0));
+      1_mps,
+      wpi::math::CopyDirectionPow<wpi::units::meters_per_second_t>(1_mps, 2.0));
+  EXPECT_UNITS_EQ(-1_mps,
+                  wpi::math::CopyDirectionPow<wpi::units::meters_per_second_t>(
+                      -1_mps, 2.0));
 
-  EXPECT_UNITS_EQ(
-      wpi::units::meters_per_second_t{0.5 * 0.5 * 10},
-      wpi::math::CopyDirectionPow<wpi::units::meters_per_second_t>(5_mps, 2.0, 10_mps));
-  EXPECT_UNITS_EQ(
-      wpi::units::meters_per_second_t{-0.5 * 0.5 * 10},
-      wpi::math::CopyDirectionPow<wpi::units::meters_per_second_t>(-5_mps, 2.0, 10_mps));
+  EXPECT_UNITS_EQ(wpi::units::meters_per_second_t{0.5 * 0.5 * 10},
+                  wpi::math::CopyDirectionPow<wpi::units::meters_per_second_t>(
+                      5_mps, 2.0, 10_mps));
+  EXPECT_UNITS_EQ(wpi::units::meters_per_second_t{-0.5 * 0.5 * 10},
+                  wpi::math::CopyDirectionPow<wpi::units::meters_per_second_t>(
+                      -5_mps, 2.0, 10_mps));
 }
 
 TEST(MathUtilTest, CopyDirectionPow2d) {
@@ -221,57 +232,70 @@ TEST(MathUtilTest, CopyDirectionPow2d) {
 }
 
 TEST(MathUtilTest, CopyDirectionPow2dMaxDistance) {
-  EXPECT_EQ((Eigen::Vector2d{{5.0}, {0.0}}),
-            wpi::math::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 1.0, 10.0));
-  EXPECT_EQ((Eigen::Vector2d{{-5.0}, {0.0}}),
-            wpi::math::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 1.0, 10.0));
+  EXPECT_EQ(
+      (Eigen::Vector2d{{5.0}, {0.0}}),
+      wpi::math::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 1.0, 10.0));
+  EXPECT_EQ(
+      (Eigen::Vector2d{{-5.0}, {0.0}}),
+      wpi::math::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 1.0, 10.0));
 
-  EXPECT_EQ((Eigen::Vector2d{{2.5}, {0.0}}),
-            wpi::math::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 2.0, 10.0));
-  EXPECT_EQ((Eigen::Vector2d{{-2.5}, {0.0}}),
-            wpi::math::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 2.0, 10.0));
+  EXPECT_EQ(
+      (Eigen::Vector2d{{2.5}, {0.0}}),
+      wpi::math::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 2.0, 10.0));
+  EXPECT_EQ(
+      (Eigen::Vector2d{{-2.5}, {0.0}}),
+      wpi::math::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 2.0, 10.0));
 
-  EXPECT_EQ((Eigen::Vector2d{{std::sqrt(0.5) * 10.0}, {0.0}}),
-            wpi::math::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 0.5, 10.0));
-  EXPECT_EQ((Eigen::Vector2d{{-std::sqrt(0.5) * 10.0}, {0.0}}),
-            wpi::math::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 0.5, 10.0));
+  EXPECT_EQ(
+      (Eigen::Vector2d{{std::sqrt(0.5) * 10.0}, {0.0}}),
+      wpi::math::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 0.5, 10.0));
+  EXPECT_EQ(
+      (Eigen::Vector2d{{-std::sqrt(0.5) * 10.0}, {0.0}}),
+      wpi::math::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 0.5, 10.0));
 
-  EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
-            wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.0}, {0.0}}, 2.0, 5.0));
-  EXPECT_EQ((Eigen::Vector2d{{5.0}, {0.0}}),
-            wpi::math::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 2.0, 5.0));
-  EXPECT_EQ((Eigen::Vector2d{{-5.0}, {0.0}}),
-            wpi::math::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 2.0, 5.0));
+  EXPECT_EQ(
+      (Eigen::Vector2d{{0.0}, {0.0}}),
+      wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.0}, {0.0}}, 2.0, 5.0));
+  EXPECT_EQ(
+      (Eigen::Vector2d{{5.0}, {0.0}}),
+      wpi::math::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 2.0, 5.0));
+  EXPECT_EQ(
+      (Eigen::Vector2d{{-5.0}, {0.0}}),
+      wpi::math::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 2.0, 5.0));
 
-  EXPECT_EQ((Eigen::Vector2d{{0.0}, {std::pow(0.8, 0.3) * 100.0}}),
-            wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.0}, {80.0}}, 0.3, 100.0));
-  EXPECT_EQ((Eigen::Vector2d{{0.0}, {-std::pow(0.8, 0.3) * 100.0}}),
-            wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.0}, {-80.0}}, 0.3, 100.0));
+  EXPECT_EQ(
+      (Eigen::Vector2d{{0.0}, {std::pow(0.8, 0.3) * 100.0}}),
+      wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.0}, {80.0}}, 0.3, 100.0));
+  EXPECT_EQ(
+      (Eigen::Vector2d{{0.0}, {-std::pow(0.8, 0.3) * 100.0}}),
+      wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.0}, {-80.0}}, 0.3, 100.0));
 }
 
 TEST(MathUtilTest, CopyDirectionPow2dUnits) {
-  EXPECT_EQ(
-      (Eigen::Vector<wpi::units::meters_per_second_t, 2>{1_mps, 0_mps}),
-      wpi::math::CopyDirectionPow(
-          Eigen::Vector<wpi::units::meters_per_second_t, 2>{1_mps, 0_mps}, 2.0));
+  EXPECT_EQ((Eigen::Vector<wpi::units::meters_per_second_t, 2>{1_mps, 0_mps}),
+            wpi::math::CopyDirectionPow(
+                Eigen::Vector<wpi::units::meters_per_second_t, 2>{1_mps, 0_mps},
+                2.0));
   EXPECT_EQ(
       (Eigen::Vector<wpi::units::meters_per_second_t, 2>{-1_mps, 0_mps}),
       wpi::math::CopyDirectionPow(
-          Eigen::Vector<wpi::units::meters_per_second_t, 2>{-1_mps, 0_mps}, 2.0));
+          Eigen::Vector<wpi::units::meters_per_second_t, 2>{-1_mps, 0_mps},
+          2.0));
 
   EXPECT_EQ((Eigen::Vector<wpi::units::meters_per_second_t, 2>{0_mps, 0_mps}),
             wpi::math::CopyDirectionPow(
-                Eigen::Vector<wpi::units::meters_per_second_t, 2>{0_mps, 0_mps}, 2.0,
-                5_mps));
+                Eigen::Vector<wpi::units::meters_per_second_t, 2>{0_mps, 0_mps},
+                2.0, 5_mps));
 
   EXPECT_EQ((Eigen::Vector<wpi::units::meters_per_second_t, 2>{5_mps, 0_mps}),
             wpi::math::CopyDirectionPow(
-                Eigen::Vector<wpi::units::meters_per_second_t, 2>{5_mps, 0_mps}, 2.0,
-                5_mps));
-  EXPECT_EQ((Eigen::Vector<wpi::units::meters_per_second_t, 2>{-5_mps, 0_mps}),
-            wpi::math::CopyDirectionPow(
-                Eigen::Vector<wpi::units::meters_per_second_t, 2>{-5_mps, 0_mps},
+                Eigen::Vector<wpi::units::meters_per_second_t, 2>{5_mps, 0_mps},
                 2.0, 5_mps));
+  EXPECT_EQ(
+      (Eigen::Vector<wpi::units::meters_per_second_t, 2>{-5_mps, 0_mps}),
+      wpi::math::CopyDirectionPow(
+          Eigen::Vector<wpi::units::meters_per_second_t, 2>{-5_mps, 0_mps}, 2.0,
+          5_mps));
 }
 
 TEST(MathUtilTest, InputModulus) {
@@ -279,25 +303,29 @@ TEST(MathUtilTest, InputModulus) {
   // result of an angle reference minus the measurement.
 
   // Test symmetric range
-  EXPECT_DOUBLE_EQ(-20.0, wpi::math::InputModulus(170.0 - (-170.0), -180.0, 180.0));
   EXPECT_DOUBLE_EQ(-20.0,
-                   wpi::math::InputModulus(170.0 + 360.0 - (-170.0), -180.0, 180.0));
-  EXPECT_DOUBLE_EQ(-20.0,
-                   wpi::math::InputModulus(170.0 - (-170.0 + 360.0), -180.0, 180.0));
-  EXPECT_DOUBLE_EQ(20.0, wpi::math::InputModulus(-170.0 - 170.0, -180.0, 180.0));
+                   wpi::math::InputModulus(170.0 - (-170.0), -180.0, 180.0));
+  EXPECT_DOUBLE_EQ(
+      -20.0, wpi::math::InputModulus(170.0 + 360.0 - (-170.0), -180.0, 180.0));
+  EXPECT_DOUBLE_EQ(
+      -20.0, wpi::math::InputModulus(170.0 - (-170.0 + 360.0), -180.0, 180.0));
   EXPECT_DOUBLE_EQ(20.0,
-                   wpi::math::InputModulus(-170.0 + 360.0 - 170.0, -180.0, 180.0));
-  EXPECT_DOUBLE_EQ(20.0,
-                   wpi::math::InputModulus(-170.0 - (170.0 + 360.0), -180.0, 180.0));
+                   wpi::math::InputModulus(-170.0 - 170.0, -180.0, 180.0));
+  EXPECT_DOUBLE_EQ(
+      20.0, wpi::math::InputModulus(-170.0 + 360.0 - 170.0, -180.0, 180.0));
+  EXPECT_DOUBLE_EQ(
+      20.0, wpi::math::InputModulus(-170.0 - (170.0 + 360.0), -180.0, 180.0));
 
   // Test range starting at zero
   EXPECT_DOUBLE_EQ(340.0, wpi::math::InputModulus(170.0 - 190.0, 0.0, 360.0));
-  EXPECT_DOUBLE_EQ(340.0, wpi::math::InputModulus(170.0 + 360.0 - 190.0, 0.0, 360.0));
   EXPECT_DOUBLE_EQ(340.0,
-                   wpi::math::InputModulus(170.0 - (190.0 + 360.0), 0.0, 360.0));
+                   wpi::math::InputModulus(170.0 + 360.0 - 190.0, 0.0, 360.0));
+  EXPECT_DOUBLE_EQ(
+      340.0, wpi::math::InputModulus(170.0 - (190.0 + 360.0), 0.0, 360.0));
 
   // Test asymmetric range that doesn't start at zero
-  EXPECT_DOUBLE_EQ(-20.0, wpi::math::InputModulus(170.0 - (-170.0), -170.0, 190.0));
+  EXPECT_DOUBLE_EQ(-20.0,
+                   wpi::math::InputModulus(170.0 - (-170.0), -170.0, 190.0));
 
   // Test range with both positive endpoints
   EXPECT_DOUBLE_EQ(2.0, wpi::math::InputModulus(0.0, 1.0, 3.0));
@@ -307,31 +335,36 @@ TEST(MathUtilTest, InputModulus) {
   EXPECT_DOUBLE_EQ(2.0, wpi::math::InputModulus(4.0, 1.0, 3.0));
 
   // Test all supported types
-  EXPECT_DOUBLE_EQ(-20.0,
-                   wpi::math::InputModulus<double>(170.0 - (-170.0), -170.0, 190.0));
+  EXPECT_DOUBLE_EQ(
+      -20.0, wpi::math::InputModulus<double>(170.0 - (-170.0), -170.0, 190.0));
   EXPECT_EQ(-20, wpi::math::InputModulus<int>(170 - (-170), -170, 190));
-  EXPECT_EQ(-20_deg, wpi::math::InputModulus<wpi::units::degree_t>(170_deg - (-170_deg),
-                                                        -170_deg, 190_deg));
+  EXPECT_EQ(-20_deg, wpi::math::InputModulus<wpi::units::degree_t>(
+                         170_deg - (-170_deg), -170_deg, 190_deg));
 }
 
 TEST(MathUtilTest, AngleModulus) {
+  EXPECT_UNITS_NEAR(wpi::math::AngleModulus(
+                        wpi::units::radian_t{-2000 * std::numbers::pi / 180}),
+                    wpi::units::radian_t{160 * std::numbers::pi / 180}, 1e-10);
+  EXPECT_UNITS_NEAR(wpi::math::AngleModulus(
+                        wpi::units::radian_t{358 * std::numbers::pi / 180}),
+                    wpi::units::radian_t{-2 * std::numbers::pi / 180}, 1e-10);
   EXPECT_UNITS_NEAR(
-      wpi::math::AngleModulus(wpi::units::radian_t{-2000 * std::numbers::pi / 180}),
-      wpi::units::radian_t{160 * std::numbers::pi / 180}, 1e-10);
-  EXPECT_UNITS_NEAR(
-      wpi::math::AngleModulus(wpi::units::radian_t{358 * std::numbers::pi / 180}),
-      wpi::units::radian_t{-2 * std::numbers::pi / 180}, 1e-10);
-  EXPECT_UNITS_NEAR(wpi::math::AngleModulus(wpi::units::radian_t{2.0 * std::numbers::pi}),
-                    0_rad, 1e-10);
+      wpi::math::AngleModulus(wpi::units::radian_t{2.0 * std::numbers::pi}),
+      0_rad, 1e-10);
 
-  EXPECT_UNITS_EQ(wpi::math::AngleModulus(wpi::units::radian_t{5 * std::numbers::pi}),
-                  wpi::units::radian_t{std::numbers::pi});
-  EXPECT_UNITS_EQ(wpi::math::AngleModulus(wpi::units::radian_t{-5 * std::numbers::pi}),
-                  wpi::units::radian_t{std::numbers::pi});
-  EXPECT_UNITS_EQ(wpi::math::AngleModulus(wpi::units::radian_t{std::numbers::pi / 2}),
-                  wpi::units::radian_t{std::numbers::pi / 2});
-  EXPECT_UNITS_EQ(wpi::math::AngleModulus(wpi::units::radian_t{-std::numbers::pi / 2}),
-                  wpi::units::radian_t{-std::numbers::pi / 2});
+  EXPECT_UNITS_EQ(
+      wpi::math::AngleModulus(wpi::units::radian_t{5 * std::numbers::pi}),
+      wpi::units::radian_t{std::numbers::pi});
+  EXPECT_UNITS_EQ(
+      wpi::math::AngleModulus(wpi::units::radian_t{-5 * std::numbers::pi}),
+      wpi::units::radian_t{std::numbers::pi});
+  EXPECT_UNITS_EQ(
+      wpi::math::AngleModulus(wpi::units::radian_t{std::numbers::pi / 2}),
+      wpi::units::radian_t{std::numbers::pi / 2});
+  EXPECT_UNITS_EQ(
+      wpi::math::AngleModulus(wpi::units::radian_t{-std::numbers::pi / 2}),
+      wpi::units::radian_t{-std::numbers::pi / 2});
 }
 
 TEST(MathUtilTest, IsNear) {

wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp

@@ -8,7 +8,8 @@
 #include "wpi/math/util/StateSpaceUtil.hpp"
 
 TEST(StateSpaceUtilTest, CostParameterPack) {
-  constexpr wpi::math::Matrixd<3, 3> mat = wpi::math::MakeCostMatrix(1.0, 2.0, 3.0);
+  constexpr wpi::math::Matrixd<3, 3> mat =
+      wpi::math::MakeCostMatrix(1.0, 2.0, 3.0);
   EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
   EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
   EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -21,7 +22,8 @@ TEST(StateSpaceUtilTest, CostParameterPack) {
 }
 
 TEST(StateSpaceUtilTest, CostArray) {
-  constexpr wpi::math::Matrixd<3, 3> mat = wpi::math::MakeCostMatrix<3>({1.0, 2.0, 3.0});
+  constexpr wpi::math::Matrixd<3, 3> mat =
+      wpi::math::MakeCostMatrix<3>({1.0, 2.0, 3.0});
   EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
   EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
   EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -47,7 +49,8 @@ TEST(StateSpaceUtilTest, CostDynamic) {
 }
 
 TEST(StateSpaceUtilTest, CovParameterPack) {
-  constexpr wpi::math::Matrixd<3, 3> mat = wpi::math::MakeCovMatrix(1.0, 2.0, 3.0);
+  constexpr wpi::math::Matrixd<3, 3> mat =
+      wpi::math::MakeCovMatrix(1.0, 2.0, 3.0);
   EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
   EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
   EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -60,7 +63,8 @@ TEST(StateSpaceUtilTest, CovParameterPack) {
 }
 
 TEST(StateSpaceUtilTest, CovArray) {
-  constexpr wpi::math::Matrixd<3, 3> mat = wpi::math::MakeCovMatrix<3>({1.0, 2.0, 3.0});
+  constexpr wpi::math::Matrixd<3, 3> mat =
+      wpi::math::MakeCovMatrix<3>({1.0, 2.0, 3.0});
   EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
   EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
   EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -105,23 +109,23 @@ TEST(StateSpaceUtilTest, IsStabilizable) {
 
   // First eigenvalue is uncontrollable and unstable.
   // Second eigenvalue is controllable and stable.
-  EXPECT_FALSE(
-      (wpi::math::IsStabilizable<2, 1>(wpi::math::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, B)));
+  EXPECT_FALSE((wpi::math::IsStabilizable<2, 1>(
+      wpi::math::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, B)));
 
   // First eigenvalue is uncontrollable and marginally stable.
   // Second eigenvalue is controllable and stable.
-  EXPECT_FALSE(
-      (wpi::math::IsStabilizable<2, 1>(wpi::math::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, B)));
+  EXPECT_FALSE((wpi::math::IsStabilizable<2, 1>(
+      wpi::math::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, B)));
 
   // First eigenvalue is uncontrollable and stable.
   // Second eigenvalue is controllable and stable.
-  EXPECT_TRUE(
-      (wpi::math::IsStabilizable<2, 1>(wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, B)));
+  EXPECT_TRUE((wpi::math::IsStabilizable<2, 1>(
+      wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, B)));
 
   // First eigenvalue is uncontrollable and stable.
   // Second eigenvalue is controllable and unstable.
-  EXPECT_TRUE(
-      (wpi::math::IsStabilizable<2, 1>(wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, B)));
+  EXPECT_TRUE((wpi::math::IsStabilizable<2, 1>(
+      wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, B)));
 }
 
 TEST(StateSpaceUtilTest, IsDetectable) {
@@ -129,21 +133,21 @@ TEST(StateSpaceUtilTest, IsDetectable) {
 
   // First eigenvalue is unobservable and unstable.
   // Second eigenvalue is observable and stable.
-  EXPECT_FALSE(
-      (wpi::math::IsDetectable<2, 1>(wpi::math::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, C)));
+  EXPECT_FALSE((wpi::math::IsDetectable<2, 1>(
+      wpi::math::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, C)));
 
   // First eigenvalue is unobservable and marginally stable.
   // Second eigenvalue is observable and stable.
-  EXPECT_FALSE(
-      (wpi::math::IsDetectable<2, 1>(wpi::math::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, C)));
+  EXPECT_FALSE((wpi::math::IsDetectable<2, 1>(
+      wpi::math::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, C)));
 
   // First eigenvalue is unobservable and stable.
   // Second eigenvalue is observable and stable.
-  EXPECT_TRUE(
-      (wpi::math::IsDetectable<2, 1>(wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, C)));
+  EXPECT_TRUE((wpi::math::IsDetectable<2, 1>(
+      wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, C)));
 
   // First eigenvalue is unobservable and stable.
   // Second eigenvalue is observable and unstable.
-  EXPECT_TRUE(
-      (wpi::math::IsDetectable<2, 1>(wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, C)));
+  EXPECT_TRUE((wpi::math::IsDetectable<2, 1>(
+      wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, C)));
 }

wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp

@@ -35,18 +35,20 @@ using Kg_unit = decltype(1_V);
  * @param dt The simulation time.
  * @return The final state as a 2-vector of angle and angular velocity.
  */
-wpi::math::Matrixd<2, 1> Simulate(Ks_unit Ks, Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
-                            wpi::units::radian_t currentAngle,
-                            wpi::units::radians_per_second_t currentVelocity,
-                            wpi::units::volt_t input, wpi::units::second_t dt) {
+wpi::math::Matrixd<2, 1> Simulate(
+    Ks_unit Ks, Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
+    wpi::units::radian_t currentAngle,
+    wpi::units::radians_per_second_t currentVelocity, wpi::units::volt_t input,
+    wpi::units::second_t dt) {
   wpi::math::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
   wpi::math::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};
 
   return wpi::math::RK4(
       [&](const wpi::math::Matrixd<2, 1>& x,
           const wpi::math::Matrixd<1, 1>& u) -> wpi::math::Matrixd<2, 1> {
-        wpi::math::Matrixd<2, 1> c{0.0, -Ks.value() / Ka.value() * wpi::util::sgn(x(1)) -
-                                      Kg.value() / Ka.value() * std::cos(x(0))};
+        wpi::math::Matrixd<2, 1> c{
+            0.0, -Ks.value() / Ka.value() * wpi::util::sgn(x(1)) -
+                     Kg.value() / Ka.value() * std::cos(x(0))};
         return A * x + B * u + c;
       },
       wpi::math::Matrixd<2, 1>{currentAngle.value(), currentVelocity.value()},

wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp

@@ -21,7 +21,7 @@ Vectord<2> StateDynamics(const Vectord<2>& x) {
 
 TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
   wpi::math::ControlAffinePlantInversionFeedforward<2, 1> feedforward{&Dynamics,
-                                                                20_ms};
+                                                                      20_ms};
 
   Vectord<2> r{2, 2};
   Vectord<2> nextR{3, 3};

wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp

@@ -38,7 +38,7 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
     Vectord<2> accels =
         plant.A() * xAccelLimiter + plant.B() * Vectord<2>{-12.0, 12.0};
     wpi::units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
-                                              trackwidth.value()};
+                                                   trackwidth.value()};
     EXPECT_GT(wpi::units::math::abs(alpha), maxAlpha);
   }
 
@@ -46,10 +46,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
   Vectord<2> u{12.0, 12.0};
   for (auto t = 0_s; t < 3_s; t += dt) {
     x = plant.CalculateX(x, u, dt);
-    auto [left, right] =
-        accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
-                               wpi::units::meters_per_second_t{xAccelLimiter(1)},
-                               wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
+    auto [left, right] = accelLimiter.Calculate(
+        wpi::units::meters_per_second_t{xAccelLimiter(0)},
+        wpi::units::meters_per_second_t{xAccelLimiter(1)},
+        wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
     xAccelLimiter =
         plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
 
@@ -57,7 +57,7 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
         plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
     wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
     wpi::units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
-                                              trackwidth.value()};
+                                                   trackwidth.value()};
     EXPECT_LE(wpi::units::math::abs(a), maxA);
     EXPECT_LE(wpi::units::math::abs(alpha), maxAlpha);
   }
@@ -66,10 +66,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
   u = Vectord<2>{-12.0, -12.0};
   for (auto t = 0_s; t < 3_s; t += dt) {
     x = plant.CalculateX(x, u, dt);
-    auto [left, right] =
-        accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
-                               wpi::units::meters_per_second_t{xAccelLimiter(1)},
-                               wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
+    auto [left, right] = accelLimiter.Calculate(
+        wpi::units::meters_per_second_t{xAccelLimiter(0)},
+        wpi::units::meters_per_second_t{xAccelLimiter(1)},
+        wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
     xAccelLimiter =
         plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
 
@@ -77,7 +77,7 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
         plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
     wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
     wpi::units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
-                                              trackwidth.value()};
+                                                   trackwidth.value()};
     EXPECT_LE(wpi::units::math::abs(a), maxA);
     EXPECT_LE(wpi::units::math::abs(alpha), maxAlpha);
   }
@@ -86,10 +86,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
   u = Vectord<2>{-12.0, 12.0};
   for (auto t = 0_s; t < 3_s; t += dt) {
     x = plant.CalculateX(x, u, dt);
-    auto [left, right] =
-        accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
-                               wpi::units::meters_per_second_t{xAccelLimiter(1)},
-                               wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
+    auto [left, right] = accelLimiter.Calculate(
+        wpi::units::meters_per_second_t{xAccelLimiter(0)},
+        wpi::units::meters_per_second_t{xAccelLimiter(1)},
+        wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
     xAccelLimiter =
         plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
 
@@ -97,7 +97,7 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
         plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
     wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
     wpi::units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
-                                              trackwidth.value()};
+                                                   trackwidth.value()};
     EXPECT_LE(wpi::units::math::abs(a), maxA);
     EXPECT_LE(wpi::units::math::abs(alpha), maxAlpha);
   }
@@ -125,10 +125,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
   Vectord<2> u{12.0, 12.0};
   for (auto t = 0_s; t < 3_s; t += dt) {
     x = plant.CalculateX(x, u, dt);
-    auto [left, right] =
-        accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
-                               wpi::units::meters_per_second_t{xAccelLimiter(1)},
-                               wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
+    auto [left, right] = accelLimiter.Calculate(
+        wpi::units::meters_per_second_t{xAccelLimiter(0)},
+        wpi::units::meters_per_second_t{xAccelLimiter(1)},
+        wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
     xAccelLimiter =
         plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
 
@@ -142,10 +142,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
   u = Vectord<2>{-12.0, -12.0};
   for (auto t = 0_s; t < 3_s; t += dt) {
     x = plant.CalculateX(x, u, dt);
-    auto [left, right] =
-        accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
-                               wpi::units::meters_per_second_t{xAccelLimiter(1)},
-                               wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
+    auto [left, right] = accelLimiter.Calculate(
+        wpi::units::meters_per_second_t{xAccelLimiter(0)},
+        wpi::units::meters_per_second_t{xAccelLimiter(1)},
+        wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
     xAccelLimiter =
         plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
 
@@ -159,10 +159,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
   u = Vectord<2>{-12.0, 12.0};
   for (auto t = 0_s; t < 3_s; t += dt) {
     x = plant.CalculateX(x, u, dt);
-    auto [left, right] =
-        accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
-                               wpi::units::meters_per_second_t{xAccelLimiter(1)},
-                               wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
+    auto [left, right] = accelLimiter.Calculate(
+        wpi::units::meters_per_second_t{xAccelLimiter(0)},
+        wpi::units::meters_per_second_t{xAccelLimiter(1)},
+        wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
     xAccelLimiter =
         plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
 
@@ -203,10 +203,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
   Vectord<2> u{12.0, 12.0};
   for (auto t = 0_s; t < 3_s; t += dt) {
     x = plant.CalculateX(x, u, dt);
-    auto [left, right] =
-        accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
-                               wpi::units::meters_per_second_t{xAccelLimiter(1)},
-                               wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
+    auto [left, right] = accelLimiter.Calculate(
+        wpi::units::meters_per_second_t{xAccelLimiter(0)},
+        wpi::units::meters_per_second_t{xAccelLimiter(1)},
+        wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
     xAccelLimiter =
         plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
 
@@ -221,10 +221,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
   u = Vectord<2>{-12.0, -12.0};
   for (auto t = 0_s; t < 3_s; t += dt) {
     x = plant.CalculateX(x, u, dt);
-    auto [left, right] =
-        accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
-                               wpi::units::meters_per_second_t{xAccelLimiter(1)},
-                               wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
+    auto [left, right] = accelLimiter.Calculate(
+        wpi::units::meters_per_second_t{xAccelLimiter(0)},
+        wpi::units::meters_per_second_t{xAccelLimiter(1)},
+        wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
     xAccelLimiter =
         plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
 

wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp

@@ -60,7 +60,7 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithoutTrackwidth) {
       kVLinear, kALinear, kVAngular, kAAngular};
   wpi::math::LinearSystem<2, 2, 2> plant =
       wpi::math::LinearSystemId::IdentifyDrivetrainSystem(kVLinear, kALinear,
-                                                    kVAngular, kAAngular);
+                                                          kVAngular, kAAngular);
   for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
        currentLeftVelocity += 2_mps) {
     for (auto currentRightVelocity = -4_mps; currentRightVelocity <= 4_mps;

wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp

@@ -18,7 +18,7 @@
 
 static constexpr wpi::units::meter_t kTolerance{1 / 12.0};
 static constexpr wpi::units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
-                                                   180.0};
+                                                        180.0};
 
 /**
  * States of the drivetrain system.
@@ -49,7 +49,8 @@ static auto plant = wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
     kLinearV, kLinearA, kAngularV, kAngularA);
 static constexpr auto kTrackwidth = 0.9_m;
 
-wpi::math::Vectord<5> Dynamics(const wpi::math::Vectord<5>& x, const wpi::math::Vectord<2>& u) {
+wpi::math::Vectord<5> Dynamics(const wpi::math::Vectord<5>& x,
+                               const wpi::math::Vectord<2>& u) {
   double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
 
   wpi::math::Vectord<5> xdot;
@@ -81,22 +82,22 @@ TEST(LTVDifferentialDriveControllerTest, ReachesReference) {
   auto totalTime = trajectory.TotalTime();
   for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
     auto state = trajectory.Sample(kDt * i);
-    robotPose =
-        wpi::math::Pose2d{wpi::units::meter_t{x(State::kX)}, wpi::units::meter_t{x(State::kY)},
-                    wpi::units::radian_t{x(State::kHeading)}};
+    robotPose = wpi::math::Pose2d{wpi::units::meter_t{x(State::kX)},
+                                  wpi::units::meter_t{x(State::kY)},
+                                  wpi::units::radian_t{x(State::kHeading)}};
     auto [leftVoltage, rightVoltage] = controller.Calculate(
         robotPose, wpi::units::meters_per_second_t{x(State::kLeftVelocity)},
         wpi::units::meters_per_second_t{x(State::kRightVelocity)}, state);
 
-    x = wpi::math::RKDP(&Dynamics, x,
-                  wpi::math::Vectord<2>{leftVoltage.value(), rightVoltage.value()},
-                  kDt);
+    x = wpi::math::RKDP(
+        &Dynamics, x,
+        wpi::math::Vectord<2>{leftVoltage.value(), rightVoltage.value()}, kDt);
   }
 
   auto& endPose = trajectory.States().back().pose;
   EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
   EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
   EXPECT_NEAR_UNITS(wpi::math::AngleModulus(endPose.Rotation().Radians() -
-                                      robotPose.Rotation().Radians()),
+                                            robotPose.Rotation().Radians()),
                     0_rad, kAngularTolerance);
 }

wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp

@@ -14,12 +14,13 @@
 
 static constexpr wpi::units::meter_t kTolerance{1 / 12.0};
 static constexpr wpi::units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
-                                                   180.0};
+                                                        180.0};
 
 TEST(LTVUnicycleControllerTest, ReachesReference) {
   constexpr wpi::units::second_t kDt = 20_ms;
 
-  wpi::math::LTVUnicycleController controller{{0.0625, 0.125, 2.5}, {4.0, 4.0}, kDt};
+  wpi::math::LTVUnicycleController controller{
+      {0.0625, 0.125, 2.5}, {4.0, 4.0}, kDt};
   wpi::math::Pose2d robotPose{2.7_m, 23_m, 0_deg};
 
   auto waypoints = std::vector{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
@@ -33,13 +34,14 @@ TEST(LTVUnicycleControllerTest, ReachesReference) {
     auto [vx, vy, omega] = controller.Calculate(robotPose, state);
     static_cast<void>(vy);
 
-    robotPose = robotPose + wpi::math::Twist2d{vx * kDt, 0_m, omega * kDt}.Exp();
+    robotPose =
+        robotPose + wpi::math::Twist2d{vx * kDt, 0_m, omega * kDt}.Exp();
   }
 
   auto& endPose = trajectory.States().back().pose;
   EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
   EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
   EXPECT_NEAR_UNITS(wpi::math::AngleModulus(endPose.Rotation().Radians() -
-                                      robotPose.Rotation().Radians()),
+                                            robotPose.Rotation().Radians()),
                     0_rad, kAngularTolerance);
 }

wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp

@@ -50,8 +50,8 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) {
     // Gear ratio
     constexpr double G = 100.0;
 
-    return wpi::math::LinearSystemId::SingleJointedArmSystem(motors,
-                                                       1.0 / 3.0 * m * r * r, G)
+    return wpi::math::LinearSystemId::SingleJointedArmSystem(
+               motors, 1.0 / 3.0 * m * r * r, G)
         .Slice(0);
   }();
 

wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp

@@ -26,8 +26,9 @@ TEST(ProfiledPIDInputOutputTest, ContinuousInput1) {
   EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
 
   // Error must be less than half the input range at all times
-  EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
-            180_deg);
+  EXPECT_LT(
+      wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
+      180_deg);
 }
 
 TEST(ProfiledPIDInputOutputTest, ContinuousInput2) {
@@ -46,8 +47,9 @@ TEST(ProfiledPIDInputOutputTest, ContinuousInput2) {
   EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
 
   // Error must be less than half the input range at all times
-  EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
-            wpi::units::radian_t{std::numbers::pi});
+  EXPECT_LT(
+      wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
+      wpi::units::radian_t{std::numbers::pi});
 }
 
 TEST(ProfiledPIDInputOutputTest, ContinuousInput3) {
@@ -66,8 +68,9 @@ TEST(ProfiledPIDInputOutputTest, ContinuousInput3) {
   EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
 
   // Error must be less than half the input range at all times
-  EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
-            wpi::units::radian_t{std::numbers::pi});
+  EXPECT_LT(
+      wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
+      wpi::units::radian_t{std::numbers::pi});
 }
 
 TEST(ProfiledPIDInputOutputTest, ContinuousInput4) {
@@ -75,8 +78,8 @@ TEST(ProfiledPIDInputOutputTest, ContinuousInput4) {
       0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
 
   controller.SetP(1);
-  controller.EnableContinuousInput(0_rad,
-                                   wpi::units::radian_t{2.0 * std::numbers::pi});
+  controller.EnableContinuousInput(
+      0_rad, wpi::units::radian_t{2.0 * std::numbers::pi});
 
   static constexpr wpi::units::radian_t kSetpoint{2.78};
   static constexpr wpi::units::radian_t kMeasurement{3.12};
@@ -86,8 +89,9 @@ TEST(ProfiledPIDInputOutputTest, ContinuousInput4) {
   EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
 
   // Error must be less than half the input range at all times
-  EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
-            wpi::units::radian_t{std::numbers::pi});
+  EXPECT_LT(
+      wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
+      wpi::units::radian_t{std::numbers::pi});
 }
 
 TEST(ProfiledPIDInputOutputTest, ProportionalGainOutput) {

wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp

@@ -47,7 +47,8 @@ TEST(SimpleMotorFeedforwardTest, NegativeGains) {
   constexpr auto Kv = -3_V / 1_mps;
   constexpr auto Ka = -0.6_V / 1_mps_sq;
   constexpr wpi::units::second_t dt = 0_ms;
-  wpi::math::SimpleMotorFeedforward<wpi::units::meter> simpleMotor{Ks, Kv, Ka, dt};
+  wpi::math::SimpleMotorFeedforward<wpi::units::meter> simpleMotor{Ks, Kv, Ka,
+                                                                   dt};
   EXPECT_EQ(simpleMotor.GetKv().value(), 0);
   EXPECT_EQ(simpleMotor.GetKa().value(), 0);
   EXPECT_EQ(simpleMotor.GetDt().value(), 0.02);

wpimath/src/test/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProtoTest.cpp

@@ -11,7 +11,8 @@ using namespace wpi::math;
 
 namespace {
 
-using ProtoType = wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVoltages>;
+using ProtoType =
+    wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVoltages>;
 
 const DifferentialDriveWheelVoltages kExpectedData =
     DifferentialDriveWheelVoltages{0.174_V, 0.191_V};

wpimath/src/test/native/cpp/controller/proto/SimpleMotorFeedforwardProtoTest.cpp

@@ -17,7 +17,8 @@ struct SimpleMotorFeedforwardProtoTestData {
   using Type = SimpleMotorFeedforward<T>;
 
   inline static const Type kTestData = {
-      wpi::units::volt_t{0.4}, wpi::units::volt_t{4.0} / (wpi::units::unit_t<T>{1} / 1_s),
+      wpi::units::volt_t{0.4},
+      wpi::units::volt_t{4.0} / (wpi::units::unit_t<T>{1} / 1_s),
       wpi::units::volt_t{0.7} / (wpi::units::unit_t<T>{1} / 1_s / 1_s), 25_ms};
 
   static void CheckEq(const Type& testData, const Type& data) {

wpimath/src/test/native/cpp/controller/struct/SimpleMotorFeedforwardStructTest.cpp

@@ -17,7 +17,8 @@ struct SimpleMotorFeedforwardStructTestData {
   using Type = SimpleMotorFeedforward<T>;
 
   inline static const Type kTestData = {
-      wpi::units::volt_t{0.4}, wpi::units::volt_t{4.0} / (wpi::units::unit_t<T>{1} / 1_s),
+      wpi::units::volt_t{0.4},
+      wpi::units::volt_t{4.0} / (wpi::units::unit_t<T>{1} / 1_s),
       wpi::units::volt_t{0.7} / (wpi::units::unit_t<T>{1} / 1_s / 1_s), 25_ms};
 
   static void CheckEq(const Type& testData, const Type& data) {

wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp

@@ -18,8 +18,9 @@ TEST(AngleStatisticsTest, Mean) {
   Eigen::Vector3d weights;
   weights.fill(1.0 / sigmas.cols());
 
-  EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
-                  .isApprox(wpi::math::AngleMean<3, 3>(sigmas, weights, 1), 1e-3));
+  EXPECT_TRUE(
+      Eigen::Vector3d(0.7333333, 0.01163323, 1)
+          .isApprox(wpi::math::AngleMean<3, 3>(sigmas, weights, 1), 1e-3));
 }
 
 TEST(AngleStatisticsTest, Mean_DynamicSize) {
@@ -31,10 +32,11 @@ TEST(AngleStatisticsTest, Mean_DynamicSize) {
   Eigen::VectorXd weights{3};
   weights.fill(1.0 / sigmas.cols());
 
-  EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
-                  .isApprox(wpi::math::AngleMean<Eigen::Dynamic, Eigen::Dynamic>(
-                                sigmas, weights, 1),
-                            1e-3));
+  EXPECT_TRUE(
+      Eigen::Vector3d(0.7333333, 0.01163323, 1)
+          .isApprox(wpi::math::AngleMean<Eigen::Dynamic, Eigen::Dynamic>(
+                        sigmas, weights, 1),
+                    1e-3));
 }
 
 TEST(AngleStatisticsTest, Residual) {
@@ -57,7 +59,8 @@ TEST(AngleStatisticsTest, Add) {
   Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
   Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
 
-  EXPECT_TRUE(wpi::math::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
+  EXPECT_TRUE(
+      wpi::math::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
 }
 
 TEST(AngleStatisticsTest, Add_DynamicSize) {

wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp

@@ -45,7 +45,8 @@ void testFollowTrajectory(
   wpi::units::second_t t = 0_s;
 
   std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
-  std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
+  std::vector<
+      std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
       visionLog;
 
   double maxError = -std::numeric_limits<double>::max();
@@ -66,9 +67,10 @@ void testFollowTrajectory(
         visionPoses.back().first + kVisionUpdateRate < t) {
       auto visionPose =
           visionMeasurementGenerator(groundTruthState) +
-          wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
-                                              distribution(generator) * 0.1_m},
-                           wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
+          wpi::math::Transform2d{
+              wpi::math::Translation2d{distribution(generator) * 0.1_m,
+                                       distribution(generator) * 0.1_m},
+              wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
       visionPoses.push_back({t, visionPose});
     }
 
@@ -92,9 +94,10 @@ void testFollowTrajectory(
 
     auto xhat = estimator.UpdateWithTime(
         t,
-        wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
-                        wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
-                        trajectory.InitialPose().Rotation()},
+        wpi::math::Rotation3d{
+            groundTruthState.pose.Rotation() +
+            wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
+            trajectory.InitialPose().Rotation()},
         leftDistance, rightDistance);
 
     if (debug) {
@@ -119,7 +122,8 @@ void testFollowTrajectory(
   }
 
   if (debug) {
-    wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
+    wpi::util::print(
+        "apply_time, measured_time, vision_x, vision_y, vision_theta\n");
 
     wpi::units::second_t apply_time;
     wpi::units::second_t measure_time;
@@ -127,9 +131,9 @@ void testFollowTrajectory(
     for (auto record : visionLog) {
       std::tie(apply_time, measure_time, vision_pose) = record;
       wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
-                 measure_time.value(), vision_pose.X().value(),
-                 vision_pose.Y().value(),
-                 vision_pose.Rotation().Radians().value());
+                       measure_time.value(), vision_pose.X().value(),
+                       vision_pose.Y().value(),
+                       vision_pose.Rotation().Radians().value());
     }
   }
 
@@ -155,49 +159,55 @@ void testFollowTrajectory(
 TEST(DifferentialDrivePoseEstimator3dTest, Accuracy) {
   wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
 
-  wpi::math::DifferentialDrivePoseEstimator3d estimator{kinematics,
-                                                  wpi::math::Rotation3d{},
-                                                  0_m,
-                                                  0_m,
-                                                  wpi::math::Pose3d{},
-                                                  {0.02, 0.02, 0.02, 0.01},
-                                                  {0.1, 0.1, 0.1, 0.1}};
+  wpi::math::DifferentialDrivePoseEstimator3d estimator{
+      kinematics,
+      wpi::math::Rotation3d{},
+      0_m,
+      0_m,
+      wpi::math::Pose3d{},
+      {0.02, 0.02, 0.02, 0.01},
+      {0.1, 0.1, 0.1, 0.1}};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
 
   testFollowTrajectory(
       kinematics, estimator, trajectory,
       [&](wpi::math::Trajectory::State& state) {
         return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
-                                  state.velocity * state.curvature};
+                                        state.velocity * state.curvature};
       },
       [&](wpi::math::Trajectory::State& state) { return state.pose; },
-      trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
-      100_ms, 250_ms, true, false);
+      trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
+      20_ms, 100_ms, 250_ms, true, false);
 }
 
 TEST(DifferentialDrivePoseEstimator3dTest, BadInitialPose) {
   wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
 
-  wpi::math::DifferentialDrivePoseEstimator3d estimator{kinematics,
-                                                  wpi::math::Rotation3d{},
-                                                  0_m,
-                                                  0_m,
-                                                  wpi::math::Pose3d{},
-                                                  {0.02, 0.02, 0.02, 0.01},
-                                                  {0.1, 0.1, 0.1, 0.1}};
+  wpi::math::DifferentialDrivePoseEstimator3d estimator{
+      kinematics,
+      wpi::math::Rotation3d{},
+      0_m,
+      0_m,
+      wpi::math::Pose3d{},
+      {0.02, 0.02, 0.02, 0.01},
+      {0.1, 0.1, 0.1, 0.1}};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
 
   for (wpi::units::degree_t offset_direction_degs = 0_deg;
        offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
@@ -206,21 +216,21 @@ TEST(DifferentialDrivePoseEstimator3dTest, BadInitialPose) {
       auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
       auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
 
-      auto initial_pose =
-          trajectory.InitialPose() +
-          wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
-                                              pose_offset.Sin() * 1_m},
-                           heading_offset};
+      auto initial_pose = trajectory.InitialPose() +
+                          wpi::math::Transform2d{
+                              wpi::math::Translation2d{pose_offset.Cos() * 1_m,
+                                                       pose_offset.Sin() * 1_m},
+                              heading_offset};
 
       testFollowTrajectory(
           kinematics, estimator, trajectory,
           [&](wpi::math::Trajectory::State& state) {
             return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
-                                      state.velocity * state.curvature};
+                                            state.velocity * state.curvature};
           },
           [&](wpi::math::Trajectory::State& state) { return state.pose; },
-          initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
-          250_ms, false, false);
+          initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
+          100_ms, 250_ms, false, false);
     }
   }
 }
@@ -238,7 +248,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
       wpi::math::Rotation3d{},
       0_m,
       0_m,
-      wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
+      wpi::math::Pose3d{1_m, 2_m, 0_m,
+                        wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
       {0.02, 0.02, 0.02, 0.01},
       {0.1, 0.1, 0.1, 0.1}};
 
@@ -246,11 +257,16 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
 
   for (int i = 0; i < 1000; i++) {
     estimator.AddVisionMeasurement(
-        wpi::math::Pose3d{0_m, 0_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
+        wpi::math::Pose3d{0_m, 0_m, 0_m,
+                          wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}},
+        0_s);
     estimator.AddVisionMeasurement(
-        wpi::math::Pose3d{3_m, 1_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
+        wpi::math::Pose3d{3_m, 1_m, 0_m,
+                          wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}},
+        0_s);
     estimator.AddVisionMeasurement(
-        wpi::math::Pose3d{2_m, 4_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
+        wpi::math::Pose3d{2_m, 4_m, 0_m,
+                          wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
         0_s);
   }
 
@@ -289,8 +305,13 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
   wpi::math::DifferentialDriveKinematics kinematics{1_m};
 
   wpi::math::DifferentialDrivePoseEstimator3d estimator{
-      kinematics,           wpi::math::Rotation3d{},       0_m, 0_m, wpi::math::Pose3d{},
-      {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
+      kinematics,
+      wpi::math::Rotation3d{},
+      0_m,
+      0_m,
+      wpi::math::Pose3d{},
+      {0.1, 0.1, 0.1, 0.1},
+      {0.45, 0.45, 0.45, 0.45}};
 
   // Add enough measurements to fill up the buffer
   for (auto time = 0_s; time < 4_s; time += 20_ms) {
@@ -301,8 +322,9 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
 
   // Apply a vision measurement from 3 seconds ago
   estimator.AddVisionMeasurement(
-      wpi::math::Pose3d{10_m, 10_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
-      {0.1, 0.1, 0.1, 0.1});
+      wpi::math::Pose3d{10_m, 10_m, 0_m,
+                        wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}},
+      1_s, {0.1, 0.1, 0.1, 0.1});
 
   EXPECT_NEAR(odometryPose.X().value(),
               estimator.GetEstimatedPosition().X().value(), 1e-6);
@@ -320,9 +342,13 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
 
 TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
   wpi::math::DifferentialDriveKinematics kinematics{1_m};
-  wpi::math::DifferentialDrivePoseEstimator3d estimator{
-      kinematics,           wpi::math::Rotation3d{},   0_m, 0_m, wpi::math::Pose3d{},
-      {1.0, 1.0, 1.0, 1.0}, {1.0, 1.0, 1.0, 1.0}};
+  wpi::math::DifferentialDrivePoseEstimator3d estimator{kinematics,
+                                                        wpi::math::Rotation3d{},
+                                                        0_m,
+                                                        0_m,
+                                                        wpi::math::Pose3d{},
+                                                        {1.0, 1.0, 1.0, 1.0},
+                                                        {1.0, 1.0, 1.0, 1.0}};
 
   // Returns empty when null
   EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
@@ -331,35 +357,45 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
   // Add a tiny tolerance for the upper bound because of floating point rounding
   // error
   for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
-    estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation3d{},
-                             wpi::units::meter_t{time}, wpi::units::meter_t{time});
+    estimator.UpdateWithTime(wpi::units::second_t{time},
+                             wpi::math::Rotation3d{}, wpi::units::meter_t{time},
+                             wpi::units::meter_t{time});
   }
 
   // Sample at an added time
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.02_s));
   // Sample between updates (test interpolation)
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.01_s));
   // Sampling before the oldest value returns the oldest value
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
-            estimator.SampleAt(0.5_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+      estimator.SampleAt(0.5_s));
   // Sampling after the newest value returns the newest value
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
-            estimator.SampleAt(2.5_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+      estimator.SampleAt(2.5_s));
 
   // Add a vision measurement after the odometry measurements (while keeping all
   // of the old odometry measurements)
   estimator.AddVisionMeasurement(
-      wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
+      wpi::math::Pose3d{2_m, 0_m, 0_m,
+                        wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
+      2.2_s);
 
   // Make sure nothing changed (except the newest value)
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.02_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.01_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
-            estimator.SampleAt(0.5_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+      estimator.SampleAt(0.5_s));
 
   // Add a vision measurement before the odometry measurements that's still in
   // the buffer
@@ -367,13 +403,17 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
       wpi::math::Pose3d{1_m, 0.2_m, 0_m, wpi::math::Rotation3d{}}, 0.9_s);
 
   // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.02_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.01_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(0.5_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(2.5_s));
 }
 
@@ -384,7 +424,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
       wpi::math::Rotation3d{},
       0_m,
       0_m,
-      wpi::math::Pose3d{-1_m, -1_m, -1_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
+      wpi::math::Pose3d{-1_m, -1_m, -1_m,
+                        wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
       {1.0, 1.0, 1.0, 1.0},
       {1.0, 1.0, 1.0, 1.0}};
 
@@ -397,8 +438,9 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
   EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Rotation().Z().value());
 
   // Test reset position
-  estimator.ResetPosition(wpi::math::Rotation3d{}, 1_m, 1_m,
-                          wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
+  estimator.ResetPosition(
+      wpi::math::Rotation3d{}, 1_m, 1_m,
+      wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
 
   EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());

wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp

@@ -46,7 +46,8 @@ void testFollowTrajectory(
   wpi::units::second_t t = 0_s;
 
   std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
-  std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
+  std::vector<
+      std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
       visionLog;
 
   double maxError = -std::numeric_limits<double>::max();
@@ -67,9 +68,10 @@ void testFollowTrajectory(
         visionPoses.back().first + kVisionUpdateRate < t) {
       auto visionPose =
           visionMeasurementGenerator(groundTruthState) +
-          wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
-                                              distribution(generator) * 0.1_m},
-                           wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
+          wpi::math::Transform2d{
+              wpi::math::Translation2d{distribution(generator) * 0.1_m,
+                                       distribution(generator) * 0.1_m},
+              wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
       visionPoses.push_back({t, visionPose});
     }
 
@@ -119,7 +121,8 @@ void testFollowTrajectory(
   }
 
   if (debug) {
-    wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
+    wpi::util::print(
+        "apply_time, measured_time, vision_x, vision_y, vision_theta\n");
 
     wpi::units::second_t apply_time;
     wpi::units::second_t measure_time;
@@ -127,9 +130,9 @@ void testFollowTrajectory(
     for (auto record : visionLog) {
       std::tie(apply_time, measure_time, vision_pose) = record;
       wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
-                 measure_time.value(), vision_pose.X().value(),
-                 vision_pose.Y().value(),
-                 vision_pose.Rotation().Radians().value());
+                       measure_time.value(), vision_pose.X().value(),
+                       vision_pose.Y().value(),
+                       vision_pose.Rotation().Radians().value());
     }
   }
 
@@ -151,41 +154,53 @@ void testFollowTrajectory(
 TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
   wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
 
-  wpi::math::DifferentialDrivePoseEstimator estimator{
-      kinematics,         wpi::math::Rotation2d{}, 0_m, 0_m, wpi::math::Pose2d{},
-      {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};
+  wpi::math::DifferentialDrivePoseEstimator estimator{kinematics,
+                                                      wpi::math::Rotation2d{},
+                                                      0_m,
+                                                      0_m,
+                                                      wpi::math::Pose2d{},
+                                                      {0.02, 0.02, 0.01},
+                                                      {0.1, 0.1, 0.1}};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
 
   testFollowTrajectory(
       kinematics, estimator, trajectory,
       [&](wpi::math::Trajectory::State& state) {
         return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
-                                  state.velocity * state.curvature};
+                                        state.velocity * state.curvature};
       },
       [&](wpi::math::Trajectory::State& state) { return state.pose; },
-      trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
-      100_ms, 250_ms, true, false);
+      trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
+      20_ms, 100_ms, 250_ms, true, false);
 }
 
 TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
   wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
 
-  wpi::math::DifferentialDrivePoseEstimator estimator{
-      kinematics,         wpi::math::Rotation2d{}, 0_m, 0_m, wpi::math::Pose2d{},
-      {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};
+  wpi::math::DifferentialDrivePoseEstimator estimator{kinematics,
+                                                      wpi::math::Rotation2d{},
+                                                      0_m,
+                                                      0_m,
+                                                      wpi::math::Pose2d{},
+                                                      {0.02, 0.02, 0.01},
+                                                      {0.1, 0.1, 0.1}};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
 
   for (wpi::units::degree_t offset_direction_degs = 0_deg;
        offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
@@ -194,21 +209,21 @@ TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
       auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
       auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
 
-      auto initial_pose =
-          trajectory.InitialPose() +
-          wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
-                                              pose_offset.Sin() * 1_m},
-                           heading_offset};
+      auto initial_pose = trajectory.InitialPose() +
+                          wpi::math::Transform2d{
+                              wpi::math::Translation2d{pose_offset.Cos() * 1_m,
+                                                       pose_offset.Sin() * 1_m},
+                              heading_offset};
 
       testFollowTrajectory(
           kinematics, estimator, trajectory,
           [&](wpi::math::Trajectory::State& state) {
             return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
-                                      state.velocity * state.curvature};
+                                            state.velocity * state.curvature};
           },
           [&](wpi::math::Trajectory::State& state) { return state.pose; },
-          initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
-          250_ms, false, false);
+          initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
+          100_ms, 250_ms, false, false);
     }
   }
 }
@@ -285,7 +300,8 @@ TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
 
   // Apply a vision measurement from 3 seconds ago
   estimator.AddVisionMeasurement(
-      wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m}, wpi::math::Rotation2d{0.1_rad}},
+      wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m},
+                        wpi::math::Rotation2d{0.1_rad}},
       1_s, {0.1, 0.1, 0.1});
 
   EXPECT_NEAR(odometryPose.X().value(),
@@ -310,16 +326,19 @@ TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) {
   // Add a tiny tolerance for the upper bound because of floating point rounding
   // error
   for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
-    estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation2d{},
-                             wpi::units::meter_t{time}, wpi::units::meter_t{time});
+    estimator.UpdateWithTime(wpi::units::second_t{time},
+                             wpi::math::Rotation2d{}, wpi::units::meter_t{time},
+                             wpi::units::meter_t{time});
   }
 
   // Sample at an added time
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.02_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.02_s));
   // Sample between updates (test interpolation)
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.01_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.01_s));
   // Sampling before the oldest value returns the oldest value
   EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
             estimator.SampleAt(0.5_s));
@@ -329,31 +348,37 @@ TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) {
 
   // Add a vision measurement after the odometry measurements (while keeping all
   // of the old odometry measurements)
-  estimator.AddVisionMeasurement(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}},
-                                 2.2_s);
+  estimator.AddVisionMeasurement(
+      wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}}, 2.2_s);
 
   // Make sure nothing changed (except the newest value)
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.02_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.01_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.02_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.01_s));
   EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
             estimator.SampleAt(0.5_s));
 
   // Add a vision measurement before the odometry measurements that's still in
   // the buffer
-  estimator.AddVisionMeasurement(wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}},
-                                 0.9_s);
+  estimator.AddVisionMeasurement(
+      wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}}, 0.9_s);
 
   // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.02_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.01_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(0.5_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(2.5_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.02_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.01_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(0.5_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(2.5_s));
 }
 
 TEST(DifferentialDrivePoseEstimatorTest, TestReset) {

wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp

@@ -18,7 +18,8 @@
 
 namespace {
 
-wpi::math::Vectord<5> Dynamics(const wpi::math::Vectord<5>& x, const wpi::math::Vectord<2>& u) {
+wpi::math::Vectord<5> Dynamics(const wpi::math::Vectord<5>& x,
+                               const wpi::math::Vectord<2>& u) {
   auto motors = wpi::math::DCMotor::CIM(2);
 
   // constexpr double Glow = 15.32;       // Low gear ratio
@@ -50,12 +51,14 @@ wpi::math::Vectord<5> Dynamics(const wpi::math::Vectord<5>& x, const wpi::math::
 }
 
 wpi::math::Vectord<3> LocalMeasurementModel(
-    const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
+    const wpi::math::Vectord<5>& x,
+    [[maybe_unused]] const wpi::math::Vectord<2>& u) {
   return wpi::math::Vectord<3>{x(2), x(3), x(4)};
 }
 
 wpi::math::Vectord<5> GlobalMeasurementModel(
-    const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
+    const wpi::math::Vectord<5>& x,
+    [[maybe_unused]] const wpi::math::Vectord<2>& u) {
   return wpi::math::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
 }
 }  // namespace
@@ -64,10 +67,10 @@ TEST(ExtendedKalmanFilterTest, Init) {
   constexpr wpi::units::second_t dt = 5_ms;
 
   wpi::math::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
-                                              LocalMeasurementModel,
-                                              {0.5, 0.5, 10.0, 1.0, 1.0},
-                                              {0.0001, 0.01, 0.01},
-                                              dt};
+                                                    LocalMeasurementModel,
+                                                    {0.5, 0.5, 10.0, 1.0, 1.0},
+                                                    {0.0001, 0.01, 0.01},
+                                                    dt};
   wpi::math::Vectord<2> u{12.0, 12.0};
   observer.Predict(u, dt);
 
@@ -84,22 +87,22 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
   constexpr auto rb = 0.8382_m / 2.0;  // Robot radius
 
   wpi::math::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
-                                              LocalMeasurementModel,
-                                              {0.5, 0.5, 10.0, 1.0, 1.0},
-                                              {0.0001, 0.5, 0.5},
-                                              dt};
+                                                    LocalMeasurementModel,
+                                                    {0.5, 0.5, 10.0, 1.0, 1.0},
+                                                    {0.0001, 0.5, 0.5},
+                                                    dt};
 
-  auto waypoints =
-      std::vector<wpi::math::Pose2d>{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
-                               wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+  auto waypoints = std::vector<wpi::math::Pose2d>{
+      wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
+      wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
   auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
       waypoints, {8.8_mps, 0.1_mps_sq});
 
   wpi::math::Vectord<5> r = wpi::math::Vectord<5>::Zero();
   wpi::math::Vectord<2> u = wpi::math::Vectord<2>::Zero();
 
-  auto B = wpi::math::NumericalJacobianU<5, 5, 2>(Dynamics, wpi::math::Vectord<5>::Zero(),
-                                            wpi::math::Vectord<2>::Zero());
+  auto B = wpi::math::NumericalJacobianU<5, 5, 2>(
+      Dynamics, wpi::math::Vectord<5>::Zero(), wpi::math::Vectord<2>::Zero());
 
   observer.SetXhat(wpi::math::Vectord<5>{
       trajectory.InitialPose().Translation().X().value(),
@@ -119,10 +122,12 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
         ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
 
     auto localY = LocalMeasurementModel(nextR, wpi::math::Vectord<2>::Zero());
-    observer.Correct(u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
+    observer.Correct(
+        u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
 
     wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
-    u = B.householderQr().solve(rdot - Dynamics(r, wpi::math::Vectord<2>::Zero()));
+    u = B.householderQr().solve(rdot -
+                                Dynamics(r, wpi::math::Vectord<2>::Zero()));
 
     observer.Predict(u, dt);
 

wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp

@@ -16,6 +16,7 @@
 
 TEST(KalmanFilterTest, Flywheel) {
   auto motor = wpi::math::DCMotor::NEO();
-  auto flywheel = wpi::math::LinearSystemId::FlywheelSystem(motor, 1_kg_sq_m, 1.0);
+  auto flywheel =
+      wpi::math::LinearSystemId::FlywheelSystem(motor, 1_kg_sq_m, 1.0);
   wpi::math::KalmanFilter<1, 1, 1> kf{flywheel, {1}, {1}, 5_ms};
 }

wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp

@@ -38,14 +38,16 @@ void testFollowTrajectory(
   wpi::units::second_t t = 0_s;
 
   std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
-  std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
+  std::vector<
+      std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
       visionLog;
 
   double maxError = -std::numeric_limits<double>::max();
   double errorSum = 0;
 
   if (debug) {
-    wpi::util::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
+    wpi::util::print(
+        "time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
   }
 
   while (t < trajectory.TotalTime()) {
@@ -57,9 +59,10 @@ void testFollowTrajectory(
         visionPoses.back().first + kVisionUpdateRate < t) {
       auto visionPose =
           visionMeasurementGenerator(groundTruthState) +
-          wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
-                                              distribution(generator) * 0.1_m},
-                           wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
+          wpi::math::Transform2d{
+              wpi::math::Translation2d{distribution(generator) * 0.1_m,
+                                       distribution(generator) * 0.1_m},
+              wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
       visionPoses.push_back({t, visionPose});
     }
 
@@ -85,9 +88,10 @@ void testFollowTrajectory(
 
     auto xhat = estimator.UpdateWithTime(
         t,
-        wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
-                        wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
-                        trajectory.InitialPose().Rotation()},
+        wpi::math::Rotation3d{
+            groundTruthState.pose.Rotation() +
+            wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
+            trajectory.InitialPose().Rotation()},
         wheelPositions);
 
     if (debug) {
@@ -111,7 +115,8 @@ void testFollowTrajectory(
   }
 
   if (debug) {
-    wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
+    wpi::util::print(
+        "apply_time, measured_time, vision_x, vision_y, vision_theta\n");
 
     wpi::units::second_t apply_time;
     wpi::units::second_t measure_time;
@@ -119,9 +124,9 @@ void testFollowTrajectory(
     for (auto record : visionLog) {
       std::tie(apply_time, measure_time, vision_pose) = record;
       wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
-                 measure_time.value(), vision_pose.X().value(),
-                 vision_pose.Y().value(),
-                 vision_pose.Rotation().Radians().value());
+                       measure_time.value(), vision_pose.X().value(),
+                       vision_pose.Y().value(),
+                       vision_pose.Rotation().Radians().value());
     }
   }
 
@@ -147,49 +152,55 @@ void testFollowTrajectory(
 TEST(MecanumDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::MecanumDriveWheelPositions wheelPositions;
 
   wpi::math::MecanumDrivePoseEstimator3d estimator{
-      kinematics,    wpi::math::Rotation3d{},    wheelPositions,
-      wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
+      kinematics,          wpi::math::Rotation3d{}, wheelPositions,
+      wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1},    {0.45, 0.45, 0.45, 0.45}};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
 
   testFollowTrajectory(
       kinematics, estimator, trajectory,
       [&](wpi::math::Trajectory::State& state) {
         return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
-                                  state.velocity * state.curvature};
+                                        state.velocity * state.curvature};
       },
       [&](wpi::math::Trajectory::State& state) { return state.pose; },
-      trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
-      100_ms, 250_ms, true, false);
+      trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
+      20_ms, 100_ms, 250_ms, true, false);
 }
 
 TEST(MecanumDrivePoseEstimator3dTest, BadInitialPose) {
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::MecanumDriveWheelPositions wheelPositions;
 
   wpi::math::MecanumDrivePoseEstimator3d estimator{
-      kinematics,    wpi::math::Rotation3d{},    wheelPositions,
-      wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.1}};
+      kinematics,          wpi::math::Rotation3d{}, wheelPositions,
+      wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1},    {0.45, 0.45, 0.45, 0.1}};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
 
   for (wpi::units::degree_t offset_direction_degs = 0_deg;
        offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
@@ -198,21 +209,21 @@ TEST(MecanumDrivePoseEstimator3dTest, BadInitialPose) {
       auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
       auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
 
-      auto initial_pose =
-          trajectory.InitialPose() +
-          wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
-                                              pose_offset.Sin() * 1_m},
-                           heading_offset};
+      auto initial_pose = trajectory.InitialPose() +
+                          wpi::math::Transform2d{
+                              wpi::math::Translation2d{pose_offset.Cos() * 1_m,
+                                                       pose_offset.Sin() * 1_m},
+                              heading_offset};
 
       testFollowTrajectory(
           kinematics, estimator, trajectory,
           [&](wpi::math::Trajectory::State& state) {
             return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
-                                      state.velocity * state.curvature};
+                                            state.velocity * state.curvature};
           },
           [&](wpi::math::Trajectory::State& state) { return state.pose; },
-          initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
-          250_ms, false, false);
+          initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
+          100_ms, 250_ms, false, false);
     }
   }
 }
@@ -225,7 +236,8 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
   // pose would converge to that measurement.
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::MecanumDriveWheelPositions wheelPositions;
 
@@ -233,7 +245,8 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
       kinematics,
       wpi::math::Rotation3d{},
       wheelPositions,
-      wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
+      wpi::math::Pose3d{1_m, 2_m, 0_m,
+                        wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
       {0.1, 0.1, 0.1, 0.1},
       {0.45, 0.45, 0.45, 0.1}};
 
@@ -241,11 +254,16 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
 
   for (int i = 0; i < 1000; i++) {
     estimator.AddVisionMeasurement(
-        wpi::math::Pose3d{0_m, 0_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
+        wpi::math::Pose3d{0_m, 0_m, 0_m,
+                          wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}},
+        0_s);
     estimator.AddVisionMeasurement(
-        wpi::math::Pose3d{3_m, 1_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
+        wpi::math::Pose3d{3_m, 1_m, 0_m,
+                          wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}},
+        0_s);
     estimator.AddVisionMeasurement(
-        wpi::math::Pose3d{2_m, 4_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
+        wpi::math::Pose3d{2_m, 4_m, 0_m,
+                          wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
         0_s);
   }
 
@@ -283,11 +301,16 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
 TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::MecanumDrivePoseEstimator3d estimator{
-      kinematics,    wpi::math::Rotation3d{},    wpi::math::MecanumDriveWheelPositions{},
-      wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
+      kinematics,
+      wpi::math::Rotation3d{},
+      wpi::math::MecanumDriveWheelPositions{},
+      wpi::math::Pose3d{},
+      {0.1, 0.1, 0.1, 0.1},
+      {0.45, 0.45, 0.45, 0.45}};
 
   // Add enough measurements to fill up the buffer
   for (auto time = 0_s; time < 4_s; time += 20_ms) {
@@ -299,8 +322,9 @@ TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
 
   // Apply a vision measurement from 3 seconds ago
   estimator.AddVisionMeasurement(
-      wpi::math::Pose3d{10_m, 10_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
-      {0.1, 0.1, 0.1, 0.1});
+      wpi::math::Pose3d{10_m, 10_m, 0_m,
+                        wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}},
+      1_s, {0.1, 0.1, 0.1, 0.1});
 
   EXPECT_NEAR(odometryPose.X().value(),
               estimator.GetEstimatedPosition().X().value(), 1e-6);
@@ -319,10 +343,15 @@ TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
 TEST(MecanumDrivePoseEstimator3dTest, TestSampleAt) {
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
   wpi::math::MecanumDrivePoseEstimator3d estimator{
-      kinematics,    wpi::math::Rotation3d{},    wpi::math::MecanumDriveWheelPositions{},
-      wpi::math::Pose3d{}, {1.0, 1.0, 1.0, 1.0}, {1.0, 1.0, 1.0, 1.0}};
+      kinematics,
+      wpi::math::Rotation3d{},
+      wpi::math::MecanumDriveWheelPositions{},
+      wpi::math::Pose3d{},
+      {1.0, 1.0, 1.0, 1.0},
+      {1.0, 1.0, 1.0, 1.0}};
 
   // Returns empty when null
   EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
@@ -332,37 +361,46 @@ TEST(MecanumDrivePoseEstimator3dTest, TestSampleAt) {
   // error
   for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
     wpi::math::MecanumDriveWheelPositions wheelPositions{
-        wpi::units::meter_t{time}, wpi::units::meter_t{time}, wpi::units::meter_t{time},
-        wpi::units::meter_t{time}};
-    estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation3d{},
-                             wheelPositions);
+        wpi::units::meter_t{time}, wpi::units::meter_t{time},
+        wpi::units::meter_t{time}, wpi::units::meter_t{time}};
+    estimator.UpdateWithTime(wpi::units::second_t{time},
+                             wpi::math::Rotation3d{}, wheelPositions);
   }
 
   // Sample at an added time
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.02_s));
   // Sample between updates (test interpolation)
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.01_s));
   // Sampling before the oldest value returns the oldest value
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
-            estimator.SampleAt(0.5_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+      estimator.SampleAt(0.5_s));
   // Sampling after the newest value returns the newest value
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
-            estimator.SampleAt(2.5_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+      estimator.SampleAt(2.5_s));
 
   // Add a vision measurement after the odometry measurements (while keeping all
   // of the old odometry measurements)
   estimator.AddVisionMeasurement(
-      wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
+      wpi::math::Pose3d{2_m, 0_m, 0_m,
+                        wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
+      2.2_s);
 
   // Make sure nothing changed (except the newest value)
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.02_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.01_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
-            estimator.SampleAt(0.5_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+      estimator.SampleAt(0.5_s));
 
   // Add a vision measurement before the odometry measurements that's still in
   // the buffer
@@ -370,25 +408,31 @@ TEST(MecanumDrivePoseEstimator3dTest, TestSampleAt) {
       wpi::math::Pose3d{1_m, 0.2_m, 0_m, wpi::math::Rotation3d{}}, 0.9_s);
 
   // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.02_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.01_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(0.5_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(2.5_s));
 }
 
 TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
   wpi::math::MecanumDrivePoseEstimator3d estimator{
       kinematics,
       wpi::math::Rotation3d{},
       wpi::math::MecanumDriveWheelPositions{},
-      wpi::math::Pose3d{-1_m, -1_m, -1_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
+      wpi::math::Pose3d{-1_m, -1_m, -1_m,
+                        wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
       {1.0, 1.0, 1.0, 1.0},
       {1.0, 1.0, 1.0, 1.0}};
 
@@ -401,8 +445,9 @@ TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
   EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Rotation().Z().value());
 
   // Test reset position
-  estimator.ResetPosition(wpi::math::Rotation3d{}, {1_m, 1_m, 1_m, 1_m},
-                          wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
+  estimator.ResetPosition(
+      wpi::math::Rotation3d{}, {1_m, 1_m, 1_m, 1_m},
+      wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
 
   EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());

wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp

@@ -30,7 +30,8 @@ void testFollowTrajectory(
     const bool debug) {
   wpi::math::MecanumDriveWheelPositions wheelPositions{};
 
-  estimator.ResetPosition(wpi::math::Rotation2d{}, wheelPositions, startingPose);
+  estimator.ResetPosition(wpi::math::Rotation2d{}, wheelPositions,
+                          startingPose);
 
   std::default_random_engine generator;
   std::normal_distribution<double> distribution(0.0, 1.0);
@@ -38,14 +39,16 @@ void testFollowTrajectory(
   wpi::units::second_t t = 0_s;
 
   std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
-  std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
+  std::vector<
+      std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
       visionLog;
 
   double maxError = -std::numeric_limits<double>::max();
   double errorSum = 0;
 
   if (debug) {
-    wpi::util::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
+    wpi::util::print(
+        "time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
   }
 
   while (t < trajectory.TotalTime()) {
@@ -57,9 +60,10 @@ void testFollowTrajectory(
         visionPoses.back().first + kVisionUpdateRate < t) {
       auto visionPose =
           visionMeasurementGenerator(groundTruthState) +
-          wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
-                                              distribution(generator) * 0.1_m},
-                           wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
+          wpi::math::Transform2d{
+              wpi::math::Translation2d{distribution(generator) * 0.1_m,
+                                       distribution(generator) * 0.1_m},
+              wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
       visionPoses.push_back({t, visionPose});
     }
 
@@ -90,11 +94,11 @@ void testFollowTrajectory(
         wheelPositions);
 
     if (debug) {
-      wpi::util::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
-                 xhat.Y().value(), xhat.Rotation().Radians().value(),
-                 groundTruthState.pose.X().value(),
-                 groundTruthState.pose.Y().value(),
-                 groundTruthState.pose.Rotation().Radians().value());
+      wpi::util::print(
+          "{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
+          xhat.Y().value(), xhat.Rotation().Radians().value(),
+          groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
+          groundTruthState.pose.Rotation().Radians().value());
     }
 
     double error = groundTruthState.pose.Translation()
@@ -110,7 +114,8 @@ void testFollowTrajectory(
   }
 
   if (debug) {
-    wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
+    wpi::util::print(
+        "apply_time, measured_time, vision_x, vision_y, vision_theta\n");
 
     wpi::units::second_t apply_time;
     wpi::units::second_t measure_time;
@@ -118,9 +123,9 @@ void testFollowTrajectory(
     for (auto record : visionLog) {
       std::tie(apply_time, measure_time, vision_pose) = record;
       wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
-                 measure_time.value(), vision_pose.X().value(),
-                 vision_pose.Y().value(),
-                 vision_pose.Rotation().Radians().value());
+                       measure_time.value(), vision_pose.X().value(),
+                       vision_pose.Y().value(),
+                       vision_pose.Rotation().Radians().value());
     }
   }
 
@@ -142,49 +147,55 @@ void testFollowTrajectory(
 TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::MecanumDriveWheelPositions wheelPositions;
 
-  wpi::math::MecanumDrivePoseEstimator estimator{kinematics,      wpi::math::Rotation2d{},
-                                           wheelPositions,  wpi::math::Pose2d{},
-                                           {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
+  wpi::math::MecanumDrivePoseEstimator estimator{
+      kinematics,          wpi::math::Rotation2d{}, wheelPositions,
+      wpi::math::Pose2d{}, {0.1, 0.1, 0.1},         {0.45, 0.45, 0.45}};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
 
   testFollowTrajectory(
       kinematics, estimator, trajectory,
       [&](wpi::math::Trajectory::State& state) {
         return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
-                                  state.velocity * state.curvature};
+                                        state.velocity * state.curvature};
       },
       [&](wpi::math::Trajectory::State& state) { return state.pose; },
-      trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
-      100_ms, 250_ms, true, false);
+      trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
+      20_ms, 100_ms, 250_ms, true, false);
 }
 
 TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::MecanumDriveWheelPositions wheelPositions;
 
-  wpi::math::MecanumDrivePoseEstimator estimator{kinematics,      wpi::math::Rotation2d{},
-                                           wheelPositions,  wpi::math::Pose2d{},
-                                           {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
+  wpi::math::MecanumDrivePoseEstimator estimator{
+      kinematics,          wpi::math::Rotation2d{}, wheelPositions,
+      wpi::math::Pose2d{}, {0.1, 0.1, 0.1},         {0.45, 0.45, 0.1}};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
 
   for (wpi::units::degree_t offset_direction_degs = 0_deg;
        offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
@@ -193,21 +204,21 @@ TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
       auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
       auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
 
-      auto initial_pose =
-          trajectory.InitialPose() +
-          wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
-                                              pose_offset.Sin() * 1_m},
-                           heading_offset};
+      auto initial_pose = trajectory.InitialPose() +
+                          wpi::math::Transform2d{
+                              wpi::math::Translation2d{pose_offset.Cos() * 1_m,
+                                                       pose_offset.Sin() * 1_m},
+                              heading_offset};
 
       testFollowTrajectory(
           kinematics, estimator, trajectory,
           [&](wpi::math::Trajectory::State& state) {
             return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
-                                      state.velocity * state.curvature};
+                                            state.velocity * state.curvature};
           },
           [&](wpi::math::Trajectory::State& state) { return state.pose; },
-          initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
-          250_ms, false, false);
+          initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
+          100_ms, 250_ms, false, false);
     }
   }
 }
@@ -220,14 +231,18 @@ TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
   // pose would converge to that measurement.
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::MecanumDriveWheelPositions wheelPositions;
 
   wpi::math::MecanumDrivePoseEstimator estimator{
-      kinematics,      wpi::math::Rotation2d{},
-      wheelPositions,  wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
-      {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
+      kinematics,
+      wpi::math::Rotation2d{},
+      wheelPositions,
+      wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
+      {0.1, 0.1, 0.1},
+      {0.45, 0.45, 0.1}};
 
   estimator.UpdateWithTime(0_s, wpi::math::Rotation2d{}, wheelPositions);
 
@@ -271,11 +286,16 @@ TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
 TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::MecanumDrivePoseEstimator estimator{
-      kinematics,    wpi::math::Rotation2d{}, wpi::math::MecanumDriveWheelPositions{},
-      wpi::math::Pose2d{}, {0.1, 0.1, 0.1},   {0.45, 0.45, 0.45}};
+      kinematics,
+      wpi::math::Rotation2d{},
+      wpi::math::MecanumDriveWheelPositions{},
+      wpi::math::Pose2d{},
+      {0.1, 0.1, 0.1},
+      {0.45, 0.45, 0.45}};
 
   // Add enough measurements to fill up the buffer
   for (auto time = 0_s; time < 4_s; time += 20_ms) {
@@ -287,7 +307,8 @@ TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
 
   // Apply a vision measurement from 3 seconds ago
   estimator.AddVisionMeasurement(
-      wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m}, wpi::math::Rotation2d{0.1_rad}},
+      wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m},
+                        wpi::math::Rotation2d{0.1_rad}},
       1_s, {0.1, 0.1, 0.1});
 
   EXPECT_NEAR(odometryPose.X().value(),
@@ -302,10 +323,15 @@ TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
 TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) {
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
   wpi::math::MecanumDrivePoseEstimator estimator{
-      kinematics,    wpi::math::Rotation2d{}, wpi::math::MecanumDriveWheelPositions{},
-      wpi::math::Pose2d{}, {1.0, 1.0, 1.0},   {1.0, 1.0, 1.0}};
+      kinematics,
+      wpi::math::Rotation2d{},
+      wpi::math::MecanumDriveWheelPositions{},
+      wpi::math::Pose2d{},
+      {1.0, 1.0, 1.0},
+      {1.0, 1.0, 1.0}};
 
   // Returns empty when null
   EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
@@ -315,18 +341,20 @@ TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) {
   // error
   for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
     wpi::math::MecanumDriveWheelPositions wheelPositions{
-        wpi::units::meter_t{time}, wpi::units::meter_t{time}, wpi::units::meter_t{time},
-        wpi::units::meter_t{time}};
-    estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation2d{},
-                             wheelPositions);
+        wpi::units::meter_t{time}, wpi::units::meter_t{time},
+        wpi::units::meter_t{time}, wpi::units::meter_t{time}};
+    estimator.UpdateWithTime(wpi::units::second_t{time},
+                             wpi::math::Rotation2d{}, wheelPositions);
   }
 
   // Sample at an added time
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.02_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.02_s));
   // Sample between updates (test interpolation)
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.01_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.01_s));
   // Sampling before the oldest value returns the oldest value
   EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
             estimator.SampleAt(0.5_s));
@@ -336,37 +364,44 @@ TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) {
 
   // Add a vision measurement after the odometry measurements (while keeping all
   // of the old odometry measurements)
-  estimator.AddVisionMeasurement(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}},
-                                 2.2_s);
+  estimator.AddVisionMeasurement(
+      wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}}, 2.2_s);
 
   // Make sure nothing changed (except the newest value)
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.02_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.01_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.02_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.01_s));
   EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
             estimator.SampleAt(0.5_s));
 
   // Add a vision measurement before the odometry measurements that's still in
   // the buffer
-  estimator.AddVisionMeasurement(wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}},
-                                 0.9_s);
+  estimator.AddVisionMeasurement(
+      wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}}, 0.9_s);
 
   // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.02_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.01_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(0.5_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(2.5_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.02_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.01_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(0.5_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(2.5_s));
 }
 
 TEST(MecanumDrivePoseEstimatorTest, TestReset) {
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
   wpi::math::MecanumDrivePoseEstimator estimator{
       kinematics,
       wpi::math::Rotation2d{},

wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp

@@ -9,11 +9,13 @@
 TEST(MerweScaledSigmaPointsTest, ZeroMean) {
   wpi::math::MerweScaledSigmaPoints<2> sigmaPoints;
   auto points = sigmaPoints.SquareRootSigmaPoints(
-      wpi::math::Vectord<2>{0.0, 0.0}, wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
+      wpi::math::Vectord<2>{0.0, 0.0},
+      wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
 
   EXPECT_TRUE(
-      (points - wpi::math::Matrixd<2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
-                                   {0.0, 0.0, 0.00173205, 0.0, -0.00173205}})
+      (points -
+       wpi::math::Matrixd<2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
+                                {0.0, 0.0, 0.00173205, 0.0, -0.00173205}})
           .norm() < 1e-3);
 }
 
@@ -25,6 +27,6 @@ TEST(MerweScaledSigmaPointsTest, NonzeroMean) {
 
   EXPECT_TRUE(
       (points - wpi::math::Matrixd<2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
-                                   {2.0, 2.0, 2.00548, 2.0, 1.99452}})
+                                         {2.0, 2.0, 2.00548, 2.0, 1.99452}})
           .norm() < 1e-3);
 }

wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp

@@ -26,7 +26,7 @@ namespace {
 
 // First test system, differential drive
 wpi::math::Vectord<5> DriveDynamics(const wpi::math::Vectord<5>& x,
-                              const wpi::math::Vectord<2>& u) {
+                                    const wpi::math::Vectord<2>& u) {
   auto motors = wpi::math::DCMotor::CIM(2);
 
   // constexpr double Glow = 15.32;    // Low gear ratio
@@ -58,12 +58,14 @@ wpi::math::Vectord<5> DriveDynamics(const wpi::math::Vectord<5>& x,
 }
 
 wpi::math::Vectord<3> DriveLocalMeasurementModel(
-    const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
+    const wpi::math::Vectord<5>& x,
+    [[maybe_unused]] const wpi::math::Vectord<2>& u) {
   return wpi::math::Vectord<3>{x(2), x(3), x(4)};
 }
 
 wpi::math::Vectord<5> DriveGlobalMeasurementModel(
-    const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
+    const wpi::math::Vectord<5>& x,
+    [[maybe_unused]] const wpi::math::Vectord<2>& u) {
   return wpi::math::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
 }
 
@@ -71,15 +73,15 @@ TEST(MerweUKFTest, DriveInit) {
   constexpr wpi::units::second_t dt = 5_ms;
 
   wpi::math::MerweUKF<5, 2, 3> observer{DriveDynamics,
-                                  DriveLocalMeasurementModel,
-                                  {0.5, 0.5, 10.0, 1.0, 1.0},
-                                  {0.0001, 0.01, 0.01},
-                                  wpi::math::AngleMean<5, 2 * 5 + 1>(2),
-                                  wpi::math::AngleMean<3, 2 * 5 + 1>(0),
-                                  wpi::math::AngleResidual<5>(2),
-                                  wpi::math::AngleResidual<3>(0),
-                                  wpi::math::AngleAdd<5>(2),
-                                  dt};
+                                        DriveLocalMeasurementModel,
+                                        {0.5, 0.5, 10.0, 1.0, 1.0},
+                                        {0.0001, 0.01, 0.01},
+                                        wpi::math::AngleMean<5, 2 * 5 + 1>(2),
+                                        wpi::math::AngleMean<3, 2 * 5 + 1>(0),
+                                        wpi::math::AngleResidual<5>(2),
+                                        wpi::math::AngleResidual<3>(0),
+                                        wpi::math::AngleAdd<5>(2),
+                                        dt};
   wpi::math::Vectord<2> u{12.0, 12.0};
   observer.Predict(u, dt);
 
@@ -88,9 +90,10 @@ TEST(MerweUKFTest, DriveInit) {
 
   auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
   auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
-  observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
-                      wpi::math::AngleMean<5, 2 * 5 + 1>(2), wpi::math::AngleResidual<5>(2),
-                      wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2));
+  observer.Correct<5>(
+      u, globalY, DriveGlobalMeasurementModel, R,
+      wpi::math::AngleMean<5, 2 * 5 + 1>(2), wpi::math::AngleResidual<5>(2),
+      wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2));
 }
 
 TEST(MerweUKFTest, DriveConvergence) {
@@ -98,19 +101,19 @@ TEST(MerweUKFTest, DriveConvergence) {
   constexpr auto rb = 0.8382_m / 2.0;  // Robot radius
 
   wpi::math::MerweUKF<5, 2, 3> observer{DriveDynamics,
-                                  DriveLocalMeasurementModel,
-                                  {0.5, 0.5, 10.0, 1.0, 1.0},
-                                  {0.0001, 0.5, 0.5},
-                                  wpi::math::AngleMean<5, 2 * 5 + 1>(2),
-                                  wpi::math::AngleMean<3, 2 * 5 + 1>(0),
-                                  wpi::math::AngleResidual<5>(2),
-                                  wpi::math::AngleResidual<3>(0),
-                                  wpi::math::AngleAdd<5>(2),
-                                  dt};
+                                        DriveLocalMeasurementModel,
+                                        {0.5, 0.5, 10.0, 1.0, 1.0},
+                                        {0.0001, 0.5, 0.5},
+                                        wpi::math::AngleMean<5, 2 * 5 + 1>(2),
+                                        wpi::math::AngleMean<3, 2 * 5 + 1>(0),
+                                        wpi::math::AngleResidual<5>(2),
+                                        wpi::math::AngleResidual<3>(0),
+                                        wpi::math::AngleAdd<5>(2),
+                                        dt};
 
-  auto waypoints =
-      std::vector<wpi::math::Pose2d>{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
-                               wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+  auto waypoints = std::vector<wpi::math::Pose2d>{
+      wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
+      wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
   auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
       waypoints, {8.8_mps, 0.1_mps_sq});
 
@@ -118,7 +121,8 @@ TEST(MerweUKFTest, DriveConvergence) {
   wpi::math::Vectord<2> u = wpi::math::Vectord<2>::Zero();
 
   auto B = wpi::math::NumericalJacobianU<5, 5, 2>(
-      DriveDynamics, wpi::math::Vectord<5>::Zero(), wpi::math::Vectord<2>::Zero());
+      DriveDynamics, wpi::math::Vectord<5>::Zero(),
+      wpi::math::Vectord<2>::Zero());
 
   observer.SetXhat(wpi::math::Vectord<5>{
       trajectory.InitialPose().Translation().X().value(),
@@ -139,12 +143,14 @@ TEST(MerweUKFTest, DriveConvergence) {
         ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
         ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
 
-    auto localY = DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
-    observer.Correct(u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
+    auto localY =
+        DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
+    observer.Correct(
+        u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
 
     wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
-    u = B.householderQr().solve(rdot -
-                                DriveDynamics(r, wpi::math::Vectord<2>::Zero()));
+    u = B.householderQr().solve(
+        rdot - DriveDynamics(r, wpi::math::Vectord<2>::Zero()));
 
     observer.Predict(u, dt);
 
@@ -158,7 +164,8 @@ TEST(MerweUKFTest, DriveConvergence) {
   auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
   auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
   observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
-                      wpi::math::AngleMean<5, 2 * 5 + 1>(2), wpi::math::AngleResidual<5>(2),
+                      wpi::math::AngleMean<5, 2 * 5 + 1>(2),
+                      wpi::math::AngleResidual<5>(2),
                       wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2)
 
   );
@@ -176,8 +183,9 @@ TEST(MerweUKFTest, DriveConvergence) {
 
 TEST(MerweUKFTest, LinearUKF) {
   constexpr wpi::units::second_t dt = 20_ms;
-  auto plant = wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
-      0.02_V / 1_mps, 0.006_V / 1_mps_sq);
+  auto plant =
+      wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
+          0.02_V / 1_mps, 0.006_V / 1_mps_sq);
   wpi::math::MerweUKF<1, 1, 1> observer{
       [&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
         return plant.A() * x + plant.B() * u;
@@ -209,8 +217,12 @@ TEST(MerweUKFTest, RoundTripP) {
   constexpr wpi::units::second_t dt = 5_ms;
 
   wpi::math::MerweUKF<2, 2, 2> observer{
-      [](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
-      [](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
+      [](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) {
+        return x;
+      },
+      [](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) {
+        return x;
+      },
       {0.0, 0.0},
       {0.0, 0.0},
       dt};
@@ -223,7 +235,7 @@ TEST(MerweUKFTest, RoundTripP) {
 
 // Second system, single motor feedforward estimator
 wpi::math::Vectord<4> MotorDynamics(const wpi::math::Vectord<4>& x,
-                              const wpi::math::Vectord<1>& u) {
+                                    const wpi::math::Vectord<1>& u) {
   double v = x(1);
   double kV = x(2);
   double kA = x(3);
@@ -235,7 +247,7 @@ wpi::math::Vectord<4> MotorDynamics(const wpi::math::Vectord<4>& x,
 }
 
 wpi::math::Vectord<3> MotorMeasurementModel(const wpi::math::Vectord<4>& x,
-                                      const wpi::math::Vectord<1>& u) {
+                                            const wpi::math::Vectord<1>& u) {
   double p = x(0);
   double v = x(1);
   double kV = x(2);
@@ -271,9 +283,9 @@ TEST(MerweUKFTest, MotorConvergence) {
   states[0] = wpi::math::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
 
   constexpr wpi::math::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
-                                 {0.0, -true_kV / true_kA, 0.0, 0.0},
-                                 {0.0, 0.0, 0.0, 0.0},
-                                 {0.0, 0.0, 0.0, 0.0}};
+                                       {0.0, -true_kV / true_kA, 0.0, 0.0},
+                                       {0.0, 0.0, 0.0, 0.0},
+                                       {0.0, 0.0, 0.0, 0.0}};
   constexpr wpi::math::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
 
   wpi::math::Matrixd<4, 4> discA;
@@ -291,7 +303,8 @@ TEST(MerweUKFTest, MotorConvergence) {
   wpi::math::Vectord<4> P0{0.001, 0.001, 10, 10};
 
   wpi::math::MerweUKF<4, 1, 3> observer{
-      MotorDynamics, MotorMeasurementModel, wpi::util::array{0.1, 1.0, 1e-10, 1e-10},
+      MotorDynamics, MotorMeasurementModel,
+      wpi::util::array{0.1, 1.0, 1e-10, 1e-10},
       wpi::util::array{pos_stddev, vel_stddev, accel_stddev}, dt};
 
   observer.SetXhat(wpi::math::Vectord<4>{0.0, 0.0, 2.0, 2.0});

wpimath/src/test/native/cpp/estimator/S3SigmaPointsTest.cpp

@@ -29,11 +29,13 @@ TEST(S3SigmaPointsTest, Simplex) {
 TEST(S3SigmaPointsTest, ZeroMean) {
   wpi::math::S3SigmaPoints<2> sigmaPoints;
   auto points = sigmaPoints.SquareRootSigmaPoints(
-      wpi::math::Vectord<2>{0.0, 0.0}, wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
+      wpi::math::Vectord<2>{0.0, 0.0},
+      wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
 
   EXPECT_TRUE(
-      (points - wpi::math::Matrixd<2, 4>{{0.0, -0.00122474, 0.00122474, 0.0},
-                                   {0.0, -0.00070711, -0.00070711, 0.00141421}})
+      (points -
+       wpi::math::Matrixd<2, 4>{{0.0, -0.00122474, 0.00122474, 0.0},
+                                {0.0, -0.00070711, -0.00070711, 0.00141421}})
           .norm() < 1e-7);
 }
 
@@ -44,7 +46,8 @@ TEST(S3SigmaPointsTest, NonzeroMean) {
       wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
 
   EXPECT_TRUE(
-      (points - wpi::math::Matrixd<2, 4>{{1.0, 0.99877526, 1.00122474, 1.0},
-                                   {2.0, 1.99776393, 1.99776393, 2.00447214}})
+      (points -
+       wpi::math::Matrixd<2, 4>{{1.0, 0.99877526, 1.00122474, 1.0},
+                                {2.0, 1.99776393, 1.99776393, 2.00447214}})
           .norm() < 1e-7);
 }

wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp

@@ -26,7 +26,7 @@ namespace {
 
 // First test system, differential drive
 wpi::math::Vectord<5> DriveDynamics(const wpi::math::Vectord<5>& x,
-                              const wpi::math::Vectord<2>& u) {
+                                    const wpi::math::Vectord<2>& u) {
   auto motors = wpi::math::DCMotor::CIM(2);
 
   // constexpr double Glow = 15.32;    // Low gear ratio
@@ -58,12 +58,14 @@ wpi::math::Vectord<5> DriveDynamics(const wpi::math::Vectord<5>& x,
 }
 
 wpi::math::Vectord<3> DriveLocalMeasurementModel(
-    const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
+    const wpi::math::Vectord<5>& x,
+    [[maybe_unused]] const wpi::math::Vectord<2>& u) {
   return wpi::math::Vectord<3>{x(2), x(3), x(4)};
 }
 
 wpi::math::Vectord<5> DriveGlobalMeasurementModel(
-    const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
+    const wpi::math::Vectord<5>& x,
+    [[maybe_unused]] const wpi::math::Vectord<2>& u) {
   return wpi::math::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
 }
 
@@ -71,15 +73,15 @@ TEST(S3UKFTest, DriveInit) {
   constexpr auto dt = 5_ms;
 
   wpi::math::S3UKF<5, 2, 3> observer{DriveDynamics,
-                               DriveLocalMeasurementModel,
-                               {0.5, 0.5, 10.0, 1.0, 1.0},
-                               {0.0001, 0.01, 0.01},
-                               wpi::math::AngleMean<5, 5 + 2>(2),
-                               wpi::math::AngleMean<3, 5 + 2>(0),
-                               wpi::math::AngleResidual<5>(2),
-                               wpi::math::AngleResidual<3>(0),
-                               wpi::math::AngleAdd<5>(2),
-                               dt};
+                                     DriveLocalMeasurementModel,
+                                     {0.5, 0.5, 10.0, 1.0, 1.0},
+                                     {0.0001, 0.01, 0.01},
+                                     wpi::math::AngleMean<5, 5 + 2>(2),
+                                     wpi::math::AngleMean<3, 5 + 2>(0),
+                                     wpi::math::AngleResidual<5>(2),
+                                     wpi::math::AngleResidual<3>(0),
+                                     wpi::math::AngleAdd<5>(2),
+                                     dt};
   wpi::math::Vectord<2> u{12.0, 12.0};
   observer.Predict(u, dt);
 
@@ -88,9 +90,10 @@ TEST(S3UKFTest, DriveInit) {
 
   auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
   auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
-  observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
-                      wpi::math::AngleMean<5, 5 + 2>(2), wpi::math::AngleResidual<5>(2),
-                      wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2));
+  observer.Correct<5>(
+      u, globalY, DriveGlobalMeasurementModel, R,
+      wpi::math::AngleMean<5, 5 + 2>(2), wpi::math::AngleResidual<5>(2),
+      wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2));
 }
 
 TEST(S3UKFTest, DriveConvergence) {
@@ -98,19 +101,19 @@ TEST(S3UKFTest, DriveConvergence) {
   constexpr auto rb = 0.8382_m / 2.0;  // Robot radius
 
   wpi::math::S3UKF<5, 2, 3> observer{DriveDynamics,
-                               DriveLocalMeasurementModel,
-                               {0.5, 0.5, 10.0, 1.0, 1.0},
-                               {0.0001, 0.5, 0.5},
-                               wpi::math::AngleMean<5, 5 + 2>(2),
-                               wpi::math::AngleMean<3, 5 + 2>(0),
-                               wpi::math::AngleResidual<5>(2),
-                               wpi::math::AngleResidual<3>(0),
-                               wpi::math::AngleAdd<5>(2),
-                               dt};
+                                     DriveLocalMeasurementModel,
+                                     {0.5, 0.5, 10.0, 1.0, 1.0},
+                                     {0.0001, 0.5, 0.5},
+                                     wpi::math::AngleMean<5, 5 + 2>(2),
+                                     wpi::math::AngleMean<3, 5 + 2>(0),
+                                     wpi::math::AngleResidual<5>(2),
+                                     wpi::math::AngleResidual<3>(0),
+                                     wpi::math::AngleAdd<5>(2),
+                                     dt};
 
-  auto waypoints =
-      std::vector<wpi::math::Pose2d>{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
-                               wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+  auto waypoints = std::vector<wpi::math::Pose2d>{
+      wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
+      wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
   auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
       waypoints, {8.8_mps, 0.1_mps_sq});
 
@@ -118,7 +121,8 @@ TEST(S3UKFTest, DriveConvergence) {
   wpi::math::Vectord<2> u = wpi::math::Vectord<2>::Zero();
 
   auto B = wpi::math::NumericalJacobianU<5, 5, 2>(
-      DriveDynamics, wpi::math::Vectord<5>::Zero(), wpi::math::Vectord<2>::Zero());
+      DriveDynamics, wpi::math::Vectord<5>::Zero(),
+      wpi::math::Vectord<2>::Zero());
 
   observer.SetXhat(wpi::math::Vectord<5>{
       trajectory.InitialPose().Translation().X().value(),
@@ -139,12 +143,14 @@ TEST(S3UKFTest, DriveConvergence) {
         ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
         ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
 
-    auto localY = DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
-    observer.Correct(u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
+    auto localY =
+        DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
+    observer.Correct(
+        u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
 
     wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
-    u = B.householderQr().solve(rdot -
-                                DriveDynamics(r, wpi::math::Vectord<2>::Zero()));
+    u = B.householderQr().solve(
+        rdot - DriveDynamics(r, wpi::math::Vectord<2>::Zero()));
 
     observer.Predict(u, dt);
 
@@ -158,7 +164,8 @@ TEST(S3UKFTest, DriveConvergence) {
   auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
   auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
   observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
-                      wpi::math::AngleMean<5, 5 + 2>(2), wpi::math::AngleResidual<5>(2),
+                      wpi::math::AngleMean<5, 5 + 2>(2),
+                      wpi::math::AngleResidual<5>(2),
                       wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2)
 
   );
@@ -176,8 +183,9 @@ TEST(S3UKFTest, DriveConvergence) {
 
 TEST(S3UKFTest, LinearUKF) {
   constexpr wpi::units::second_t dt = 20_ms;
-  auto plant = wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
-      0.02_V / 1_mps, 0.006_V / 1_mps_sq);
+  auto plant =
+      wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
+          0.02_V / 1_mps, 0.006_V / 1_mps_sq);
   wpi::math::S3UKF<1, 1, 1> observer{
       [&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
         return plant.A() * x + plant.B() * u;
@@ -209,8 +217,12 @@ TEST(S3UKFTest, RoundTripP) {
   constexpr auto dt = 5_ms;
 
   wpi::math::S3UKF<2, 2, 2> observer{
-      [](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
-      [](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
+      [](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) {
+        return x;
+      },
+      [](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) {
+        return x;
+      },
       {0.0, 0.0},
       {0.0, 0.0},
       dt};
@@ -223,7 +235,7 @@ TEST(S3UKFTest, RoundTripP) {
 
 // Second system, single motor feedforward estimator
 wpi::math::Vectord<4> MotorDynamics(const wpi::math::Vectord<4>& x,
-                              const wpi::math::Vectord<1>& u) {
+                                    const wpi::math::Vectord<1>& u) {
   double v = x(1);
   double kV = x(2);
   double kA = x(3);
@@ -235,7 +247,7 @@ wpi::math::Vectord<4> MotorDynamics(const wpi::math::Vectord<4>& x,
 }
 
 wpi::math::Vectord<3> MotorMeasurementModel(const wpi::math::Vectord<4>& x,
-                                      const wpi::math::Vectord<1>& u) {
+                                            const wpi::math::Vectord<1>& u) {
   double p = x(0);
   double v = x(1);
   double kV = x(2);
@@ -271,9 +283,9 @@ TEST(S3UKFTest, MotorConvergence) {
   states[0] = wpi::math::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
 
   constexpr wpi::math::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
-                                 {0.0, -true_kV / true_kA, 0.0, 0.0},
-                                 {0.0, 0.0, 0.0, 0.0},
-                                 {0.0, 0.0, 0.0, 0.0}};
+                                       {0.0, -true_kV / true_kA, 0.0, 0.0},
+                                       {0.0, 0.0, 0.0, 0.0},
+                                       {0.0, 0.0, 0.0, 0.0}};
   constexpr wpi::math::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
 
   wpi::math::Matrixd<4, 4> discA;
@@ -291,7 +303,8 @@ TEST(S3UKFTest, MotorConvergence) {
   wpi::math::Vectord<4> P0{0.001, 0.001, 10, 10};
 
   wpi::math::S3UKF<4, 1, 3> observer{
-      MotorDynamics, MotorMeasurementModel, wpi::util::array{0.1, 1.0, 1e-10, 1e-10},
+      MotorDynamics, MotorMeasurementModel,
+      wpi::util::array{0.1, 1.0, 1e-10, 1e-10},
       wpi::util::array{pos_stddev, vel_stddev, accel_stddev}, dt};
 
   observer.SetXhat(wpi::math::Vectord<4>{0.0, 0.0, 2.0, 2.0});

wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp

@@ -29,7 +29,8 @@ void testFollowTrajectory(
     const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
     const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
     const bool debug) {
-  wpi::util::array<wpi::math::SwerveModulePosition, 4> positions{wpi::util::empty_array};
+  wpi::util::array<wpi::math::SwerveModulePosition, 4> positions{
+      wpi::util::empty_array};
 
   estimator.ResetPosition(wpi::math::Rotation3d{}, positions,
                           wpi::math::Pose3d{startingPose});
@@ -40,14 +41,16 @@ void testFollowTrajectory(
   wpi::units::second_t t = 0_s;
 
   std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
-  std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
+  std::vector<
+      std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
       visionLog;
 
   double maxError = -std::numeric_limits<double>::max();
   double errorSum = 0;
 
   if (debug) {
-    wpi::util::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
+    wpi::util::print(
+        "time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
   }
 
   while (t < trajectory.TotalTime()) {
@@ -59,9 +62,10 @@ void testFollowTrajectory(
         visionPoses.back().first + kVisionUpdateRate < t) {
       auto visionPose =
           visionMeasurementGenerator(groundTruthState) +
-          wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
-                                              distribution(generator) * 0.1_m},
-                           wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
+          wpi::math::Transform2d{
+              wpi::math::Translation2d{distribution(generator) * 0.1_m,
+                                       distribution(generator) * 0.1_m},
+              wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
       visionPoses.push_back({t, visionPose});
     }
 
@@ -87,9 +91,10 @@ void testFollowTrajectory(
 
     auto xhat = estimator.UpdateWithTime(
         t,
-        wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
-                        wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
-                        trajectory.InitialPose().Rotation()},
+        wpi::math::Rotation3d{
+            groundTruthState.pose.Rotation() +
+            wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
+            trajectory.InitialPose().Rotation()},
         positions);
 
     if (debug) {
@@ -113,7 +118,8 @@ void testFollowTrajectory(
   }
 
   if (debug) {
-    wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
+    wpi::util::print(
+        "apply_time, measured_time, vision_x, vision_y, vision_theta\n");
 
     wpi::units::second_t apply_time;
     wpi::units::second_t measure_time;
@@ -121,9 +127,9 @@ void testFollowTrajectory(
     for (auto record : visionLog) {
       std::tie(apply_time, measure_time, vision_pose) = record;
       wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
-                 measure_time.value(), vision_pose.X().value(),
-                 vision_pose.Y().value(),
-                 vision_pose.Rotation().Radians().value());
+                       measure_time.value(), vision_pose.X().value(),
+                       vision_pose.Y().value(),
+                       vision_pose.Rotation().Radians().value());
     }
   }
 
@@ -149,7 +155,8 @@ void testFollowTrajectory(
 TEST(SwerveDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
   wpi::math::SwerveDriveKinematics<4> kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::SwerveModulePosition fl;
   wpi::math::SwerveModulePosition fr;
@@ -157,31 +164,35 @@ TEST(SwerveDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
   wpi::math::SwerveModulePosition br;
 
   wpi::math::SwerveDrivePoseEstimator3d<4> estimator{
-      kinematics,    wpi::math::Rotation3d{},    {fl, fr, bl, br},
-      wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
+      kinematics,          wpi::math::Rotation3d{}, {fl, fr, bl, br},
+      wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1},    {0.45, 0.45, 0.45, 0.45}};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
 
   testFollowTrajectory(
       kinematics, estimator, trajectory,
       [&](wpi::math::Trajectory::State& state) {
         return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
-                                  state.velocity * state.curvature};
+                                        state.velocity * state.curvature};
       },
       [&](wpi::math::Trajectory::State& state) { return state.pose; },
-      {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
-      20_ms, 100_ms, 250_ms, true, false);
+      {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
+      {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms, 250_ms, true,
+      false);
 }
 
 TEST(SwerveDrivePoseEstimator3dTest, BadInitialPose) {
   wpi::math::SwerveDriveKinematics<4> kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::SwerveModulePosition fl;
   wpi::math::SwerveModulePosition fr;
@@ -189,15 +200,17 @@ TEST(SwerveDrivePoseEstimator3dTest, BadInitialPose) {
   wpi::math::SwerveModulePosition br;
 
   wpi::math::SwerveDrivePoseEstimator3d<4> estimator{
-      kinematics,    wpi::math::Rotation3d{},    {fl, fr, bl, br},
-      wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.9, 0.9, 0.9, 0.9}};
+      kinematics,          wpi::math::Rotation3d{}, {fl, fr, bl, br},
+      wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1},    {0.9, 0.9, 0.9, 0.9}};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
 
   for (wpi::units::degree_t offset_direction_degs = 0_deg;
        offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
@@ -206,21 +219,21 @@ TEST(SwerveDrivePoseEstimator3dTest, BadInitialPose) {
       auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
       auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
 
-      auto initial_pose =
-          trajectory.InitialPose() +
-          wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
-                                              pose_offset.Sin() * 1_m},
-                           heading_offset};
+      auto initial_pose = trajectory.InitialPose() +
+                          wpi::math::Transform2d{
+                              wpi::math::Translation2d{pose_offset.Cos() * 1_m,
+                                                       pose_offset.Sin() * 1_m},
+                              heading_offset};
 
       testFollowTrajectory(
           kinematics, estimator, trajectory,
           [&](wpi::math::Trajectory::State& state) {
             return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
-                                      state.velocity * state.curvature};
+                                            state.velocity * state.curvature};
           },
           [&](wpi::math::Trajectory::State& state) { return state.pose; },
-          initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
-          250_ms, false, false);
+          initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
+          100_ms, 250_ms, false, false);
     }
   }
 }
@@ -233,7 +246,8 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
   // pose would converge to that measurement.
   wpi::math::SwerveDriveKinematics<4> kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::SwerveModulePosition fl;
   wpi::math::SwerveModulePosition fr;
@@ -244,7 +258,8 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
       kinematics,
       wpi::math::Rotation3d{},
       {fl, fr, bl, br},
-      wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
+      wpi::math::Pose3d{1_m, 2_m, 0_m,
+                        wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
       {0.1, 0.1, 0.1, 0.1},
       {0.45, 0.45, 0.45, 0.45}};
 
@@ -252,11 +267,16 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
 
   for (int i = 0; i < 1000; i++) {
     estimator.AddVisionMeasurement(
-        wpi::math::Pose3d{0_m, 0_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
+        wpi::math::Pose3d{0_m, 0_m, 0_m,
+                          wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}},
+        0_s);
     estimator.AddVisionMeasurement(
-        wpi::math::Pose3d{3_m, 1_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
+        wpi::math::Pose3d{3_m, 1_m, 0_m,
+                          wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}},
+        0_s);
     estimator.AddVisionMeasurement(
-        wpi::math::Pose3d{2_m, 4_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
+        wpi::math::Pose3d{2_m, 4_m, 0_m,
+                          wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
         0_s);
   }
 
@@ -294,7 +314,8 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
 TEST(SwerveDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
   wpi::math::SwerveDriveKinematics<4> kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::SwerveModulePosition fl;
   wpi::math::SwerveModulePosition fr;
@@ -302,8 +323,8 @@ TEST(SwerveDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
   wpi::math::SwerveModulePosition br;
 
   wpi::math::SwerveDrivePoseEstimator3d<4> estimator{
-      kinematics,    wpi::math::Rotation3d{},    {fl, fr, bl, br},
-      wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
+      kinematics,          wpi::math::Rotation3d{}, {fl, fr, bl, br},
+      wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1},    {0.45, 0.45, 0.45, 0.45}};
 
   // Add enough measurements to fill up the buffer
   for (auto time = 0_s; time < 4_s; time += 20_ms) {
@@ -314,8 +335,9 @@ TEST(SwerveDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
 
   // Apply a vision measurement from 3 seconds ago
   estimator.AddVisionMeasurement(
-      wpi::math::Pose3d{10_m, 10_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
-      {0.1, 0.1, 0.1, 0.1});
+      wpi::math::Pose3d{10_m, 10_m, 0_m,
+                        wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}},
+      1_s, {0.1, 0.1, 0.1, 0.1});
 
   EXPECT_NEAR(odometryPose.X().value(),
               estimator.GetEstimatedPosition().X().value(), 1e-6);
@@ -334,7 +356,8 @@ TEST(SwerveDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
 TEST(SwerveDrivePoseEstimator3dTest, TestSampleAt) {
   wpi::math::SwerveDriveKinematics<4> kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
   wpi::math::SwerveDrivePoseEstimator3d estimator{
       kinematics,
       wpi::math::Rotation3d{},
@@ -352,39 +375,52 @@ TEST(SwerveDrivePoseEstimator3dTest, TestSampleAt) {
   // error
   for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
     wpi::util::array<wpi::math::SwerveModulePosition, 4> wheelPositions{
-        {wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
-         wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
-         wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
-         wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}}}};
-    estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation3d{},
-                             wheelPositions);
+        {wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
+                                         wpi::math::Rotation2d{}},
+         wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
+                                         wpi::math::Rotation2d{}},
+         wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
+                                         wpi::math::Rotation2d{}},
+         wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
+                                         wpi::math::Rotation2d{}}}};
+    estimator.UpdateWithTime(wpi::units::second_t{time},
+                             wpi::math::Rotation3d{}, wheelPositions);
   }
 
   // Sample at an added time
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.02_s));
   // Sample between updates (test interpolation)
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.01_s));
   // Sampling before the oldest value returns the oldest value
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
-            estimator.SampleAt(0.5_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+      estimator.SampleAt(0.5_s));
   // Sampling after the newest value returns the newest value
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
-            estimator.SampleAt(2.5_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+      estimator.SampleAt(2.5_s));
 
   // Add a vision measurement after the odometry measurements (while keeping all
   // of the old odometry measurements)
   estimator.AddVisionMeasurement(
-      wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
+      wpi::math::Pose3d{2_m, 0_m, 0_m,
+                        wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
+      2.2_s);
 
   // Make sure nothing changed (except the newest value)
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.02_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.01_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
-            estimator.SampleAt(0.5_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
+      estimator.SampleAt(0.5_s));
 
   // Add a vision measurement before the odometry measurements that's still in
   // the buffer
@@ -392,26 +428,32 @@ TEST(SwerveDrivePoseEstimator3dTest, TestSampleAt) {
       wpi::math::Pose3d{1_m, 0.2_m, 0_m, wpi::math::Rotation3d{}}, 0.9_s);
 
   // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.02_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(1.01_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(0.5_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
+  EXPECT_EQ(std::optional(
+                wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
             estimator.SampleAt(2.5_s));
 }
 
 TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
   wpi::math::SwerveDriveKinematics<4> kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
   wpi::math::SwerveDrivePoseEstimator3d estimator{
       kinematics,
       wpi::math::Rotation3d{},
       {wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{},
        wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{}},
-      wpi::math::Pose3d{-1_m, -1_m, -1_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
+      wpi::math::Pose3d{-1_m, -1_m, -1_m,
+                        wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
       {1.0, 1.0, 1.0, 1.0},
       {1.0, 1.0, 1.0, 1.0}};
 
@@ -425,7 +467,8 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
 
   // Test reset position
   {
-    wpi::math::SwerveModulePosition modulePosition{1_m, wpi::math::Rotation2d{}};
+    wpi::math::SwerveModulePosition modulePosition{1_m,
+                                                   wpi::math::Rotation2d{}};
     estimator.ResetPosition(
         wpi::math::Rotation3d{},
         {modulePosition, modulePosition, modulePosition, modulePosition},
@@ -441,9 +484,10 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
 
   // Test orientation and wheel positions
   {
-    wpi::math::SwerveModulePosition modulePosition{2_m, wpi::math::Rotation2d{}};
+    wpi::math::SwerveModulePosition modulePosition{2_m,
+                                                   wpi::math::Rotation2d{}};
     estimator.Update(wpi::math::Rotation3d{}, {modulePosition, modulePosition,
-                                         modulePosition, modulePosition});
+                                               modulePosition, modulePosition});
   }
 
   EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
@@ -466,9 +510,10 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
 
   // Test orientation
   {
-    wpi::math::SwerveModulePosition modulePosition{3_m, wpi::math::Rotation2d{}};
+    wpi::math::SwerveModulePosition modulePosition{3_m,
+                                                   wpi::math::Rotation2d{}};
     estimator.Update(wpi::math::Rotation3d{}, {modulePosition, modulePosition,
-                                         modulePosition, modulePosition});
+                                               modulePosition, modulePosition});
   }
 
   EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());

wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp

@@ -30,7 +30,8 @@ void testFollowTrajectory(
     const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
     const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
     const bool debug) {
-  wpi::util::array<wpi::math::SwerveModulePosition, 4> positions{wpi::util::empty_array};
+  wpi::util::array<wpi::math::SwerveModulePosition, 4> positions{
+      wpi::util::empty_array};
 
   estimator.ResetPosition(wpi::math::Rotation2d{}, positions, startingPose);
 
@@ -40,14 +41,16 @@ void testFollowTrajectory(
   wpi::units::second_t t = 0_s;
 
   std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
-  std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
+  std::vector<
+      std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
       visionLog;
 
   double maxError = -std::numeric_limits<double>::max();
   double errorSum = 0;
 
   if (debug) {
-    wpi::util::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
+    wpi::util::print(
+        "time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
   }
 
   while (t < trajectory.TotalTime()) {
@@ -59,9 +62,10 @@ void testFollowTrajectory(
         visionPoses.back().first + kVisionUpdateRate < t) {
       auto visionPose =
           visionMeasurementGenerator(groundTruthState) +
-          wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
-                                              distribution(generator) * 0.1_m},
-                           wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
+          wpi::math::Transform2d{
+              wpi::math::Translation2d{distribution(generator) * 0.1_m,
+                                       distribution(generator) * 0.1_m},
+              wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
       visionPoses.push_back({t, visionPose});
     }
 
@@ -92,11 +96,11 @@ void testFollowTrajectory(
         positions);
 
     if (debug) {
-      wpi::util::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
-                 xhat.Y().value(), xhat.Rotation().Radians().value(),
-                 groundTruthState.pose.X().value(),
-                 groundTruthState.pose.Y().value(),
-                 groundTruthState.pose.Rotation().Radians().value());
+      wpi::util::print(
+          "{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
+          xhat.Y().value(), xhat.Rotation().Radians().value(),
+          groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
+          groundTruthState.pose.Rotation().Radians().value());
     }
 
     double error = groundTruthState.pose.Translation()
@@ -112,7 +116,8 @@ void testFollowTrajectory(
   }
 
   if (debug) {
-    wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
+    wpi::util::print(
+        "apply_time, measured_time, vision_x, vision_y, vision_theta\n");
 
     wpi::units::second_t apply_time;
     wpi::units::second_t measure_time;
@@ -120,9 +125,9 @@ void testFollowTrajectory(
     for (auto record : visionLog) {
       std::tie(apply_time, measure_time, vision_pose) = record;
       wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
-                 measure_time.value(), vision_pose.X().value(),
-                 vision_pose.Y().value(),
-                 vision_pose.Rotation().Radians().value());
+                       measure_time.value(), vision_pose.X().value(),
+                       vision_pose.Y().value(),
+                       vision_pose.Rotation().Radians().value());
     }
   }
 
@@ -144,7 +149,8 @@ void testFollowTrajectory(
 TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
   wpi::math::SwerveDriveKinematics<4> kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::SwerveModulePosition fl;
   wpi::math::SwerveModulePosition fr;
@@ -152,31 +158,35 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
   wpi::math::SwerveModulePosition br;
 
   wpi::math::SwerveDrivePoseEstimator<4> estimator{
-      kinematics,    wpi::math::Rotation2d{}, {fl, fr, bl, br},
-      wpi::math::Pose2d{}, {0.1, 0.1, 0.1},   {0.45, 0.45, 0.45}};
+      kinematics,          wpi::math::Rotation2d{}, {fl, fr, bl, br},
+      wpi::math::Pose2d{}, {0.1, 0.1, 0.1},         {0.45, 0.45, 0.45}};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
 
   testFollowTrajectory(
       kinematics, estimator, trajectory,
       [&](wpi::math::Trajectory::State& state) {
         return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
-                                  state.velocity * state.curvature};
+                                        state.velocity * state.curvature};
       },
       [&](wpi::math::Trajectory::State& state) { return state.pose; },
-      {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
-      20_ms, 100_ms, 250_ms, true, false);
+      {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
+      {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms, 250_ms, true,
+      false);
 }
 
 TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
   wpi::math::SwerveDriveKinematics<4> kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::SwerveModulePosition fl;
   wpi::math::SwerveModulePosition fr;
@@ -184,15 +194,17 @@ TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
   wpi::math::SwerveModulePosition br;
 
   wpi::math::SwerveDrivePoseEstimator<4> estimator{
-      kinematics,    wpi::math::Rotation2d{}, {fl, fr, bl, br},
-      wpi::math::Pose2d{}, {0.1, 0.1, 0.1},   {0.9, 0.9, 0.9}};
+      kinematics,          wpi::math::Rotation2d{}, {fl, fr, bl, br},
+      wpi::math::Pose2d{}, {0.1, 0.1, 0.1},         {0.9, 0.9, 0.9}};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
 
   for (wpi::units::degree_t offset_direction_degs = 0_deg;
        offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
@@ -201,21 +213,21 @@ TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
       auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
       auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
 
-      auto initial_pose =
-          trajectory.InitialPose() +
-          wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
-                                              pose_offset.Sin() * 1_m},
-                           heading_offset};
+      auto initial_pose = trajectory.InitialPose() +
+                          wpi::math::Transform2d{
+                              wpi::math::Translation2d{pose_offset.Cos() * 1_m,
+                                                       pose_offset.Sin() * 1_m},
+                              heading_offset};
 
       testFollowTrajectory(
           kinematics, estimator, trajectory,
           [&](wpi::math::Trajectory::State& state) {
             return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
-                                      state.velocity * state.curvature};
+                                            state.velocity * state.curvature};
           },
           [&](wpi::math::Trajectory::State& state) { return state.pose; },
-          initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
-          250_ms, false, false);
+          initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
+          100_ms, 250_ms, false, false);
     }
   }
 }
@@ -228,7 +240,8 @@ TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
   // pose would converge to that measurement.
   wpi::math::SwerveDriveKinematics<4> kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::SwerveModulePosition fl;
   wpi::math::SwerveModulePosition fr;
@@ -236,9 +249,12 @@ TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
   wpi::math::SwerveModulePosition br;
 
   wpi::math::SwerveDrivePoseEstimator<4> estimator{
-      kinematics,       wpi::math::Rotation2d{},
-      {fl, fr, bl, br}, wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
-      {0.1, 0.1, 0.1},  {0.45, 0.45, 0.45}};
+      kinematics,
+      wpi::math::Rotation2d{},
+      {fl, fr, bl, br},
+      wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
+      {0.1, 0.1, 0.1},
+      {0.45, 0.45, 0.45}};
 
   estimator.UpdateWithTime(0_s, wpi::math::Rotation2d{}, {fl, fr, bl, br});
 
@@ -282,7 +298,8 @@ TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
 TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
   wpi::math::SwerveDriveKinematics<4> kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::SwerveModulePosition fl;
   wpi::math::SwerveModulePosition fr;
@@ -290,8 +307,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
   wpi::math::SwerveModulePosition br;
 
   wpi::math::SwerveDrivePoseEstimator<4> estimator{
-      kinematics,    wpi::math::Rotation2d{}, {fl, fr, bl, br},
-      wpi::math::Pose2d{}, {0.1, 0.1, 0.1},   {0.45, 0.45, 0.45}};
+      kinematics,          wpi::math::Rotation2d{}, {fl, fr, bl, br},
+      wpi::math::Pose2d{}, {0.1, 0.1, 0.1},         {0.45, 0.45, 0.45}};
 
   // Add enough measurements to fill up the buffer
   for (auto time = 0_s; time < 4_s; time += 20_ms) {
@@ -302,7 +319,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
 
   // Apply a vision measurement from 3 seconds ago
   estimator.AddVisionMeasurement(
-      wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m}, wpi::math::Rotation2d{0.1_rad}},
+      wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m},
+                        wpi::math::Rotation2d{0.1_rad}},
       1_s, {0.1, 0.1, 0.1});
 
   EXPECT_NEAR(odometryPose.X().value(),
@@ -317,7 +335,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
 TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) {
   wpi::math::SwerveDriveKinematics<4> kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
   wpi::math::SwerveDrivePoseEstimator estimator{
       kinematics,
       wpi::math::Rotation2d{},
@@ -335,20 +354,26 @@ TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) {
   // error
   for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
     wpi::util::array<wpi::math::SwerveModulePosition, 4> wheelPositions{
-        {wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
-         wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
-         wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
-         wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}}}};
-    estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation2d{},
-                             wheelPositions);
+        {wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
+                                         wpi::math::Rotation2d{}},
+         wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
+                                         wpi::math::Rotation2d{}},
+         wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
+                                         wpi::math::Rotation2d{}},
+         wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
+                                         wpi::math::Rotation2d{}}}};
+    estimator.UpdateWithTime(wpi::units::second_t{time},
+                             wpi::math::Rotation2d{}, wheelPositions);
   }
 
   // Sample at an added time
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.02_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.02_s));
   // Sample between updates (test interpolation)
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.01_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.01_s));
   // Sampling before the oldest value returns the oldest value
   EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
             estimator.SampleAt(0.5_s));
@@ -358,37 +383,44 @@ TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) {
 
   // Add a vision measurement after the odometry measurements (while keeping all
   // of the old odometry measurements)
-  estimator.AddVisionMeasurement(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}},
-                                 2.2_s);
+  estimator.AddVisionMeasurement(
+      wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}}, 2.2_s);
 
   // Make sure nothing changed (except the newest value)
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.02_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.01_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.02_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.01_s));
   EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
             estimator.SampleAt(0.5_s));
 
   // Add a vision measurement before the odometry measurements that's still in
   // the buffer
-  estimator.AddVisionMeasurement(wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}},
-                                 0.9_s);
+  estimator.AddVisionMeasurement(
+      wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}}, 0.9_s);
 
   // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.02_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(1.01_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(0.5_s));
-  EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
-            estimator.SampleAt(2.5_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.02_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(1.01_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(0.5_s));
+  EXPECT_EQ(
+      std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
+      estimator.SampleAt(2.5_s));
 }
 
 TEST(SwerveDrivePoseEstimatorTest, TestReset) {
   wpi::math::SwerveDriveKinematics<4> kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
   wpi::math::SwerveDrivePoseEstimator estimator{
       kinematics,
       wpi::math::Rotation2d{},
@@ -406,7 +438,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
 
   // Test reset position
   {
-    wpi::math::SwerveModulePosition modulePosition{1_m, wpi::math::Rotation2d{}};
+    wpi::math::SwerveModulePosition modulePosition{1_m,
+                                                   wpi::math::Rotation2d{}};
     estimator.ResetPosition(
         wpi::math::Rotation2d{},
         {modulePosition, modulePosition, modulePosition, modulePosition},
@@ -420,9 +453,10 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
 
   // Test orientation and wheel positions
   {
-    wpi::math::SwerveModulePosition modulePosition{2_m, wpi::math::Rotation2d{}};
+    wpi::math::SwerveModulePosition modulePosition{2_m,
+                                                   wpi::math::Rotation2d{}};
     estimator.Update(wpi::math::Rotation2d{}, {modulePosition, modulePosition,
-                                         modulePosition, modulePosition});
+                                               modulePosition, modulePosition});
   }
 
   EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
@@ -441,9 +475,10 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
 
   // Test orientation
   {
-    wpi::math::SwerveModulePosition modulePosition{3_m, wpi::math::Rotation2d{}};
+    wpi::math::SwerveModulePosition modulePosition{3_m,
+                                                   wpi::math::Rotation2d{}};
     estimator.Update(wpi::math::Rotation2d{}, {modulePosition, modulePosition,
-                                         modulePosition, modulePosition});
+                                               modulePosition, modulePosition});
   }
 
   EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());

wpimath/src/test/native/cpp/filter/DebouncerTest.cpp

@@ -13,7 +13,8 @@ static wpi::units::second_t now = 0_s;
 class DebouncerTest : public ::testing::Test {
  protected:
   void SetUp() override {
-    WPI_SetNowImpl([] { return wpi::units::microsecond_t{now}.to<uint64_t>(); });
+    WPI_SetNowImpl(
+        [] { return wpi::units::microsecond_t{now}.to<uint64_t>(); });
   }
 
   void TearDown() override { WPI_SetNowImpl(nullptr); }
@@ -31,7 +32,8 @@ TEST_F(DebouncerTest, DebounceRising) {
 }
 
 TEST_F(DebouncerTest, DebounceFalling) {
-  wpi::math::Debouncer debouncer{20_ms, wpi::math::Debouncer::DebounceType::kFalling};
+  wpi::math::Debouncer debouncer{20_ms,
+                                 wpi::math::Debouncer::DebounceType::kFalling};
 
   debouncer.Calculate(true);
   EXPECT_TRUE(debouncer.Calculate(false));
@@ -42,7 +44,8 @@ TEST_F(DebouncerTest, DebounceFalling) {
 }
 
 TEST_F(DebouncerTest, DebounceBoth) {
-  wpi::math::Debouncer debouncer{20_ms, wpi::math::Debouncer::DebounceType::kBoth};
+  wpi::math::Debouncer debouncer{20_ms,
+                                 wpi::math::Debouncer::DebounceType::kBoth};
 
   debouncer.Calculate(false);
   EXPECT_FALSE(debouncer.Calculate(true));
@@ -58,7 +61,8 @@ TEST_F(DebouncerTest, DebounceBoth) {
 }
 
 TEST_F(DebouncerTest, DebounceParams) {
-  wpi::math::Debouncer debouncer{20_ms, wpi::math::Debouncer::DebounceType::kBoth};
+  wpi::math::Debouncer debouncer{20_ms,
+                                 wpi::math::Debouncer::DebounceType::kBoth};
 
   EXPECT_TRUE(debouncer.GetDebounceTime() == 20_ms);
   EXPECT_TRUE(debouncer.GetDebounceType() ==

wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp

@@ -59,7 +59,7 @@ class LinearFilterOutputTest
         break;
       case kTestHighPass:
         return wpi::math::LinearFilter<double>::HighPass(kHighPassTimeConstant,
-                                                   kFilterStep);
+                                                         kFilterStep);
         break;
       case kTestMovAvg:
         return wpi::math::LinearFilter<double>::MovingAverage(kMovAvgTaps);
@@ -121,8 +121,8 @@ INSTANTIATE_TEST_SUITE_P(Tests, LinearFilterOutputTest,
                                          kTestMovAvg, kTestPulse));
 
 template <int Derivative, int Samples, typename F, typename DfDx>
-void AssertCentralResults(F&& f, DfDx&& dfdx, wpi::units::second_t h, double min,
-                          double max) {
+void AssertCentralResults(F&& f, DfDx&& dfdx, wpi::units::second_t h,
+                          double min, double max) {
   static_assert(Samples % 2 != 0, "Number of samples must be odd.");
 
   // Generate stencil points from -(samples - 1)/2 to (samples - 1)/2
@@ -132,8 +132,8 @@ void AssertCentralResults(F&& f, DfDx&& dfdx, wpi::units::second_t h, double min
   }
 
   auto filter =
-      wpi::math::LinearFilter<double>::FiniteDifference<Derivative, Samples>(stencil,
-                                                                       h);
+      wpi::math::LinearFilter<double>::FiniteDifference<Derivative, Samples>(
+          stencil, h);
 
   for (int i = min / h.value(); i < max / h.value(); ++i) {
     // Let filter initialize
@@ -153,11 +153,11 @@ void AssertCentralResults(F&& f, DfDx&& dfdx, wpi::units::second_t h, double min
 }
 
 template <int Derivative, int Samples, typename F, typename DfDx>
-void AssertBackwardResults(F&& f, DfDx&& dfdx, wpi::units::second_t h, double min,
-                           double max) {
+void AssertBackwardResults(F&& f, DfDx&& dfdx, wpi::units::second_t h,
+                           double min, double max) {
   auto filter =
-      wpi::math::LinearFilter<double>::BackwardFiniteDifference<Derivative, Samples>(
-          h);
+      wpi::math::LinearFilter<double>::BackwardFiniteDifference<Derivative,
+                                                                Samples>(h);
 
   for (int i = min / h.value(); i < max / h.value(); ++i) {
     // Let filter initialize

wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp

@@ -15,7 +15,8 @@ static wpi::units::second_t now = 0_s;
 class SlewRateLimiterTest : public ::testing::Test {
  protected:
   void SetUp() override {
-    WPI_SetNowImpl([] { return wpi::units::microsecond_t{now}.to<uint64_t>(); });
+    WPI_SetNowImpl(
+        [] { return wpi::units::microsecond_t{now}.to<uint64_t>(); });
   }
 
   void TearDown() override { WPI_SetNowImpl(nullptr); }

wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp

@@ -92,8 +92,8 @@ TEST(Pose3dTest, RotateAround) {
 
   EXPECT_NEAR(-5.0, rotated.X().value(), 1e-9);
   EXPECT_NEAR(0.0, rotated.Y().value(), 1e-9);
-  EXPECT_NEAR(wpi::units::radian_t{180_deg}.value(), rotated.Rotation().Z().value(),
-              1e-9);
+  EXPECT_NEAR(wpi::units::radian_t{180_deg}.value(),
+              rotated.Rotation().Z().value(), 1e-9);
 }
 
 TEST(Pose3dTest, Equality) {

wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp

@@ -14,9 +14,11 @@
 using namespace wpi::math;
 
 TEST(Rotation3dTest, GimbalLockAccuracy) {
-  auto rot1 = Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi / 2}};
+  auto rot1 =
+      Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi / 2}};
   auto rot2 = Rotation3d{wpi::units::radian_t{std::numbers::pi}, 0_rad, 0_rad};
-  auto rot3 = Rotation3d{-wpi::units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
+  auto rot3 =
+      Rotation3d{-wpi::units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
   const auto result1 = rot1 + rot2 + rot3;
   const auto expected1 =
       Rotation3d{0_rad, -wpi::units::radian_t{std::numbers::pi / 2},
@@ -51,21 +53,24 @@ TEST(Rotation3dTest, GimbalLockAccuracy) {
 TEST(Rotation3dTest, InitAxisAngleAndRollPitchYaw) {
   const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
   const Rotation3d rot1{xAxis, wpi::units::radian_t{std::numbers::pi / 3}};
-  const Rotation3d rot2{wpi::units::radian_t{std::numbers::pi / 3}, 0_rad, 0_rad};
+  const Rotation3d rot2{wpi::units::radian_t{std::numbers::pi / 3}, 0_rad,
+                        0_rad};
   const Rotation3d rvec1{Eigen::Vector3d{xAxis * std::numbers::pi / 3}};
   EXPECT_EQ(rot1, rot2);
   EXPECT_EQ(rot1, rvec1);
 
   const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
   const Rotation3d rot3{yAxis, wpi::units::radian_t{std::numbers::pi / 3}};
-  const Rotation3d rot4{0_rad, wpi::units::radian_t{std::numbers::pi / 3}, 0_rad};
+  const Rotation3d rot4{0_rad, wpi::units::radian_t{std::numbers::pi / 3},
+                        0_rad};
   const Rotation3d rvec2{Eigen::Vector3d{yAxis * std::numbers::pi / 3}};
   EXPECT_EQ(rot3, rot4);
   EXPECT_EQ(rot3, rvec2);
 
   const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
   const Rotation3d rot5{zAxis, wpi::units::radian_t{std::numbers::pi / 3}};
-  const Rotation3d rot6{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi / 3}};
+  const Rotation3d rot6{0_rad, 0_rad,
+                        wpi::units::radian_t{std::numbers::pi / 3}};
   const Rotation3d rvec3{Eigen::Vector3d{zAxis * std::numbers::pi / 3}};
   EXPECT_EQ(rot5, rot6);
   EXPECT_EQ(rot5, rvec3);
@@ -102,12 +107,14 @@ TEST(Rotation3dTest, InitTwoVector) {
 
   // 90 degree CW rotation around y-axis
   const Rotation3d rot1{xAxis, zAxis};
-  const Rotation3d expected1{yAxis, wpi::units::radian_t{-std::numbers::pi / 2.0}};
+  const Rotation3d expected1{yAxis,
+                             wpi::units::radian_t{-std::numbers::pi / 2.0}};
   EXPECT_EQ(expected1, rot1);
 
   // 45 degree CCW rotation around z-axis
   const Rotation3d rot2{xAxis, Eigen::Vector3d{1.0, 1.0, 0.0}};
-  const Rotation3d expected2{zAxis, wpi::units::radian_t{std::numbers::pi / 4.0}};
+  const Rotation3d expected2{zAxis,
+                             wpi::units::radian_t{std::numbers::pi / 4.0}};
   EXPECT_EQ(expected2, rot2);
 
   // 0 degree rotation of x-axes

wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp

@@ -38,9 +38,12 @@ TEST(Twist3dTest, StraightZ) {
 TEST(Twist3dTest, QuarterCircle) {
   Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
 
-  const Twist3d quarterCircle{
-      5_m / 2.0 * std::numbers::pi,           0_m, 0_m, 0_rad, 0_rad,
-      wpi::units::radian_t{std::numbers::pi / 2.0}};
+  const Twist3d quarterCircle{5_m / 2.0 * std::numbers::pi,
+                              0_m,
+                              0_m,
+                              0_rad,
+                              0_rad,
+                              wpi::units::radian_t{std::numbers::pi / 2.0}};
   const auto quarterCirclePose = quarterCircle.Exp();
 
   Transform3d expected{5_m, 5_m, 0_m, Rotation3d{zAxis, 90_deg}};
@@ -89,8 +92,9 @@ TEST(Twist3dTest, Pose3dLogY) {
 
   const auto twist = (end - start).Log();
 
-  Twist3d expected{0_m,   0_m,    wpi::units::meter_t{5.0 / 2.0 * std::numbers::pi},
-                   0_deg, 90_deg, 0_deg};
+  Twist3d expected{
+      0_m,   0_m,    wpi::units::meter_t{5.0 / 2.0 * std::numbers::pi},
+      0_deg, 90_deg, 0_deg};
   EXPECT_EQ(expected, twist);
 
   // Make sure computed twist gives back original end pose

wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp

@@ -16,7 +16,8 @@ TEST(ChassisSpeedsTest, Discretize) {
   constexpr wpi::units::second_t dt = 10_ms;
 
   const auto speeds = target.Discretize(duration);
-  const wpi::math::Twist2d twist{speeds.vx * dt, speeds.vy * dt, speeds.omega * dt};
+  const wpi::math::Twist2d twist{speeds.vx * dt, speeds.vy * dt,
+                                 speeds.omega * dt};
 
   wpi::math::Pose2d pose;
   for (wpi::units::second_t time = 0_s; time < duration; time += dt) {
@@ -41,7 +42,8 @@ TEST(ChassisSpeedsTest, ToRobotRelative) {
 
 TEST(ChassisSpeedsTest, ToFieldRelative) {
   const auto chassisSpeeds =
-      wpi::math::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToFieldRelative(45.0_deg);
+      wpi::math::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToFieldRelative(
+          45.0_deg);
 
   EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vx.value(), kEpsilon);
   EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vy.value(), kEpsilon);

wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp

@@ -16,7 +16,8 @@ using namespace wpi::math;
 TEST(DifferentialDriveOdometry3dTest, Initialize) {
   DifferentialDriveOdometry3d odometry{
       wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}, 0_m, 0_m,
-      wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 45_deg}}};
+      wpi::math::Pose3d{1_m, 2_m, 0_m,
+                        wpi::math::Rotation3d{0_deg, 0_deg, 45_deg}}};
 
   const wpi::math::Pose3d& pose = odometry.GetPose();
 
@@ -27,11 +28,12 @@ TEST(DifferentialDriveOdometry3dTest, Initialize) {
 }
 
 TEST(DifferentialDriveOdometry3dTest, EncoderDistances) {
-  DifferentialDriveOdometry3d odometry{wpi::math::Rotation3d{0_deg, 0_deg, 45_deg},
-                                       0_m, 0_m};
+  DifferentialDriveOdometry3d odometry{
+      wpi::math::Rotation3d{0_deg, 0_deg, 45_deg}, 0_m, 0_m};
 
-  const auto& pose = odometry.Update(wpi::math::Rotation3d{0_deg, 0_deg, 135_deg},
-                                     0_m, wpi::units::meter_t{5 * std::numbers::pi});
+  const auto& pose =
+      odometry.Update(wpi::math::Rotation3d{0_deg, 0_deg, 135_deg}, 0_m,
+                      wpi::units::meter_t{5 * std::numbers::pi});
 
   EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon);
   EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon);

wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp

@@ -28,7 +28,8 @@ class MecanumDriveOdometry3dTest : public ::testing::Test {
 TEST_F(MecanumDriveOdometry3dTest, Initialize) {
   MecanumDriveOdometry3d odometry{
       kinematics, wpi::math::Rotation3d{}, zero,
-      wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 45_deg}}};
+      wpi::math::Pose3d{1_m, 2_m, 0_m,
+                        wpi::math::Rotation3d{0_deg, 0_deg, 45_deg}}};
 
   const wpi::math::Pose3d& pose = odometry.GetPose();
 
@@ -82,7 +83,8 @@ TEST_F(MecanumDriveOdometry3dTest, 90DegreeTurn) {
 }
 
 TEST_F(MecanumDriveOdometry3dTest, GyroAngleReset) {
-  odometry.ResetPosition(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}, zero, Pose3d{});
+  odometry.ResetPosition(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}, zero,
+                         Pose3d{});
 
   MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
                                          0.3536_m};
@@ -99,19 +101,22 @@ TEST_F(MecanumDriveOdometry3dTest, GyroAngleReset) {
 TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) {
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::MecanumDriveWheelPositions wheelPositions;
 
-  wpi::math::MecanumDriveOdometry3d odometry{kinematics, wpi::math::Rotation3d{},
-                                       wheelPositions};
+  wpi::math::MecanumDriveOdometry3d odometry{
+      kinematics, wpi::math::Rotation3d{}, wheelPositions};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
 
   std::default_random_engine generator;
   std::normal_distribution<double> distribution(0.0, 1.0);
@@ -140,8 +145,9 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) {
     wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
 
     auto xhat = odometry.Update(
-        wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
-                        wpi::math::Rotation2d{distribution(generator) * 0.05_rad}},
+        wpi::math::Rotation3d{
+            groundTruthState.pose.Rotation() +
+            wpi::math::Rotation2d{distribution(generator) * 0.05_rad}},
         wheelPositions);
     double error = groundTruthState.pose.Translation()
                        .Distance(xhat.Translation().ToTranslation2d())
@@ -162,19 +168,22 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) {
 TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) {
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::MecanumDriveWheelPositions wheelPositions;
 
-  wpi::math::MecanumDriveOdometry3d odometry{kinematics, wpi::math::Rotation3d{},
-                                       wheelPositions};
+  wpi::math::MecanumDriveOdometry3d odometry{
+      kinematics, wpi::math::Rotation3d{}, wheelPositions};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
 
   std::default_random_engine generator;
   std::normal_distribution<double> distribution(0.0, 1.0);

wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp

@@ -80,19 +80,22 @@ TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
 TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::MecanumDriveWheelPositions wheelPositions;
 
   wpi::math::MecanumDriveOdometry odometry{kinematics, wpi::math::Rotation2d{},
-                                     wheelPositions};
+                                           wheelPositions};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
 
   std::default_random_engine generator;
   std::normal_distribution<double> distribution(0.0, 1.0);
@@ -120,10 +123,10 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
     wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
     wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
 
-    auto xhat =
-        odometry.Update(groundTruthState.pose.Rotation() +
-                            wpi::math::Rotation2d{distribution(generator) * 0.05_rad},
-                        wheelPositions);
+    auto xhat = odometry.Update(
+        groundTruthState.pose.Rotation() +
+            wpi::math::Rotation2d{distribution(generator) * 0.05_rad},
+        wheelPositions);
     double error = groundTruthState.pose.Translation()
                        .Distance(xhat.Translation())
                        .value();
@@ -143,19 +146,22 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
 TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) {
   wpi::math::MecanumDriveKinematics kinematics{
       wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
-      wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
+      wpi::math::Translation2d{-1_m, -1_m},
+      wpi::math::Translation2d{-1_m, 1_m}};
 
   wpi::math::MecanumDriveWheelPositions wheelPositions;
 
   wpi::math::MecanumDriveOdometry odometry{kinematics, wpi::math::Rotation2d{},
-                                     wheelPositions};
+                                           wheelPositions};
 
-  wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 135_deg},
-                  wpi::math::Pose2d{-3_m, 0_m, -90_deg},
-                  wpi::math::Pose2d{0_m, 0_m, 45_deg}},
-      wpi::math::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+  wpi::math::Trajectory trajectory =
+      wpi::math::TrajectoryGenerator::GenerateTrajectory(
+          std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
+                      wpi::math::Pose2d{3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 135_deg},
+                      wpi::math::Pose2d{-3_m, 0_m, -90_deg},
+                      wpi::math::Pose2d{0_m, 0_m, 45_deg}},
+          wpi::math::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
 
   std::default_random_engine generator;
   std::normal_distribution<double> distribution(0.0, 1.0);
@@ -185,7 +191,8 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) {
     wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
 
     auto xhat = odometry.Update(
-        wpi::math::Rotation2d{distribution(generator) * 0.05_rad}, wheelPositions);
+        wpi::math::Rotation2d{distribution(generator) * 0.05_rad},
+        wheelPositions);
     double error = groundTruthState.pose.Translation()
                        .Distance(xhat.Translation())
                        .value();

wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelSpeedsTest.cpp

@@ -7,8 +7,10 @@
 #include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
 
 TEST(MecanumDriveWheelSpeedsTest, Plus) {
-  const wpi::math::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
-  const wpi::math::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps, 1.0_mps};
+  const wpi::math::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps,
+                                                1.5_mps};
+  const wpi::math::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps,
+                                                 1.0_mps};
 
   const wpi::math::MecanumDriveWheelSpeeds result = left + right;
 
@@ -19,8 +21,10 @@ TEST(MecanumDriveWheelSpeedsTest, Plus) {
 }
 
 TEST(MecanumDriveWheelSpeedsTest, Minus) {
-  const wpi::math::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
-  const wpi::math::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps, 1.0_mps};
+  const wpi::math::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps,
+                                                1.5_mps};
+  const wpi::math::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps,
+                                                 1.0_mps};
 
   const wpi::math::MecanumDriveWheelSpeeds result = left - right;
 
@@ -31,7 +35,8 @@ TEST(MecanumDriveWheelSpeedsTest, Minus) {
 }
 
 TEST(MecanumDriveWheelSpeedsTest, UnaryMinus) {
-  const wpi::math::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+  const wpi::math::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps,
+                                                  1.5_mps};
 
   const wpi::math::MecanumDriveWheelSpeeds result = -speeds;
 
@@ -42,7 +47,8 @@ TEST(MecanumDriveWheelSpeedsTest, UnaryMinus) {
 }
 
 TEST(MecanumDriveWheelSpeedsTest, Multiplication) {
-  const wpi::math::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+  const wpi::math::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps,
+                                                  1.5_mps};
 
   const wpi::math::MecanumDriveWheelSpeeds result = speeds * 2;
 
@@ -53,7 +59,8 @@ TEST(MecanumDriveWheelSpeedsTest, Multiplication) {
 }
 
 TEST(MecanumDriveWheelSpeedsTest, Division) {
-  const wpi::math::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+  const wpi::math::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps,
+                                                  1.5_mps};
 
   const wpi::math::MecanumDriveWheelSpeeds result = speeds / 2;
 

wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp

@@ -35,7 +35,8 @@ TEST_F(SwerveDriveOdometry3dTest, Initialize) {
       m_kinematics,
       wpi::math::Rotation3d{},
       {zero, zero, zero, zero},
-      wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 45_deg}}};
+      wpi::math::Pose3d{1_m, 2_m, 0_m,
+                        wpi::math::Rotation3d{0_deg, 0_deg, 45_deg}}};
 
   const wpi::math::Pose3d& pose = odometry.GetPose();
 
@@ -140,8 +141,9 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingTrajectory) {
     br.angle = moduleStates[3].angle;
 
     auto xhat = odometry.Update(
-        wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
-                        wpi::math::Rotation2d{distribution(generator) * 0.05_rad}},
+        wpi::math::Rotation3d{
+            groundTruthState.pose.Rotation() +
+            wpi::math::Rotation2d{distribution(generator) * 0.05_rad}},
         {fl, fr, bl, br});
     double error = groundTruthState.pose.Translation()
                        .Distance(xhat.Translation().ToTranslation2d())

wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp

@@ -117,10 +117,10 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
     bl.angle = moduleStates[2].angle;
     br.angle = moduleStates[3].angle;
 
-    auto xhat =
-        odometry.Update(groundTruthState.pose.Rotation() +
-                            wpi::math::Rotation2d{distribution(generator) * 0.05_rad},
-                        {fl, fr, bl, br});
+    auto xhat = odometry.Update(
+        groundTruthState.pose.Rotation() +
+            wpi::math::Rotation2d{distribution(generator) * 0.05_rad},
+        {fl, fr, bl, br});
     double error = groundTruthState.pose.Translation()
                        .Distance(xhat.Translation())
                        .value();
@@ -183,7 +183,8 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) {
     br.angle = groundTruthState.pose.Rotation();
 
     auto xhat = odometry.Update(
-        wpi::math::Rotation2d{distribution(generator) * 0.05_rad}, {fl, fr, bl, br});
+        wpi::math::Rotation2d{distribution(generator) * 0.05_rad},
+        {fl, fr, bl, br});
     double error = groundTruthState.pose.Translation()
                        .Distance(xhat.Translation())
                        .value();

wpimath/src/test/native/cpp/kinematics/proto/SwerveDriveKinematicsProtoTest.cpp

@@ -13,9 +13,10 @@ using namespace wpi::math;
 struct SwerveDriveKinematicsProtoTestData {
   using Type = SwerveDriveKinematics<4>;
 
-  inline static const Type kTestData{
-      wpi::math::Translation2d{1.0_m, 0.9_m}, wpi::math::Translation2d{1.1_m, -0.8_m},
-      wpi::math::Translation2d{-1.2_m, 0.7_m}, wpi::math::Translation2d{-1.3_m, -0.6_m}};
+  inline static const Type kTestData{wpi::math::Translation2d{1.0_m, 0.9_m},
+                                     wpi::math::Translation2d{1.1_m, -0.8_m},
+                                     wpi::math::Translation2d{-1.2_m, 0.7_m},
+                                     wpi::math::Translation2d{-1.3_m, -0.6_m}};
 
   static void CheckEq(const Type& testData, const Type& data) {
     EXPECT_EQ(testData.GetModules(), data.GetModules());

wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStructTest.cpp

@@ -10,7 +10,8 @@ using namespace wpi::math;
 
 namespace {
 
-using StructType = wpi::util::Struct<wpi::math::DifferentialDriveWheelPositions>;
+using StructType =
+    wpi::util::Struct<wpi::math::DifferentialDriveWheelPositions>;
 const DifferentialDriveWheelPositions kExpectedData{
     DifferentialDriveWheelPositions{1.74_m, 35.04_m}};
 }  // namespace

wpimath/src/test/native/cpp/kinematics/struct/SwerveDriveKinematicsStructTest.cpp

@@ -13,9 +13,10 @@ using namespace wpi::math;
 struct SwerveDriveKinematicsStructTestData {
   using Type = SwerveDriveKinematics<4>;
 
-  inline static const Type kTestData{
-      wpi::math::Translation2d{1.0_m, 0.9_m}, wpi::math::Translation2d{1.1_m, -0.8_m},
-      wpi::math::Translation2d{-1.2_m, 0.7_m}, wpi::math::Translation2d{-1.3_m, -0.6_m}};
+  inline static const Type kTestData{wpi::math::Translation2d{1.0_m, 0.9_m},
+                                     wpi::math::Translation2d{1.1_m, -0.8_m},
+                                     wpi::math::Translation2d{-1.2_m, 0.7_m},
+                                     wpi::math::Translation2d{-1.3_m, -0.6_m}};
 
   static void CheckEq(const Type& testData, const Type& data) {
     EXPECT_EQ(testData.GetModules(), data.GetModules());

wpimath/src/test/native/cpp/path/TravelingSalesmanTest.cpp

@@ -26,7 +26,8 @@ bool IsMatchingCycle(std::span<const wpi::math::Pose2d> expected,
   assert(expected.size() == actual.size());
 
   // Check actual has expected cycle (forward)
-  wpi::util::circular_buffer<wpi::math::Pose2d> actualBufferForward{expected.size()};
+  wpi::util::circular_buffer<wpi::math::Pose2d> actualBufferForward{
+      expected.size()};
   for (size_t i = 0; i < actual.size(); ++i) {
     actualBufferForward.push_back(actual[i % actual.size()]);
   }
@@ -36,7 +37,8 @@ bool IsMatchingCycle(std::span<const wpi::math::Pose2d> expected,
   }
 
   // Check actual has expected cycle (reverse)
-  wpi::util::circular_buffer<wpi::math::Pose2d> actualBufferReverse{expected.size()};
+  wpi::util::circular_buffer<wpi::math::Pose2d> actualBufferReverse{
+      expected.size()};
   for (size_t i = 0; i < actual.size(); ++i) {
     actualBufferReverse.push_front(actual[(1 + i) % actual.size()]);
   }
@@ -64,8 +66,8 @@ TEST(TravelingSalesmanTest, FiveLengthStaticPathWithDistanceCost) {
   wpi::math::TravelingSalesman traveler;
   wpi::util::array<wpi::math::Pose2d, 5> solution = traveler.Solve(poses, 500);
 
-  wpi::util::array<wpi::math::Pose2d, 5> expected{poses[0], poses[2], poses[4], poses[1],
-                                      poses[3]};
+  wpi::util::array<wpi::math::Pose2d, 5> expected{poses[0], poses[2], poses[4],
+                                                  poses[1], poses[3]};
 
   EXPECT_TRUE(IsMatchingCycle(expected, solution));
 }
@@ -87,8 +89,8 @@ TEST(TravelingSalesmanTest, FiveLengthDynamicPathWithDistanceCost) {
       traveler.Solve(std::span<const wpi::math::Pose2d>{poses}, 500);
 
   ASSERT_EQ(5u, solution.size());
-  wpi::util::array<wpi::math::Pose2d, 5> expected{poses[0], poses[2], poses[4], poses[1],
-                                      poses[3]};
+  wpi::util::array<wpi::math::Pose2d, 5> expected{poses[0], poses[2], poses[4],
+                                                  poses[1], poses[3]};
 
   EXPECT_TRUE(IsMatchingCycle(expected, solution));
 }
@@ -110,9 +112,9 @@ TEST(TravelingSalesmanTest, TenLengthStaticPathWithDistanceCost) {
   wpi::math::TravelingSalesman traveler;
   wpi::util::array<wpi::math::Pose2d, 10> solution = traveler.Solve(poses, 500);
 
-  wpi::util::array<wpi::math::Pose2d, 10> expected{poses[0], poses[4], poses[6], poses[3],
-                                       poses[1], poses[2], poses[9], poses[8],
-                                       poses[5], poses[7]};
+  wpi::util::array<wpi::math::Pose2d, 10> expected{
+      poses[0], poses[4], poses[6], poses[3], poses[1],
+      poses[2], poses[9], poses[8], poses[5], poses[7]};
 
   EXPECT_TRUE(IsMatchingCycle(expected, solution));
 }
@@ -136,9 +138,9 @@ TEST(TravelingSalesmanTest, TenLengthDynamicPathWithDistanceCost) {
       traveler.Solve(std::span<const wpi::math::Pose2d>{poses}, 500);
 
   ASSERT_EQ(10u, solution.size());
-  wpi::util::array<wpi::math::Pose2d, 10> expected{poses[0], poses[4], poses[6], poses[3],
-                                       poses[1], poses[2], poses[9], poses[8],
-                                       poses[5], poses[7]};
+  wpi::util::array<wpi::math::Pose2d, 10> expected{
+      poses[0], poses[4], poses[6], poses[3], poses[1],
+      poses[2], poses[9], poses[8], poses[5], poses[7]};
 
   EXPECT_TRUE(IsMatchingCycle(expected, solution));
 }

wpimath/src/test/native/cpp/spline/proto/CubicHermiteSplineProtoTest.cpp

@@ -12,9 +12,10 @@ using namespace wpi::math;
 struct CubicHermiteSplineProtoTestData {
   using Type = CubicHermiteSpline;
 
-  inline static const Type kTestData{
-      wpi::util::array<double, 2>{{0.1, 0.2}}, wpi::util::array<double, 2>{{0.3, 0.4}},
-      wpi::util::array<double, 2>{{0.5, 0.6}}, wpi::util::array<double, 2>{{0.7, 0.8}}};
+  inline static const Type kTestData{wpi::util::array<double, 2>{{0.1, 0.2}},
+                                     wpi::util::array<double, 2>{{0.3, 0.4}},
+                                     wpi::util::array<double, 2>{{0.5, 0.6}},
+                                     wpi::util::array<double, 2>{{0.7, 0.8}}};
 
   static void CheckEq(const Type& testData, const Type& data) {
     EXPECT_EQ(testData.GetInitialControlVector().x,

wpimath/src/test/native/cpp/spline/proto/QuinticHermiteSplineProtoTest.cpp

@@ -12,10 +12,11 @@ using namespace wpi::math;
 struct QuinticHermiteSplineProtoTestData {
   using Type = QuinticHermiteSpline;
 
-  inline static const Type kTestData{wpi::util::array<double, 3>{{0.01, 0.02, 0.03}},
-                                     wpi::util::array<double, 3>{{0.04, 0.05, 0.06}},
-                                     wpi::util::array<double, 3>{{0.07, 0.08, 0.09}},
-                                     wpi::util::array<double, 3>{{0.10, 0.11, 0.11}}};
+  inline static const Type kTestData{
+      wpi::util::array<double, 3>{{0.01, 0.02, 0.03}},
+      wpi::util::array<double, 3>{{0.04, 0.05, 0.06}},
+      wpi::util::array<double, 3>{{0.07, 0.08, 0.09}},
+      wpi::util::array<double, 3>{{0.10, 0.11, 0.11}}};
 
   static void CheckEq(const Type& testData, const Type& data) {
     EXPECT_EQ(testData.GetInitialControlVector().x,

wpimath/src/test/native/cpp/spline/struct/CubicHermiteSplineStructTest.cpp

@@ -12,9 +12,10 @@ using namespace wpi::math;
 struct CubicHermiteSplineStructTestData {
   using Type = CubicHermiteSpline;
 
-  inline static const Type kTestData{
-      wpi::util::array<double, 2>{{0.1, 0.2}}, wpi::util::array<double, 2>{{0.3, 0.4}},
-      wpi::util::array<double, 2>{{0.5, 0.6}}, wpi::util::array<double, 2>{{0.7, 0.8}}};
+  inline static const Type kTestData{wpi::util::array<double, 2>{{0.1, 0.2}},
+                                     wpi::util::array<double, 2>{{0.3, 0.4}},
+                                     wpi::util::array<double, 2>{{0.5, 0.6}},
+                                     wpi::util::array<double, 2>{{0.7, 0.8}}};
 
   static void CheckEq(const Type& testData, const Type& data) {
     EXPECT_EQ(testData.GetInitialControlVector().x,

wpimath/src/test/native/cpp/spline/struct/QuinticHermiteSplineStructTest.cpp

@@ -12,10 +12,11 @@ using namespace wpi::math;
 struct QuinticHermiteSplineStructTestData {
   using Type = QuinticHermiteSpline;
 
-  inline static const Type kTestData{wpi::util::array<double, 3>{{0.01, 0.02, 0.03}},
-                                     wpi::util::array<double, 3>{{0.04, 0.05, 0.06}},
-                                     wpi::util::array<double, 3>{{0.07, 0.08, 0.09}},
-                                     wpi::util::array<double, 3>{{0.10, 0.11, 0.11}}};
+  inline static const Type kTestData{
+      wpi::util::array<double, 3>{{0.01, 0.02, 0.03}},
+      wpi::util::array<double, 3>{{0.04, 0.05, 0.06}},
+      wpi::util::array<double, 3>{{0.07, 0.08, 0.09}},
+      wpi::util::array<double, 3>{{0.10, 0.11, 0.11}}};
 
   static void CheckEq(const Type& testData, const Type& data) {
     EXPECT_EQ(testData.GetInitialControlVector().x,

wpimath/src/test/native/cpp/system/DiscretizationTest.cpp

@@ -23,7 +23,8 @@ TEST(DiscretizationTest, DiscretizeA) {
   wpi::math::Vectord<2> x1Discrete = discA * x0;
 
   // We now have pos = vel = 1 and accel = 0, which should give us:
-  wpi::math::Vectord<2> x1Truth{1.0 * x0(0) + 1.0 * x0(1), 0.0 * x0(0) + 1.0 * x0(1)};
+  wpi::math::Vectord<2> x1Truth{1.0 * x0(0) + 1.0 * x0(1),
+                                0.0 * x0(0) + 1.0 * x0(1)};
 
   EXPECT_EQ(x1Truth, x1Discrete);
 }
@@ -44,7 +45,7 @@ TEST(DiscretizationTest, DiscretizeAB) {
 
   // We now have pos = vel = accel = 1, which should give us:
   wpi::math::Vectord<2> x1Truth{1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0),
-                          0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0)};
+                                0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0)};
 
   EXPECT_EQ(x1Truth, x1Discrete);
 }
@@ -61,15 +62,15 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
   //       T
   // Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
   //       0
-  wpi::math::Matrixd<2, 2> discQIntegrated =
-      wpi::math::RKDP<std::function<wpi::math::Matrixd<2, 2>(wpi::units::second_t,
-                                                 const wpi::math::Matrixd<2, 2>&)>,
-                wpi::math::Matrixd<2, 2>>(
-          [&](wpi::units::second_t t, const wpi::math::Matrixd<2, 2>&) {
-            return wpi::math::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
-                                      (contA.transpose() * t.value()).exp());
-          },
-          0_s, wpi::math::Matrixd<2, 2>::Zero(), dt);
+  wpi::math::Matrixd<2, 2> discQIntegrated = wpi::math::RKDP<
+      std::function<wpi::math::Matrixd<2, 2>(wpi::units::second_t,
+                                             const wpi::math::Matrixd<2, 2>&)>,
+      wpi::math::Matrixd<2, 2>>(
+      [&](wpi::units::second_t t, const wpi::math::Matrixd<2, 2>&) {
+        return wpi::math::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
+                                        (contA.transpose() * t.value()).exp());
+      },
+      0_s, wpi::math::Matrixd<2, 2>::Zero(), dt);
 
   wpi::math::Matrixd<2, 2> discA;
   wpi::math::Matrixd<2, 2> discQ;
@@ -93,15 +94,15 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
   //       T
   // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
   //       0
-  wpi::math::Matrixd<2, 2> discQIntegrated =
-      wpi::math::RKDP<std::function<wpi::math::Matrixd<2, 2>(wpi::units::second_t,
-                                                 const wpi::math::Matrixd<2, 2>&)>,
-                wpi::math::Matrixd<2, 2>>(
-          [&](wpi::units::second_t t, const wpi::math::Matrixd<2, 2>&) {
-            return wpi::math::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
-                                      (contA.transpose() * t.value()).exp());
-          },
-          0_s, wpi::math::Matrixd<2, 2>::Zero(), dt);
+  wpi::math::Matrixd<2, 2> discQIntegrated = wpi::math::RKDP<
+      std::function<wpi::math::Matrixd<2, 2>(wpi::units::second_t,
+                                             const wpi::math::Matrixd<2, 2>&)>,
+      wpi::math::Matrixd<2, 2>>(
+      [&](wpi::units::second_t t, const wpi::math::Matrixd<2, 2>&) {
+        return wpi::math::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
+                                        (contA.transpose() * t.value()).exp());
+      },
+      0_s, wpi::math::Matrixd<2, 2>::Zero(), dt);
 
   wpi::math::Matrixd<2, 2> discA;
   wpi::math::Matrixd<2, 2> discQ;

wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp

@@ -20,18 +20,19 @@ TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) {
 #endif
 
   ASSERT_TRUE(model.A().isApprox(
-      wpi::math::Matrixd<2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}}, 0.001));
+      wpi::math::Matrixd<2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}},
+      0.001));
   ASSERT_TRUE(model.B().isApprox(
       wpi::math::Matrixd<2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}}, 0.001));
-  ASSERT_TRUE(
-      model.C().isApprox(wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
-  ASSERT_TRUE(
-      model.D().isApprox(wpi::math::Matrixd<2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
+  ASSERT_TRUE(model.C().isApprox(
+      wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
+  ASSERT_TRUE(model.D().isApprox(
+      wpi::math::Matrixd<2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
 }
 
 TEST(LinearSystemIDTest, ElevatorSystem) {
-  auto model = wpi::math::LinearSystemId::ElevatorSystem(wpi::math::DCMotor::NEO(2), 5_kg,
-                                                   0.05_m, 12)
+  auto model = wpi::math::LinearSystemId::ElevatorSystem(
+                   wpi::math::DCMotor::NEO(2), 5_kg, 0.05_m, 12)
                    .Slice(0);
   ASSERT_TRUE(model.A().isApprox(
       wpi::math::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
@@ -42,8 +43,8 @@ TEST(LinearSystemIDTest, ElevatorSystem) {
 
 TEST(LinearSystemIDTest, FlywheelSystem) {
 #if __GNUC__ <= 11
-  auto model = wpi::math::LinearSystemId::FlywheelSystem(wpi::math::DCMotor::NEO(2),
-                                                   0.00032_kg_sq_m, 1.0);
+  auto model = wpi::math::LinearSystemId::FlywheelSystem(
+      wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
 #else
   constexpr auto model = wpi::math::LinearSystemId::FlywheelSystem(
       wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
@@ -57,18 +58,19 @@ TEST(LinearSystemIDTest, FlywheelSystem) {
 
 TEST(LinearSystemIDTest, DCMotorSystem) {
 #if __GNUC__ <= 11
-  auto model = wpi::math::LinearSystemId::DCMotorSystem(wpi::math::DCMotor::NEO(2),
-                                                  0.00032_kg_sq_m, 1.0);
+  auto model = wpi::math::LinearSystemId::DCMotorSystem(
+      wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
 #else
   constexpr auto model = wpi::math::LinearSystemId::DCMotorSystem(
       wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
 #endif
 
+  ASSERT_TRUE(model.A().isApprox(
+      wpi::math::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
   ASSERT_TRUE(
-      model.A().isApprox(wpi::math::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
-  ASSERT_TRUE(model.B().isApprox(wpi::math::Matrixd<2, 1>{0, 1354.166667}, 0.001));
-  ASSERT_TRUE(
-      model.C().isApprox(wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
+      model.B().isApprox(wpi::math::Matrixd<2, 1>{0, 1354.166667}, 0.001));
+  ASSERT_TRUE(model.C().isApprox(
+      wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
   ASSERT_TRUE(model.D().isApprox(wpi::math::Matrixd<2, 1>{0.0, 0.0}, 0.001));
 }
 
@@ -79,8 +81,9 @@ TEST(LinearSystemIDTest, IdentifyPositionSystem) {
   constexpr double ka = 0.5;
 
 #if __GNUC__ <= 11
-  auto model = wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::meter>(
-      kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
+  auto model =
+      wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::meter>(
+          kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
 #else
   constexpr auto model =
       wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::meter>(
@@ -89,7 +92,8 @@ TEST(LinearSystemIDTest, IdentifyPositionSystem) {
 
   ASSERT_TRUE(model.A().isApprox(
       wpi::math::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
-  ASSERT_TRUE(model.B().isApprox(wpi::math::Matrixd<2, 1>{0.0, 1.0 / ka}, 0.001));
+  ASSERT_TRUE(
+      model.B().isApprox(wpi::math::Matrixd<2, 1>{0.0, 1.0 / ka}, 0.001));
 }
 
 TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
@@ -100,8 +104,9 @@ TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
   constexpr double ka = 0.5;
 
 #if __GNUC__ <= 11
-  auto model = wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meter>(
-      kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
+  auto model =
+      wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meter>(
+          kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
 #else
   constexpr auto model =
       wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meter>(

wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp

@@ -14,7 +14,9 @@ TEST(NumericalIntegrationTest, Exponential) {
   wpi::math::Vectord<1> y0{0.0};
 
   wpi::math::Vectord<1> y1 = wpi::math::RK4(
-      [](const wpi::math::Vectord<1>& x) { return wpi::math::Vectord<1>{std::exp(x(0))}; },
+      [](const wpi::math::Vectord<1>& x) {
+        return wpi::math::Vectord<1>{std::exp(x(0))};
+      },
       y0, 0.1_s);
   EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
 }
@@ -40,12 +42,13 @@ TEST(NumericalIntegrationTest, ExponentialWithU) {
 //
 //   x(t) = 12eᵗ/(eᵗ + 1)²
 TEST(NumericalIntegrationTest, RK4TimeVarying) {
-  wpi::math::Vectord<1> y0{12.0 * std::exp(5.0) / std::pow(std::exp(5.0) + 1.0, 2.0)};
+  wpi::math::Vectord<1> y0{12.0 * std::exp(5.0) /
+                           std::pow(std::exp(5.0) + 1.0, 2.0)};
 
   wpi::math::Vectord<1> y1 = wpi::math::RK4(
       [](wpi::units::second_t t, const wpi::math::Vectord<1>& x) {
         return wpi::math::Vectord<1>{x(0) *
-                               (2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
+                                     (2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
       },
       5_s, y0, 1_s);
   EXPECT_NEAR(y1(0), 12.0 * std::exp(6.0) / std::pow(std::exp(6.0) + 1.0, 2.0),
@@ -83,12 +86,13 @@ TEST(NumericalIntegrationTest, ExponentialRKDP) {
 //
 //   x(t) = 12eᵗ/(eᵗ + 1)²
 TEST(NumericalIntegrationTest, RKDPTimeVarying) {
-  wpi::math::Vectord<1> y0{12.0 * std::exp(5.0) / std::pow(std::exp(5.0) + 1.0, 2.0)};
+  wpi::math::Vectord<1> y0{12.0 * std::exp(5.0) /
+                           std::pow(std::exp(5.0) + 1.0, 2.0)};
 
   wpi::math::Vectord<1> y1 = wpi::math::RKDP(
       [](wpi::units::second_t t, const wpi::math::Vectord<1>& x) {
         return wpi::math::Vectord<1>{x(0) *
-                               (2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
+                                     (2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
       },
       5_s, y0, 1_s, 1e-12);
   EXPECT_NEAR(y1(0), 12.0 * std::exp(6.0) / std::pow(std::exp(6.0) + 1.0, 2.0),

wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp

@@ -6,11 +6,13 @@
 
 #include "wpi/math/system/NumericalJacobian.hpp"
 
-wpi::math::Matrixd<4, 4> A{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}};
+wpi::math::Matrixd<4, 4> A{
+    {1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}};
 wpi::math::Matrixd<4, 2> B{{1, 1}, {2, 1}, {3, 2}, {3, 7}};
 
 // Function from which to recover A and B
-wpi::math::Vectord<4> AxBuFn(const wpi::math::Vectord<4>& x, const wpi::math::Vectord<2>& u) {
+wpi::math::Vectord<4> AxBuFn(const wpi::math::Vectord<4>& x,
+                             const wpi::math::Vectord<2>& u) {
   return A * x + B * u;
 }
 
@@ -36,14 +38,16 @@ Eigen::VectorXd AxBuFn_DynamicSize(const Eigen::VectorXd& x,
 // Test that we can recover A from AxBuFn() pretty accurately
 TEST(NumericalJacobianTest, Ax_DynamicSize) {
   Eigen::MatrixXd newA = wpi::math::NumericalJacobianX(
-      AxBuFn_DynamicSize, wpi::math::Vectord<4>::Zero(), wpi::math::Vectord<2>::Zero());
+      AxBuFn_DynamicSize, wpi::math::Vectord<4>::Zero(),
+      wpi::math::Vectord<2>::Zero());
   EXPECT_TRUE(newA.isApprox(A));
 }
 
 // Test that we can recover B from AxBuFn() pretty accurately
 TEST(NumericalJacobianTest, Bu_DynamicSize) {
   Eigen::MatrixXd newB = wpi::math::NumericalJacobianU(
-      AxBuFn_DynamicSize, wpi::math::Vectord<4>::Zero(), wpi::math::Vectord<2>::Zero());
+      AxBuFn_DynamicSize, wpi::math::Vectord<4>::Zero(),
+      wpi::math::Vectord<2>::Zero());
   EXPECT_TRUE(newB.isApprox(B));
 }
 
@@ -51,7 +55,8 @@ wpi::math::Matrixd<3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}};
 wpi::math::Matrixd<3, 2> D{{1, 1}, {2, 1}, {3, 2}};
 
 // Function from which to recover C and D
-wpi::math::Vectord<3> CxDuFn(const wpi::math::Vectord<4>& x, const wpi::math::Vectord<2>& u) {
+wpi::math::Vectord<3> CxDuFn(const wpi::math::Vectord<4>& x,
+                             const wpi::math::Vectord<2>& u) {
   return C * x + D * u;
 }
 

wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp

@@ -23,7 +23,7 @@ using namespace wpi::math;
 TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
   // Pick an unreasonably large kA to ensure the constraint has to do some work
   SimpleMotorFeedforward<wpi::units::meter> feedforward{1_V, 1_V / 1_mps,
-                                                   3_V / 1_mps_sq};
+                                                        3_V / 1_mps_sq};
   const DifferentialDriveKinematics kinematics{0.5_m};
   const auto maxVoltage = 10_V;
 
@@ -64,7 +64,7 @@ TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
 
 TEST(DifferentialDriveVoltageConstraintTest, HighCurvature) {
   SimpleMotorFeedforward<wpi::units::meter> feedforward{1_V, 1_V / 1_mps,
-                                                   3_V / 1_mps_sq};
+                                                        3_V / 1_mps_sq};
   // Large trackwidth - need to test with radius of curvature less than half of
   // trackwidth
   const DifferentialDriveKinematics kinematics{3_m};

wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp

@@ -26,8 +26,10 @@ TEST(EllipticalRegionConstraintTest, Constraint) {
   bool exceededConstraintOutsideRegion = false;
   for (auto& point : trajectory.States()) {
     if (ellipse.Contains(point.pose.Translation())) {
-      EXPECT_TRUE(wpi::units::math::abs(point.velocity) < maxVelocity + 0.05_mps);
-    } else if (wpi::units::math::abs(point.velocity) >= maxVelocity + 0.05_mps) {
+      EXPECT_TRUE(wpi::units::math::abs(point.velocity) <
+                  maxVelocity + 0.05_mps);
+    } else if (wpi::units::math::abs(point.velocity) >=
+               maxVelocity + 0.05_mps) {
       exceededConstraintOutsideRegion = true;
     }
   }

wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp

@@ -33,13 +33,16 @@ static constexpr auto kA = 0.43277_V / 1_mps_sq;
     EXPECT_NEAR_UNITS(val1, val2, eps);          \
   }
 
-wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State CheckDynamics(
+wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+CheckDynamics(
     wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile,
-    wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints
-        constraints,
+    wpi::math::ExponentialProfile<wpi::units::meter,
+                                  wpi::units::volts>::Constraints constraints,
     wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward,
-    wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State current,
-    wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal) {
+    wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+        current,
+    wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+        goal) {
   auto next = profile.Calculate(kDt, current, goal);
   auto signal = feedforward.Calculate(current.velocity, next.velocity);
 
@@ -49,13 +52,17 @@ wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State Check
 }
 
 TEST(ExponentialProfileTest, ReachesGoal) {
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
+  wpi::math::ExponentialProfile<wpi::units::meter,
+                                wpi::units::volts>::Constraints constraints{
       12_V, -kV / kA, 1 / kA};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
+      constraints};
   wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
       0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{10_m, 0_mps};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      goal{10_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      state{0_m, 0_mps};
 
   for (int i = 0; i < 450; ++i) {
     state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -66,19 +73,24 @@ TEST(ExponentialProfileTest, ReachesGoal) {
 // Tests that decreasing the maximum velocity in the middle when it is already
 // moving faster than the new max is handled correctly
 TEST(ExponentialProfileTest, PosContinuousUnderVelChange) {
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
+  wpi::math::ExponentialProfile<wpi::units::meter,
+                                wpi::units::volts>::Constraints constraints{
       12_V, -kV / kA, 1 / kA};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
+      constraints};
   wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
       0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{10_m, 0_mps};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      goal{10_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      state{0_m, 0_mps};
 
   for (int i = 0; i < 300; ++i) {
     if (i == 150) {
       constraints.maxInput = 9_V;
       profile =
-          wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>{constraints};
+          wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>{
+              constraints};
     }
 
     state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -89,19 +101,24 @@ TEST(ExponentialProfileTest, PosContinuousUnderVelChange) {
 // Tests that decreasing the maximum velocity in the middle when it is already
 // moving faster than the new max is handled correctly
 TEST(ExponentialProfileTest, PosContinuousUnderVelChangeBackward) {
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
+  wpi::math::ExponentialProfile<wpi::units::meter,
+                                wpi::units::volts>::Constraints constraints{
       12_V, -kV / kA, 1 / kA};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
+      constraints};
   wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
       0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{-10_m, 0_mps};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      goal{-10_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      state{0_m, 0_mps};
 
   for (int i = 0; i < 300; ++i) {
     if (i == 150) {
       constraints.maxInput = 9_V;
       profile =
-          wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>{constraints};
+          wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>{
+              constraints};
     }
 
     state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -111,13 +128,17 @@ TEST(ExponentialProfileTest, PosContinuousUnderVelChangeBackward) {
 
 // There is some somewhat tricky code for dealing with going backwards
 TEST(ExponentialProfileTest, Backwards) {
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
+  wpi::math::ExponentialProfile<wpi::units::meter,
+                                wpi::units::volts>::Constraints constraints{
       12_V, -kV / kA, 1 / kA};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
+      constraints};
   wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
       0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{-10_m, 0_mps};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state;
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      goal{-10_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      state;
 
   for (int i = 0; i < 400; ++i) {
     state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -126,13 +147,17 @@ TEST(ExponentialProfileTest, Backwards) {
 }
 
 TEST(ExponentialProfileTest, SwitchGoalInMiddle) {
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
+  wpi::math::ExponentialProfile<wpi::units::meter,
+                                wpi::units::volts>::Constraints constraints{
       12_V, -kV / kA, 1 / kA};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
+      constraints};
   wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
       0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{-10_m, 0_mps};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      goal{-10_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      state{0_m, 0_mps};
 
   for (int i = 0; i < 50; ++i) {
     state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -148,13 +173,17 @@ TEST(ExponentialProfileTest, SwitchGoalInMiddle) {
 
 // Checks to make sure that it hits top speed on long trajectories
 TEST(ExponentialProfileTest, TopSpeed) {
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
+  wpi::math::ExponentialProfile<wpi::units::meter,
+                                wpi::units::volts>::Constraints constraints{
       12_V, -kV / kA, 1 / kA};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
+      constraints};
   wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
       0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{40_m, 0_mps};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state;
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      goal{40_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      state;
 
   wpi::units::meters_per_second_t maxSpeed = 0_mps;
 
@@ -169,13 +198,17 @@ TEST(ExponentialProfileTest, TopSpeed) {
 
 // Checks to make sure that it hits top speed on long trajectories
 TEST(ExponentialProfileTest, TopSpeedBackward) {
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
+  wpi::math::ExponentialProfile<wpi::units::meter,
+                                wpi::units::volts>::Constraints constraints{
       12_V, -kV / kA, 1 / kA};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
+      constraints};
   wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
       0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{-40_m, 0_mps};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state;
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      goal{-40_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      state;
 
   wpi::units::meters_per_second_t maxSpeed = 0_mps;
 
@@ -190,13 +223,17 @@ TEST(ExponentialProfileTest, TopSpeedBackward) {
 
 // Checks to make sure that it hits top speed on long trajectories
 TEST(ExponentialProfileTest, HighInitialSpeed) {
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
+  wpi::math::ExponentialProfile<wpi::units::meter,
+                                wpi::units::volts>::Constraints constraints{
       12_V, -kV / kA, 1 / kA};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
+      constraints};
   wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
       0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{40_m, 0_mps};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 8_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      goal{40_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      state{0_m, 8_mps};
 
   for (int i = 0; i < 900; ++i) {
     state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -207,13 +244,17 @@ TEST(ExponentialProfileTest, HighInitialSpeed) {
 
 // Checks to make sure that it hits top speed on long trajectories
 TEST(ExponentialProfileTest, HighInitialSpeedBackward) {
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
+  wpi::math::ExponentialProfile<wpi::units::meter,
+                                wpi::units::volts>::Constraints constraints{
       12_V, -kV / kA, 1 / kA};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
+      constraints};
   wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
       0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{-40_m, 0_mps};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, -8_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      goal{-40_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      state{0_m, -8_mps};
 
   for (int i = 0; i < 900; ++i) {
     state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -223,14 +264,19 @@ TEST(ExponentialProfileTest, HighInitialSpeedBackward) {
 }
 
 TEST(ExponentialProfileTest, TestHeuristic) {
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
+  wpi::math::ExponentialProfile<wpi::units::meter,
+                                wpi::units::volts>::Constraints constraints{
       12_V, -kV / kA, 1 / kA};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
+      constraints};
   std::vector<std::tuple<
-      wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State,  // initial
-      wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State,  // goal
-      wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State>  // inflection
-                                                                   // point
+      wpi::math::ExponentialProfile<wpi::units::meter,
+                                    wpi::units::volts>::State,  // initial
+      wpi::math::ExponentialProfile<wpi::units::meter,
+                                    wpi::units::volts>::State,  // goal
+      wpi::math::ExponentialProfile<wpi::units::meter,
+                                    wpi::units::volts>::State>  // inflection
+                                                                // point
               >
       testCases{
           // red > green and purple => always positive => false
@@ -275,13 +321,17 @@ TEST(ExponentialProfileTest, TestHeuristic) {
 }
 
 TEST(ExponentialProfileTest, TimingToCurrent) {
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
+  wpi::math::ExponentialProfile<wpi::units::meter,
+                                wpi::units::volts>::Constraints constraints{
       12_V, -kV / kA, 1 / kA};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
+      constraints};
   wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
       0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{2_m, 0_mps};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      goal{2_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      state{0_m, 0_mps};
 
   for (int i = 0; i < 900; ++i) {
     state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -292,13 +342,17 @@ TEST(ExponentialProfileTest, TimingToCurrent) {
 }
 
 TEST(ExponentialProfileTest, TimingToGoal) {
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
+  wpi::math::ExponentialProfile<wpi::units::meter,
+                                wpi::units::volts>::Constraints constraints{
       12_V, -kV / kA, 1 / kA};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
+      constraints};
   wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
       0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{2_m, 0_mps};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      goal{2_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      state{0_m, 0_mps};
 
   auto prediction = profile.TimeLeftUntil(state, goal);
   auto reachedGoal = false;
@@ -315,13 +369,17 @@ TEST(ExponentialProfileTest, TimingToGoal) {
 }
 
 TEST(ExponentialProfileTest, TimingToNegativeGoal) {
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
+  wpi::math::ExponentialProfile<wpi::units::meter,
+                                wpi::units::volts>::Constraints constraints{
       12_V, -kV / kA, 1 / kA};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
+      constraints};
   wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
       0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{-2_m, 0_mps};
-  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      goal{-2_m, 0_mps};
+  wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
+      state{0_m, 0_mps};
 
   auto prediction = profile.TimeLeftUntil(state, goal);
   auto reachedGoal = false;

wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp

@@ -26,8 +26,10 @@ TEST(RectangularRegionConstraintTest, Constraint) {
   bool exceededConstraintOutsideRegion = false;
   for (auto& point : trajectory.States()) {
     if (rectangle.Contains(point.pose.Translation())) {
-      EXPECT_TRUE(wpi::units::math::abs(point.velocity) < maxVelocity + 0.05_mps);
-    } else if (wpi::units::math::abs(point.velocity) >= maxVelocity + 0.05_mps) {
+      EXPECT_TRUE(wpi::units::math::abs(point.velocity) <
+                  maxVelocity + 0.05_mps);
+    } else if (wpi::units::math::abs(point.velocity) >=
+               maxVelocity + 0.05_mps) {
       exceededConstraintOutsideRegion = true;
     }
   }

wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp

@@ -10,8 +10,9 @@
 #include "wpi/math/trajectory/TrajectoryConfig.hpp"
 #include "wpi/math/trajectory/TrajectoryGenerator.hpp"
 
-void TestSameShapedTrajectory(std::vector<wpi::math::Trajectory::State> statesA,
-                              std::vector<wpi::math::Trajectory::State> statesB) {
+void TestSameShapedTrajectory(
+    std::vector<wpi::math::Trajectory::State> statesA,
+    std::vector<wpi::math::Trajectory::State> statesB) {
   for (unsigned int i = 0; i < statesA.size() - 1; i++) {
     auto a1 = statesA[i].pose;
     auto a2 = statesA[i + 1].pose;
@@ -48,7 +49,8 @@ TEST(TrajectoryTransformsTest, TransformBy) {
 TEST(TrajectoryTransformsTest, RelativeTo) {
   wpi::math::TrajectoryConfig config{3_mps, 3_mps_sq};
   auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-      wpi::math::Pose2d{1_m, 2_m, 30_deg}, {}, wpi::math::Pose2d{5_m, 7_m, 90_deg}, config);
+      wpi::math::Pose2d{1_m, 2_m, 30_deg}, {},
+      wpi::math::Pose2d{5_m, 7_m, 90_deg}, config);
 
   auto transformedTrajectory = trajectory.RelativeTo({1_m, 2_m, 30_deg});
 

wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp

@@ -27,8 +27,8 @@ static constexpr auto kDt = 10_ms;
   }
 
 TEST(TrapezoidProfileTest, ReachesGoal) {
-  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{1.75_mps,
-                                                               0.75_mps_sq};
+  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
+      1.75_mps, 0.75_mps_sq};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{3_m, 0_mps};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State state;
 
@@ -42,8 +42,8 @@ TEST(TrapezoidProfileTest, ReachesGoal) {
 // Tests that decreasing the maximum velocity in the middle when it is already
 // moving faster than the new max is handled correctly
 TEST(TrapezoidProfileTest, PosContinuousUnderVelChange) {
-  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{1.75_mps,
-                                                               0.75_mps_sq};
+  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
+      1.75_mps, 0.75_mps_sq};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{12_m, 0_mps};
 
   wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
@@ -76,8 +76,8 @@ TEST(TrapezoidProfileTest, PosContinuousUnderVelChange) {
 
 // There is some somewhat tricky code for dealing with going backwards
 TEST(TrapezoidProfileTest, Backwards) {
-  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
+  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
+      0.75_mps, 0.75_mps_sq};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{-2_m, 0_mps};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State state;
 
@@ -89,8 +89,8 @@ TEST(TrapezoidProfileTest, Backwards) {
 }
 
 TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
-  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
+  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
+      0.75_mps, 0.75_mps_sq};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{-2_m, 0_mps};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State state;
 
@@ -110,8 +110,8 @@ TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
 
 // Checks to make sure that it hits top speed
 TEST(TrapezoidProfileTest, TopSpeed) {
-  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
+  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
+      0.75_mps, 0.75_mps_sq};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{4_m, 0_mps};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State state;
 
@@ -129,8 +129,8 @@ TEST(TrapezoidProfileTest, TopSpeed) {
 }
 
 TEST(TrapezoidProfileTest, TimingToCurrent) {
-  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
+  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
+      0.75_mps, 0.75_mps_sq};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{2_m, 0_mps};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State state;
 
@@ -144,13 +144,13 @@ TEST(TrapezoidProfileTest, TimingToCurrent) {
 TEST(TrapezoidProfileTest, TimingToGoal) {
   using wpi::units::unit_cast;
 
-  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
+  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
+      0.75_mps, 0.75_mps_sq};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{2_m, 0_mps};
 
   wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
-  auto state = profile.Calculate(kDt, goal,
-                                 wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
+  auto state = profile.Calculate(
+      kDt, goal, wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
 
   auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
   bool reachedGoal = false;
@@ -168,13 +168,13 @@ TEST(TrapezoidProfileTest, TimingToGoal) {
 TEST(TrapezoidProfileTest, TimingBeforeGoal) {
   using wpi::units::unit_cast;
 
-  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
+  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
+      0.75_mps, 0.75_mps_sq};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{2_m, 0_mps};
 
   wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
-  auto state = profile.Calculate(kDt, goal,
-                                 wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
+  auto state = profile.Calculate(
+      kDt, goal, wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
 
   auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
   bool reachedGoal = false;
@@ -191,13 +191,13 @@ TEST(TrapezoidProfileTest, TimingBeforeGoal) {
 TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
   using wpi::units::unit_cast;
 
-  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
+  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
+      0.75_mps, 0.75_mps_sq};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{-2_m, 0_mps};
 
   wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
-  auto state = profile.Calculate(kDt, goal,
-                                 wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
+  auto state = profile.Calculate(
+      kDt, goal, wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
 
   auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
   bool reachedGoal = false;
@@ -215,13 +215,13 @@ TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
 TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
   using wpi::units::unit_cast;
 
-  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
+  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
+      0.75_mps, 0.75_mps_sq};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{-2_m, 0_mps};
 
   wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
-  auto state = profile.Calculate(kDt, goal,
-                                 wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
+  auto state = profile.Calculate(
+      kDt, goal, wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
 
   auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
   bool reachedGoal = false;
@@ -236,15 +236,16 @@ TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
 }
 
 TEST(TrapezoidProfileTest, InitalizationOfCurrentState) {
-  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{1_mps, 1_mps_sq};
+  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
+      1_mps, 1_mps_sq};
   wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
   EXPECT_NEAR_UNITS(profile.TimeLeftUntil(0_m), 0_s, 1e-10_s);
   EXPECT_NEAR_UNITS(profile.TotalTime(), 0_s, 1e-10_s);
 }
 
 TEST(TrapezoidProfileTest, InitialVelocityConstraints) {
-  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
+  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
+      0.75_mps, 0.75_mps_sq};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{10_m, 0_mps};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State state{0_m, -10_mps};
 
@@ -258,8 +259,8 @@ TEST(TrapezoidProfileTest, InitialVelocityConstraints) {
 }
 
 TEST(TrapezoidProfileTest, GoalVelocityConstraints) {
-  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
+  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
+      0.75_mps, 0.75_mps_sq};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{10_m, 5_mps};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State state{0_m, 0.75_mps};
 
@@ -273,8 +274,8 @@ TEST(TrapezoidProfileTest, GoalVelocityConstraints) {
 }
 
 TEST(TrapezoidProfileTest, NegativeGoalVelocityConstraints) {
-  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
+  wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
+      0.75_mps, 0.75_mps_sq};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{10_m, -5_mps};
   wpi::math::TrapezoidProfile<wpi::units::meter>::State state{0_m, 0.75_mps};
 

wpimath/src/test/native/cpp/trajectory/proto/TrajectoryProtoTest.cpp

@@ -13,16 +13,20 @@ namespace {
 
 using ProtoType = wpi::util::Protobuf<wpi::math::Trajectory>;
 
-const Trajectory kExpectedData = Trajectory{std::vector<wpi::math::Trajectory::State>{
-    Trajectory::State{1.1_s, 2.2_mps, 3.3_mps_sq,
-                      Pose2d(Translation2d(1.1_m, 2.2_m), Rotation2d(2.2_rad)),
-                      wpi::units::curvature_t{6.6}},
-    Trajectory::State{2.1_s, 2.2_mps, 3.3_mps_sq,
-                      Pose2d(Translation2d(2.1_m, 2.2_m), Rotation2d(2.2_rad)),
-                      wpi::units::curvature_t{6.6}},
-    Trajectory::State{3.1_s, 2.2_mps, 3.3_mps_sq,
-                      Pose2d(Translation2d(3.1_m, 2.2_m), Rotation2d(2.2_rad)),
-                      wpi::units::curvature_t{6.6}}}};
+const Trajectory kExpectedData =
+    Trajectory{std::vector<wpi::math::Trajectory::State>{
+        Trajectory::State{
+            1.1_s, 2.2_mps, 3.3_mps_sq,
+            Pose2d(Translation2d(1.1_m, 2.2_m), Rotation2d(2.2_rad)),
+            wpi::units::curvature_t{6.6}},
+        Trajectory::State{
+            2.1_s, 2.2_mps, 3.3_mps_sq,
+            Pose2d(Translation2d(2.1_m, 2.2_m), Rotation2d(2.2_rad)),
+            wpi::units::curvature_t{6.6}},
+        Trajectory::State{
+            3.1_s, 2.2_mps, 3.3_mps_sq,
+            Pose2d(Translation2d(3.1_m, 2.2_m), Rotation2d(2.2_rad)),
+            wpi::units::curvature_t{6.6}}}};
 }  // namespace
 
 TEST(TrajectoryProtoTest, Roundtrip) {