diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
index 4628653dd4..5f53575df8 100644
--- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
@@ -41,8 +41,8 @@ DifferentialDrivetrainSim::DifferentialDrivetrainSim(
               driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing),
           trackWidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {}
 
-Eigen::Matrix<double, 2, 1> DifferentialDrivetrainSim::ClampInput(
-    Eigen::Matrix<double, 2, 1> u) {
+Eigen::Vector<double, 2> DifferentialDrivetrainSim::ClampInput(
+    const Eigen::Vector<double, 2>& u) {
   return frc::NormalizeInputVector<2>(u,
                                       frc::RobotController::GetInputVoltage());
 }
@@ -66,11 +66,11 @@ double DifferentialDrivetrainSim::GetGearing() const {
   return m_currentGearing;
 }
 
-Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::GetOutput() const {
+Eigen::Vector<double, 7> DifferentialDrivetrainSim::GetOutput() const {
   return m_y;
 }
 
-Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::GetState() const {
+Eigen::Vector<double, 7> DifferentialDrivetrainSim::GetState() const {
   return m_x;
 }
 
@@ -117,7 +117,7 @@ units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const {
 }
 
 void DifferentialDrivetrainSim::SetState(
-    const Eigen::Matrix<double, 7, 1>& state) {
+    const Eigen::Vector<double, 7>& state) {
   m_x = state;
 }
 
@@ -129,9 +129,8 @@ void DifferentialDrivetrainSim::SetPose(const frc::Pose2d& pose) {
   m_x(State::kRightPosition) = 0;
 }
 
-Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::Dynamics(
-    const Eigen::Matrix<double, 7, 1>& x,
-    const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 7> DifferentialDrivetrainSim::Dynamics(
+    const Eigen::Vector<double, 7>& x, const Eigen::Vector<double, 2>& u) {
   // Because G^2 can be factored out of A, we can divide by the old ratio
   // squared and multiply by the new ratio squared to get a new drivetrain
   // model.
@@ -150,7 +149,7 @@ Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::Dynamics(
 
   double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
 
-  Eigen::Matrix<double, 7, 1> xdot;
+  Eigen::Vector<double, 7> xdot;
   xdot(0) = v * std::cos(x(State::kHeading));
   xdot(1) = v * std::sin(x(State::kHeading));
   xdot(2) =
diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
index a2951ecf86..31d2f3a89a 100644
--- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
@@ -6,7 +6,6 @@
 
 #include <wpi/MathExtras.h>
 
-#include "frc/StateSpaceUtil.h"
 #include "frc/system/NumericalIntegration.h"
 #include "frc/system/plant/LinearSystemId.h"
 
@@ -79,25 +78,25 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const {
 }
 
 void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
-  SetInput(frc::MakeMatrix<1, 1>(voltage.to<double>()));
+  SetInput(Eigen::Vector<double, 1>{voltage.to<double>()});
 }
 
-Eigen::Matrix<double, 2, 1> ElevatorSim::UpdateX(
-    const Eigen::Matrix<double, 2, 1>& currentXhat,
-    const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
+Eigen::Vector<double, 2> ElevatorSim::UpdateX(
+    const Eigen::Vector<double, 2>& currentXhat,
+    const Eigen::Vector<double, 1>& u, units::second_t dt) {
   auto updatedXhat = RKDP(
-      [&](const Eigen::Matrix<double, 2, 1>& x,
-          const Eigen::Matrix<double, 1, 1>& u_)
-          -> Eigen::Matrix<double, 2, 1> {
-        return m_plant.A() * x + m_plant.B() * u_ + MakeMatrix<2, 1>(0.0, -9.8);
+      [&](const Eigen::Vector<double, 2>& x,
+          const Eigen::Vector<double, 1>& u_) -> Eigen::Vector<double, 2> {
+        return m_plant.A() * x + m_plant.B() * u_ +
+               Eigen::Vector<double, 2>{0.0, -9.8};
       },
       currentXhat, u, dt);
   // Check for collision after updating x-hat.
   if (WouldHitLowerLimit(units::meter_t(updatedXhat(0)))) {
-    return MakeMatrix<2, 1>(m_minHeight.to<double>(), 0.0);
+    return Eigen::Vector<double, 2>{m_minHeight.to<double>(), 0.0};
   }
   if (WouldHitUpperLimit(units::meter_t(updatedXhat(0)))) {
-    return MakeMatrix<2, 1>(m_maxHeight.to<double>(), 0.0);
+    return Eigen::Vector<double, 2>{m_maxHeight.to<double>(), 0.0};
   }
   return updatedXhat;
 }
diff --git a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
index fef5792765..442183a163 100644
--- a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
@@ -38,5 +38,5 @@ units::ampere_t FlywheelSim::GetCurrentDraw() const {
 }
 
 void FlywheelSim::SetInputVoltage(units::volt_t voltage) {
-  SetInput(frc::MakeMatrix<1, 1>(voltage.to<double>()));
+  SetInput(Eigen::Vector<double, 1>{voltage.to<double>()});
 }
diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
index 908a340528..eeb982d1d6 100644
--- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
@@ -72,12 +72,12 @@ units::ampere_t SingleJointedArmSim::GetCurrentDraw() const {
 }
 
 void SingleJointedArmSim::SetInputVoltage(units::volt_t voltage) {
-  SetInput(frc::MakeMatrix<1, 1>(voltage.to<double>()));
+  SetInput(Eigen::Vector<double, 1>{voltage.to<double>()});
 }
 
-Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
-    const Eigen::Matrix<double, 2, 1>& currentXhat,
-    const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
+Eigen::Vector<double, 2> SingleJointedArmSim::UpdateX(
+    const Eigen::Vector<double, 2>& currentXhat,
+    const Eigen::Vector<double, 1>& u, units::second_t dt) {
   // Horizontal case:
   // Torque = F * r = I * alpha
   // alpha = F * r / I
@@ -88,14 +88,15 @@ Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
   // We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
   // std::cos(theta)]]
 
-  Eigen::Matrix<double, 2, 1> updatedXhat = RKDP(
-      [&](const auto& x, const auto& u) -> Eigen::Matrix<double, 2, 1> {
-        Eigen::Matrix<double, 2, 1> xdot = m_plant.A() * x + m_plant.B() * u;
+  Eigen::Vector<double, 2> updatedXhat = RKDP(
+      [&](const auto& x, const auto& u) -> Eigen::Vector<double, 2> {
+        Eigen::Vector<double, 2> xdot = m_plant.A() * x + m_plant.B() * u;
 
         if (m_simulateGravity) {
-          xdot += MakeMatrix<2, 1>(0.0, (m_mass * m_r * -9.8 * 3.0 /
-                                         (m_mass * m_r * m_r) * std::cos(x(0)))
-                                            .template to<double>());
+          xdot += Eigen::Vector<double, 2>{
+              0.0, (m_mass * m_r * -9.8 * 3.0 / (m_mass * m_r * m_r) *
+                    std::cos(x(0)))
+                       .template to<double>()};
         }
         return xdot;
       },
@@ -103,9 +104,9 @@ Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
 
   // Check for collisions.
   if (WouldHitLowerLimit(units::radian_t(updatedXhat(0)))) {
-    return MakeMatrix<2, 1>(m_minAngle.to<double>(), 0.0);
+    return Eigen::Vector<double, 2>{m_minAngle.to<double>(), 0.0};
   } else if (WouldHitUpperLimit(units::radian_t(updatedXhat(0)))) {
-    return MakeMatrix<2, 1>(m_maxAngle.to<double>(), 0.0);
+    return Eigen::Vector<double, 2>{m_maxAngle.to<double>(), 0.0};
   }
   return updatedXhat;
 }
diff --git a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
index bcb23e32e1..3de59b5aea 100644
--- a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
+++ b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
@@ -80,7 +80,7 @@ class DifferentialDrivetrainSim {
    * @param maxVoltage The maximum voltage.
    * @return The normalized input.
    */
-  Eigen::Matrix<double, 2, 1> ClampInput(Eigen::Matrix<double, 2, 1> u);
+  Eigen::Vector<double, 2> ClampInput(const Eigen::Vector<double, 2>& u);
 
   /**
    * Sets the applied voltage to the drivetrain. Note that positive voltage must
@@ -178,7 +178,7 @@ class DifferentialDrivetrainSim {
    *
    * @param state The state.
    */
-  void SetState(const Eigen::Matrix<double, 7, 1>& state);
+  void SetState(const Eigen::Vector<double, 7>& state);
 
   /**
    * Sets the system pose.
@@ -187,8 +187,8 @@ class DifferentialDrivetrainSim {
    */
   void SetPose(const frc::Pose2d& pose);
 
-  Eigen::Matrix<double, 7, 1> Dynamics(const Eigen::Matrix<double, 7, 1>& x,
-                                       const Eigen::Matrix<double, 2, 1>& u);
+  Eigen::Vector<double, 7> Dynamics(const Eigen::Vector<double, 7>& x,
+                                    const Eigen::Vector<double, 2>& u);
 
   class State {
    public:
@@ -295,7 +295,7 @@ class DifferentialDrivetrainSim {
   /**
    * Returns the current output vector y.
    */
-  Eigen::Matrix<double, 7, 1> GetOutput() const;
+  Eigen::Vector<double, 7> GetOutput() const;
 
   /**
    * Returns an element of the state vector. Note that this will not include
@@ -308,7 +308,7 @@ class DifferentialDrivetrainSim {
   /**
    * Returns the current state vector x. Note that this will not include noise!
    */
-  Eigen::Matrix<double, 7, 1> GetState() const;
+  Eigen::Vector<double, 7> GetState() const;
 
   LinearSystem<2, 2, 2> m_plant;
   units::meter_t m_rb;
@@ -319,9 +319,9 @@ class DifferentialDrivetrainSim {
   double m_originalGearing;
   double m_currentGearing;
 
-  Eigen::Matrix<double, 7, 1> m_x;
-  Eigen::Matrix<double, 2, 1> m_u;
-  Eigen::Matrix<double, 7, 1> m_y;
+  Eigen::Vector<double, 7> m_x;
+  Eigen::Vector<double, 2> m_u;
+  Eigen::Vector<double, 7> m_y;
   std::array<double, 7> m_measurementStdDevs;
 };
 }  // namespace frc::sim
diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
index c7849452e1..14b6d2df29 100644
--- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
+++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
@@ -123,9 +123,9 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
    * @param u           The system inputs (voltage).
    * @param dt          The time difference between controller updates.
    */
-  Eigen::Matrix<double, 2, 1> UpdateX(
-      const Eigen::Matrix<double, 2, 1>& currentXhat,
-      const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) override;
+  Eigen::Vector<double, 2> UpdateX(const Eigen::Vector<double, 2>& currentXhat,
+                                   const Eigen::Vector<double, 1>& u,
+                                   units::second_t dt) override;
 
  private:
   DCMotor m_gearbox;
diff --git a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
index db0a3a7ddc..61cbd4630b 100644
--- a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
+++ b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
@@ -40,9 +40,9 @@ class LinearSystemSim {
       const LinearSystem<States, Inputs, Outputs>& system,
       const std::array<double, Outputs>& measurementStdDevs = {})
       : m_plant(system), m_measurementStdDevs(measurementStdDevs) {
-    m_x = Eigen::Matrix<double, States, 1>::Zero();
-    m_y = Eigen::Matrix<double, Outputs, 1>::Zero();
-    m_u = Eigen::Matrix<double, Inputs, 1>::Zero();
+    m_x = Eigen::Vector<double, States>::Zero();
+    m_y = Eigen::Vector<double, Outputs>::Zero();
+    m_u = Eigen::Vector<double, Inputs>::Zero();
   }
 
   /**
@@ -69,7 +69,7 @@ class LinearSystemSim {
    *
    * @return The current output of the plant.
    */
-  const Eigen::Matrix<double, Outputs, 1>& GetOutput() const { return m_y; }
+  const Eigen::Vector<double, Outputs>& GetOutput() const { return m_y; }
 
   /**
    * Returns an element of the current output of the plant.
@@ -84,9 +84,7 @@ class LinearSystemSim {
    *
    * @param u The system inputs.
    */
-  void SetInput(const Eigen::Matrix<double, Inputs, 1>& u) {
-    m_u = ClampInput(u);
-  }
+  void SetInput(const Eigen::Vector<double, Inputs>& u) { m_u = ClampInput(u); }
 
   /*
    * Sets the system inputs.
@@ -104,7 +102,7 @@ class LinearSystemSim {
    *
    * @param state The new state.
    */
-  void SetState(const Eigen::Matrix<double, States, 1>& state) { m_x = state; }
+  void SetState(const Eigen::Vector<double, States>& state) { m_x = state; }
 
   /**
    * Returns the current drawn by this simulated system. Override this method to
@@ -124,9 +122,9 @@ class LinearSystemSim {
    * @param u           The system inputs (usually voltage).
    * @param dt          The time difference between controller updates.
    */
-  virtual Eigen::Matrix<double, States, 1> UpdateX(
-      const Eigen::Matrix<double, States, 1>& currentXhat,
-      const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
+  virtual Eigen::Vector<double, States> UpdateX(
+      const Eigen::Vector<double, States>& currentXhat,
+      const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
     return m_plant.CalculateX(currentXhat, u, dt);
   }
 
@@ -137,17 +135,16 @@ class LinearSystemSim {
    * @param u          The input vector.
    * @return The normalized input.
    */
-  Eigen::Matrix<double, Inputs, 1> ClampInput(
-      Eigen::Matrix<double, Inputs, 1> u) {
+  Eigen::Vector<double, Inputs> ClampInput(Eigen::Vector<double, Inputs> u) {
     return frc::NormalizeInputVector<Inputs>(
         u, frc::RobotController::GetInputVoltage());
   }
 
   LinearSystem<States, Inputs, Outputs> m_plant;
 
-  Eigen::Matrix<double, States, 1> m_x;
-  Eigen::Matrix<double, Outputs, 1> m_y;
-  Eigen::Matrix<double, Inputs, 1> m_u;
+  Eigen::Vector<double, States> m_x;
+  Eigen::Vector<double, Outputs> m_y;
+  Eigen::Vector<double, Inputs> m_u;
   std::array<double, Outputs> m_measurementStdDevs;
 };
 }  // namespace frc::sim
diff --git a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
index 3fe42ba0fb..4ff29cc0b5 100644
--- a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
+++ b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
@@ -142,9 +142,9 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
    * @param u           The system inputs (voltage).
    * @param dt          The time difference between controller updates.
    */
-  Eigen::Matrix<double, 2, 1> UpdateX(
-      const Eigen::Matrix<double, 2, 1>& currentXhat,
-      const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) override;
+  Eigen::Vector<double, 2> UpdateX(const Eigen::Vector<double, 2>& currentXhat,
+                                   const Eigen::Vector<double, 1>& u,
+                                   units::second_t dt) override;
 
  private:
   units::meter_t m_r;
diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
index 3eeb4ef49a..6d208aa5c8 100644
--- a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
@@ -30,11 +30,10 @@ TEST(DifferentialDriveSim, Convergence) {
   frc::LinearPlantInversionFeedforward feedforward{plant, 20_ms};
   frc::RamseteController ramsete;
 
-  feedforward.Reset(frc::MakeMatrix<2, 1>(0.0, 0.0));
+  feedforward.Reset(Eigen::Vector<double, 2>{0.0, 0.0});
 
   // Ground truth.
-  Eigen::Matrix<double, 7, 1> groundTruthX =
-      Eigen::Matrix<double, 7, 1>::Zero();
+  Eigen::Vector<double, 7> groundTruthX = Eigen::Vector<double, 7>::Zero();
 
   frc::TrajectoryConfig config{1_mps, 1_mps_sq};
   config.AddConstraint(
@@ -50,7 +49,7 @@ TEST(DifferentialDriveSim, Convergence) {
 
     auto [l, r] = kinematics.ToWheelSpeeds(ramseteOut);
     auto voltages = feedforward.Calculate(
-        frc::MakeMatrix<2, 1>(l.to<double>(), r.to<double>()));
+        Eigen::Vector<double, 2>{l.to<double>(), r.to<double>()});
 
     // Sim periodic code.
     sim.SetInputs(units::volt_t(voltages(0, 0)), units::volt_t(voltages(1, 0)));
@@ -58,7 +57,7 @@ TEST(DifferentialDriveSim, Convergence) {
 
     // Update ground truth.
     groundTruthX = frc::RK4(
-        [&sim](const auto& x, const auto& u) -> Eigen::Matrix<double, 7, 1> {
+        [&sim](const auto& x, const auto& u) -> Eigen::Vector<double, 7> {
           return sim.Dynamics(x, u);
         },
         groundTruthX, voltages, 20_ms);
diff --git a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
index 1a52036c1c..340f51338c 100644
--- a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
@@ -8,7 +8,6 @@
 
 #include "frc/Encoder.h"
 #include "frc/RobotController.h"
-#include "frc/StateSpaceUtil.h"
 #include "frc/controller/PIDController.h"
 #include "frc/motorcontrol/PWMVictorSPX.h"
 #include "frc/simulation/ElevatorSim.h"
@@ -33,8 +32,8 @@ TEST(ElevatorSim, StateSpaceSim) {
     auto nextVoltage = controller.Calculate(encoderSim.GetDistance());
     motor.Set(nextVoltage / frc::RobotController::GetInputVoltage());
 
-    auto u = frc::MakeMatrix<1, 1>(motor.Get() *
-                                   frc::RobotController::GetInputVoltage());
+    Eigen::Vector<double, 1> u{motor.Get() *
+                               frc::RobotController::GetInputVoltage()};
     sim.SetInput(u);
     sim.Update(20_ms);
 
@@ -50,7 +49,7 @@ TEST(ElevatorSim, MinMax) {
                             units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 1_m,
                             {0.01});
   for (size_t i = 0; i < 100; ++i) {
-    sim.SetInput(frc::MakeMatrix<1, 1>(0.0));
+    sim.SetInput(Eigen::Vector<double, 1>{0.0});
     sim.Update(20_ms);
 
     auto height = sim.GetPosition();
@@ -58,7 +57,7 @@ TEST(ElevatorSim, MinMax) {
   }
 
   for (size_t i = 0; i < 100; ++i) {
-    sim.SetInput(frc::MakeMatrix<1, 1>(12.0));
+    sim.SetInput(Eigen::Vector<double, 1>{12.0});
     sim.Update(20_ms);
 
     auto height = sim.GetPosition();
@@ -75,14 +74,14 @@ TEST(ElevatorSim, Stability) {
   frc::LinearSystem<2, 1, 1> system = frc::LinearSystemId::ElevatorSystem(
       m_elevatorGearbox, kCarriageMass, kElevatorDrumRadius, kElevatorGearing);
 
-  Eigen::Matrix<double, 2, 1> x0 = frc::MakeMatrix<2, 1>(0.0, 0.0);
-  Eigen::Matrix<double, 1, 1> u0 = frc::MakeMatrix<1, 1>(12.0);
+  Eigen::Vector<double, 2> x0{0.0, 0.0};
+  Eigen::Vector<double, 1> u0{12.0};
 
-  Eigen::Matrix<double, 2, 1> x1 = frc::MakeMatrix<2, 1>(0.0, 0.0);
+  Eigen::Vector<double, 2> x1{0.0, 0.0};
   for (size_t i = 0; i < 50; i++) {
     x1 = frc::RKDP(
-        [&](Eigen::Matrix<double, 2, 1> x,
-            Eigen::Matrix<double, 1, 1> u) -> Eigen::Matrix<double, 2, 1> {
+        [&](const Eigen::Vector<double, 2>& x,
+            const Eigen::Vector<double, 1>& u) -> Eigen::Vector<double, 2> {
           return system.A() * x + system.B() * u;
         },
         x1, u0, 0.020_s);
diff --git a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp
index f682fe7867..7046a5a785 100644
--- a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp
@@ -10,10 +10,10 @@
 TEST(SingleJointedArmTest, Disabled) {
   frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 100, 3_kg_sq_m,
                                     9.5_in, -180_deg, 0_deg, 10_lb, true);
-  sim.SetState(frc::MakeMatrix<2, 1>(0.0, 0.0));
+  sim.SetState(Eigen::Vector<double, 2>{0.0, 0.0});
 
   for (size_t i = 0; i < 12 / 0.02; ++i) {
-    sim.SetInput(frc::MakeMatrix<1, 1>(0.0));
+    sim.SetInput(Eigen::Vector<double, 1>{0.0});
     sim.Update(20_ms);
   }
 
diff --git a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
index 9fafe7c9ef..557e77b9ce 100644
--- a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
@@ -48,8 +48,8 @@ TEST(StateSpaceSimTest, TestFlywheelSim) {
     // Then, SimulationPeriodic runs
     frc::sim::RoboRioSim::SetVInVoltage(
         frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
-    sim.SetInput(frc::MakeMatrix<1, 1>(
-        motor.Get() * frc::RobotController::GetInputVoltage()));
+    sim.SetInput(Eigen::Vector<double, 1>{
+        motor.Get() * frc::RobotController::GetInputVoltage()});
     sim.Update(20_ms);
     encoderSim.SetRate(sim.GetAngularVelocity().to<double>());
   }
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp
index ce0751facd..bb8a93de47 100644
--- a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp
@@ -5,7 +5,6 @@
 #include <frc/Encoder.h>
 #include <frc/Joystick.h>
 #include <frc/RobotController.h>
-#include <frc/StateSpaceUtil.h>
 #include <frc/TimedRobot.h>
 #include <frc/controller/PIDController.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
@@ -69,8 +68,8 @@ class Robot : public frc::TimedRobot {
   void SimulationPeriodic() override {
     // In this method, we update our simulation of what our arm is doing
     // First, we set our "inputs" (voltages)
-    m_armSim.SetInput(frc::MakeMatrix<1, 1>(
-        m_motor.Get() * frc::RobotController::GetInputVoltage()));
+    m_armSim.SetInput(Eigen::Vector<double, 1>{
+        m_motor.Get() * frc::RobotController::GetInputVoltage()});
 
     // Next, we update it. The standard loop time is 20ms.
     m_armSim.Update(20_ms);
diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp
index ece158b670..49c607d840 100644
--- a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp
@@ -5,7 +5,6 @@
 #include <frc/Encoder.h>
 #include <frc/Joystick.h>
 #include <frc/RobotController.h>
-#include <frc/StateSpaceUtil.h>
 #include <frc/TimedRobot.h>
 #include <frc/controller/PIDController.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
@@ -68,8 +67,8 @@ class Robot : public frc::TimedRobot {
   void SimulationPeriodic() override {
     // In this method, we update our simulation of what our elevator is doing
     // First, we set our "inputs" (voltages)
-    m_elevatorSim.SetInput(frc::MakeMatrix<1, 1>(
-        m_motor.Get() * frc::RobotController::GetInputVoltage()));
+    m_elevatorSim.SetInput(Eigen::Vector<double, 1>{
+        m_motor.Get() * frc::RobotController::GetInputVoltage()});
 
     // Next, we update it. The standard loop time is 20ms.
     m_elevatorSim.Update(20_ms);
diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
index 3d28515b05..71233a30a8 100644
--- a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
@@ -3,7 +3,6 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Encoder.h>
-#include <frc/StateSpaceUtil.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 #include <frc/controller/LinearPlantInversionFeedforward.h>
@@ -100,7 +99,7 @@ class Robot : public frc::TimedRobot {
 
   void TeleopInit() override {
     m_loop.Reset(
-        frc::MakeMatrix<2, 1>(m_encoder.GetDistance(), m_encoder.GetRate()));
+        Eigen::Vector<double, 2>{m_encoder.GetDistance(), m_encoder.GetRate()});
 
     m_lastProfiledReference = {
         units::radian_t(m_encoder.GetDistance()),
@@ -123,12 +122,12 @@ class Robot : public frc::TimedRobot {
                                                m_lastProfiledReference))
             .Calculate(20_ms);
 
-    m_loop.SetNextR(
-        frc::MakeMatrix<2, 1>(m_lastProfiledReference.position.to<double>(),
-                              m_lastProfiledReference.velocity.to<double>()));
+    m_loop.SetNextR(Eigen::Vector<double, 2>{
+        m_lastProfiledReference.position.to<double>(),
+        m_lastProfiledReference.velocity.to<double>()});
 
     // Correct our Kalman filter's state vector estimate with encoder data.
-    m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetDistance()));
+    m_loop.Correct(Eigen::Vector<double, 1>{m_encoder.GetDistance()});
 
     // Update our LQR to generate new voltage commands and use the voltages to
     // predict the next state with out Kalman filter.
diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
index 95471f87d8..70a3ac6bb8 100644
--- a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
@@ -3,7 +3,6 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Encoder.h>
-#include <frc/StateSpaceUtil.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 #include <frc/controller/LinearQuadraticRegulator.h>
@@ -98,7 +97,7 @@ class Robot : public frc::TimedRobot {
   void TeleopInit() override {
     // Reset our loop to make sure it's in a known state.
     m_loop.Reset(
-        frc::MakeMatrix<2, 1>(m_encoder.GetDistance(), m_encoder.GetRate()));
+        Eigen::Vector<double, 2>{m_encoder.GetDistance(), m_encoder.GetRate()});
 
     m_lastProfiledReference = {units::meter_t(m_encoder.GetDistance()),
                                units::meters_per_second_t(m_encoder.GetRate())};
@@ -120,12 +119,12 @@ class Robot : public frc::TimedRobot {
                                               m_lastProfiledReference))
             .Calculate(20_ms);
 
-    m_loop.SetNextR(
-        frc::MakeMatrix<2, 1>(m_lastProfiledReference.position.to<double>(),
-                              m_lastProfiledReference.velocity.to<double>()));
+    m_loop.SetNextR(Eigen::Vector<double, 2>{
+        m_lastProfiledReference.position.to<double>(),
+        m_lastProfiledReference.velocity.to<double>()});
 
     // Correct our Kalman filter's state vector estimate with encoder data.
-    m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetDistance()));
+    m_loop.Correct(Eigen::Vector<double, 1>{m_encoder.GetDistance()});
 
     // Update our LQR to generate new voltage commands and use the voltages to
     // predict the next state with out Kalman filter.
diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp
index 5c07ee8686..1e9e6851a1 100644
--- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp
@@ -4,7 +4,6 @@
 
 #include <frc/DriverStation.h>
 #include <frc/Encoder.h>
-#include <frc/StateSpaceUtil.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 #include <frc/controller/LinearQuadraticRegulator.h>
@@ -86,7 +85,7 @@ class Robot : public frc::TimedRobot {
   }
 
   void TeleopInit() override {
-    m_loop.Reset(frc::MakeMatrix<1, 1>(m_encoder.GetRate()));
+    m_loop.Reset(Eigen::Vector<double, 1>{m_encoder.GetRate()});
   }
 
   void TeleopPeriodic() override {
@@ -94,14 +93,14 @@ class Robot : public frc::TimedRobot {
     // setpoint of a PID controller.
     if (m_joystick.GetRightBumper()) {
       // We pressed the bumper, so let's set our next reference
-      m_loop.SetNextR(frc::MakeMatrix<1, 1>(kSpinup.to<double>()));
+      m_loop.SetNextR(Eigen::Vector<double, 1>{kSpinup.to<double>()});
     } else {
       // We released the bumper, so let's spin down
-      m_loop.SetNextR(frc::MakeMatrix<1, 1>(0.0));
+      m_loop.SetNextR(Eigen::Vector<double, 1>{0.0});
     }
 
     // Correct our Kalman filter's state vector estimate with encoder data.
-    m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetRate()));
+    m_loop.Correct(Eigen::Vector<double, 1>{m_encoder.GetRate()});
 
     // Update our LQR to generate new voltage commands and use the voltages to
     // predict the next state with out Kalman filter.
diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp
index 0f3e36315f..32f6ceca28 100644
--- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp
@@ -4,7 +4,6 @@
 
 #include <frc/DriverStation.h>
 #include <frc/Encoder.h>
-#include <frc/StateSpaceUtil.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 #include <frc/controller/LinearPlantInversionFeedforward.h>
@@ -87,7 +86,7 @@ class Robot : public frc::TimedRobot {
   }
 
   void TeleopInit() override {
-    m_loop.Reset(frc::MakeMatrix<1, 1>(m_encoder.GetRate()));
+    m_loop.Reset(Eigen::Vector<double, 1>{m_encoder.GetRate()});
   }
 
   void TeleopPeriodic() override {
@@ -95,14 +94,14 @@ class Robot : public frc::TimedRobot {
     // setpoint of a PID controller.
     if (m_joystick.GetRightBumper()) {
       // We pressed the bumper, so let's set our next reference
-      m_loop.SetNextR(frc::MakeMatrix<1, 1>(kSpinup.to<double>()));
+      m_loop.SetNextR(Eigen::Vector<double, 1>{kSpinup.to<double>()});
     } else {
       // We released the bumper, so let's spin down
-      m_loop.SetNextR(frc::MakeMatrix<1, 1>(0.0));
+      m_loop.SetNextR(Eigen::Vector<double, 1>{0.0});
     }
 
     // Correct our Kalman filter's state vector estimate with encoder data.
-    m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetRate()));
+    m_loop.Correct(Eigen::Vector<double, 1>{m_encoder.GetRate()});
 
     // Update our LQR to generate new voltage commands and use the voltages to
     // predict the next state with out Kalman filter.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
index d613135fbb..3ea39e5d26 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
@@ -109,12 +109,12 @@ public class MecanumDriveKinematics {
         chassisSpeeds.vyMetersPerSecond,
         chassisSpeeds.omegaRadiansPerSecond);
 
-    var wheelsMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
+    var wheelsVector = m_inverseKinematics.mult(chassisSpeedsVector);
     return new MecanumDriveWheelSpeeds(
-        wheelsMatrix.get(0, 0),
-        wheelsMatrix.get(1, 0),
-        wheelsMatrix.get(2, 0),
-        wheelsMatrix.get(3, 0));
+        wheelsVector.get(0, 0),
+        wheelsVector.get(1, 0),
+        wheelsVector.get(2, 0),
+        wheelsVector.get(3, 0));
   }
 
   /**
@@ -137,15 +137,15 @@ public class MecanumDriveKinematics {
    * @return The resulting chassis speed.
    */
   public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
-    var wheelSpeedsMatrix = new SimpleMatrix(4, 1);
-    wheelSpeedsMatrix.setColumn(
+    var wheelSpeedsVector = new SimpleMatrix(4, 1);
+    wheelSpeedsVector.setColumn(
         0,
         0,
         wheelSpeeds.frontLeftMetersPerSecond,
         wheelSpeeds.frontRightMetersPerSecond,
         wheelSpeeds.rearLeftMetersPerSecond,
         wheelSpeeds.rearRightMetersPerSecond);
-    var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsMatrix);
+    var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsVector);
 
     return new ChassisSpeeds(
         chassisSpeedsVector.get(0, 0),
diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
index 09a4b7873f..869a2bb71a 100644
--- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
+++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
@@ -6,16 +6,16 @@
 
 namespace frc {
 
-Eigen::Matrix<double, 3, 1> PoseTo3dVector(const Pose2d& pose) {
-  return frc::MakeMatrix<3, 1>(pose.Translation().X().to<double>(),
-                               pose.Translation().Y().to<double>(),
-                               pose.Rotation().Radians().to<double>());
+Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose) {
+  return Eigen::Vector<double, 3>{pose.Translation().X().to<double>(),
+                                  pose.Translation().Y().to<double>(),
+                                  pose.Rotation().Radians().to<double>()};
 }
 
-Eigen::Matrix<double, 4, 1> PoseTo4dVector(const Pose2d& pose) {
-  return frc::MakeMatrix<4, 1>(pose.Translation().X().to<double>(),
-                               pose.Translation().Y().to<double>(),
-                               pose.Rotation().Cos(), pose.Rotation().Sin());
+Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose) {
+  return Eigen::Vector<double, 4>{pose.Translation().X().to<double>(),
+                                  pose.Translation().Y().to<double>(),
+                                  pose.Rotation().Cos(), pose.Rotation().Sin()};
 }
 
 template <>
@@ -30,9 +30,9 @@ bool IsStabilizable<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
   return detail::IsStabilizableImpl<2, 1>(A, B);
 }
 
-Eigen::Matrix<double, 3, 1> PoseToVector(const Pose2d& pose) {
-  return frc::MakeMatrix<3, 1>(pose.X().to<double>(), pose.Y().to<double>(),
-                               pose.Rotation().Radians().to<double>());
+Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose) {
+  return Eigen::Vector<double, 3>{pose.X().to<double>(), pose.Y().to<double>(),
+                                  pose.Rotation().Radians().to<double>()};
 }
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
index 8ec18eb27b..a82384b13d 100644
--- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
@@ -19,9 +19,9 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
     units::second_t nominalDt)
     : m_observer(
           &DifferentialDrivePoseEstimator::F,
-          [](const Eigen::Matrix<double, 5, 1>& x,
-             const Eigen::Matrix<double, 3, 1>& u) {
-            return frc::MakeMatrix<3, 1>(x(3, 0), x(4, 0), x(2, 0));
+          [](const Eigen::Vector<double, 5>& x,
+             const Eigen::Vector<double, 3>& u) {
+            return Eigen::Vector<double, 3>{x(3, 0), x(4, 0), x(2, 0)};
           },
           stateStdDevs, localMeasurementStdDevs, frc::AngleMean<5, 5>(2),
           frc::AngleMean<3, 5>(2), frc::AngleResidual<5>(2),
@@ -30,12 +30,13 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
   SetVisionMeasurementStdDevs(visionMeasurmentStdDevs);
 
   // Create correction mechanism for vision measurements.
-  m_visionCorrect = [&](const Eigen::Matrix<double, 3, 1>& u,
-                        const Eigen::Matrix<double, 3, 1>& y) {
+  m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
+                        const Eigen::Vector<double, 3>& y) {
     m_observer.Correct<3>(
         u, y,
-        [](const Eigen::Matrix<double, 5, 1>& x,
-           const Eigen::Matrix<double, 3, 1>&) { return x.block<3, 1>(0, 0); },
+        [](const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>&) {
+          return x.block<3, 1>(0, 0);
+        },
         m_visionContR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2),
         frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
   };
@@ -94,15 +95,15 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
   auto angle = gyroAngle + m_gyroOffset;
   auto omega = (gyroAngle - m_previousAngle).Radians() / dt;
 
-  auto u = frc::MakeMatrix<3, 1>(
+  auto u = Eigen::Vector<double, 3>{
       (wheelSpeeds.left + wheelSpeeds.right).to<double>() / 2.0, 0.0,
-      omega.to<double>());
+      omega.to<double>()};
 
   m_previousAngle = angle;
 
-  auto localY = frc::MakeMatrix<3, 1>(leftDistance.to<double>(),
-                                      rightDistance.to<double>(),
-                                      angle.Radians().to<double>());
+  auto localY = Eigen::Vector<double, 3>{leftDistance.to<double>(),
+                                         rightDistance.to<double>(),
+                                         angle.Radians().to<double>()};
 
   m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
   m_observer.Predict(u, dt);
@@ -111,26 +112,24 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
   return GetEstimatedPosition();
 }
 
-Eigen::Matrix<double, 5, 1> DifferentialDrivePoseEstimator::F(
-    const Eigen::Matrix<double, 5, 1>& x,
-    const Eigen::Matrix<double, 3, 1>& u) {
+Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::F(
+    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>& u) {
   // Apply a rotation matrix. Note that we do not add x because Runge-Kutta does
   // that for us.
-  auto& theta = x(2, 0);
-  Eigen::Matrix<double, 5, 5> toFieldRotation = frc::MakeMatrix<5, 5>(
-      // clang-format off
-    std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0,
-    std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0,
-    0.0, 0.0, 1.0, 0.0, 0.0,
-    0.0, 0.0, 0.0, 1.0, 0.0,
-    0.0, 0.0, 0.0, 0.0, 1.0);  // clang-format on
+  auto& theta = x(2);
+  Eigen::Matrix<double, 5, 5> toFieldRotation{
+      {std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0},
+      {std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0},
+      {0.0, 0.0, 1.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 1.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 1.0}};
   return toFieldRotation *
-         frc::MakeMatrix<5, 1>(u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0));
+         Eigen::Vector<double, 5>{u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)};
 }
 
 template <int Dim>
 wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
-    const Eigen::Matrix<double, Dim, 1>& stdDevs) {
+    const Eigen::Vector<double, Dim>& stdDevs) {
   wpi::array<double, Dim> array;
   for (size_t i = 0; i < Dim; ++i) {
     array[i] = stdDevs(i);
@@ -138,11 +137,11 @@ wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
   return array;
 }
 
-Eigen::Matrix<double, 5, 1> DifferentialDrivePoseEstimator::FillStateVector(
+Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::FillStateVector(
     const Pose2d& pose, units::meter_t leftDistance,
     units::meter_t rightDistance) {
-  return frc::MakeMatrix<5, 1>(
+  return Eigen::Vector<double, 5>{
       pose.Translation().X().to<double>(), pose.Translation().Y().to<double>(),
       pose.Rotation().Radians().to<double>(), leftDistance.to<double>(),
-      rightDistance.to<double>());
+      rightDistance.to<double>()};
 }
diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
index ef086bd73e..0d1413d9e0 100644
--- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
@@ -18,26 +18,26 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
     const wpi::array<double, 1>& localMeasurementStdDevs,
     const wpi::array<double, 3>& visionMeasurementStdDevs,
     units::second_t nominalDt)
-    : m_observer([](const Eigen::Matrix<double, 3, 1>& x,
-                    const Eigen::Matrix<double, 3, 1>& u) { return u; },
-                 [](const Eigen::Matrix<double, 3, 1>& x,
-                    const Eigen::Matrix<double, 3, 1>& u) {
-                   return x.block<1, 1>(2, 0);
-                 },
-                 stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
-                 frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
-                 frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
+    : m_observer(
+          [](const Eigen::Vector<double, 3>& x,
+             const Eigen::Vector<double, 3>& u) { return u; },
+          [](const Eigen::Vector<double, 3>& x,
+             const Eigen::Vector<double, 3>& u) { return x.block<1, 1>(2, 0); },
+          stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
+          frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
+          frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
       m_kinematics(kinematics),
       m_nominalDt(nominalDt) {
   SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
 
   // Create vision correction mechanism.
-  m_visionCorrect = [&](const Eigen::Matrix<double, 3, 1>& u,
-                        const Eigen::Matrix<double, 3, 1>& y) {
+  m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
+                        const Eigen::Vector<double, 3>& y) {
     m_observer.Correct<3>(
         u, y,
-        [](const Eigen::Matrix<double, 3, 1>& x,
-           const Eigen::Matrix<double, 3, 1>&) { return x; },
+        [](const Eigen::Vector<double, 3>& x, const Eigen::Vector<double, 3>&) {
+          return x;
+        },
         m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
         frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
   };
@@ -100,11 +100,11 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
       Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
           .RotateBy(angle);
 
-  auto u = frc::MakeMatrix<3, 1>(fieldRelativeVelocities.X().to<double>(),
-                                 fieldRelativeVelocities.Y().to<double>(),
-                                 omega.to<double>());
+  Eigen::Vector<double, 3> u{fieldRelativeVelocities.X().to<double>(),
+                             fieldRelativeVelocities.Y().to<double>(),
+                             omega.to<double>()};
 
-  auto localY = frc::MakeMatrix<1, 1>(angle.Radians().template to<double>());
+  Eigen::Vector<double, 1> localY{angle.Radians().template to<double>()};
   m_previousAngle = angle;
 
   m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
index a89e4f9fed..7f2f75daf4 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
@@ -21,31 +21,29 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
     m_previousCoR = centerOfRotation;
   }
 
-  Eigen::Vector3d chassisSpeedsVector;
-  chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
-      chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
+  Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.to<double>(),
+                                      chassisSpeeds.vy.to<double>(),
+                                      chassisSpeeds.omega.to<double>()};
 
-  Eigen::Matrix<double, 4, 1> wheelsMatrix =
+  Eigen::Vector<double, 4> wheelsVector =
       m_inverseKinematics * chassisSpeedsVector;
 
   MecanumDriveWheelSpeeds wheelSpeeds;
-  wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsMatrix(0, 0)};
-  wheelSpeeds.frontRight = units::meters_per_second_t{wheelsMatrix(1, 0)};
-  wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsMatrix(2, 0)};
-  wheelSpeeds.rearRight = units::meters_per_second_t{wheelsMatrix(3, 0)};
+  wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsVector(0)};
+  wheelSpeeds.frontRight = units::meters_per_second_t{wheelsVector(1)};
+  wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsVector(2)};
+  wheelSpeeds.rearRight = units::meters_per_second_t{wheelsVector(3)};
   return wheelSpeeds;
 }
 
 ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
     const MecanumDriveWheelSpeeds& wheelSpeeds) const {
-  Eigen::Matrix<double, 4, 1> wheelSpeedsMatrix;
-  // clang-format off
-  wheelSpeedsMatrix << wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
-                       wheelSpeeds.rearLeft.to<double>(), wheelSpeeds.rearRight.to<double>();
-  // clang-format on
+  Eigen::Vector<double, 4> wheelSpeedsVector{
+      wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
+      wheelSpeeds.rearLeft.to<double>(), wheelSpeeds.rearRight.to<double>()};
 
   Eigen::Vector3d chassisSpeedsVector =
-      m_forwardKinematics.solve(wheelSpeedsMatrix);
+      m_forwardKinematics.solve(wheelSpeedsVector);
 
   return {units::meters_per_second_t{chassisSpeedsVector(0)},  // NOLINT
           units::meters_per_second_t{chassisSpeedsVector(1)},
@@ -56,10 +54,9 @@ void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
                                                   Translation2d fr,
                                                   Translation2d rl,
                                                   Translation2d rr) const {
-  // clang-format off
-  m_inverseKinematics << 1, -1, (-(fl.X() + fl.Y())).template to<double>(),
-                         1,  1, (fr.X() - fr.Y()).template to<double>(),
-                         1,  1, (rl.X() - rl.Y()).template to<double>(),
-                         1, -1, (-(rr.X() + rr.Y())).template to<double>();
-  // clang-format on
+  m_inverseKinematics = Eigen::Matrix<double, 4, 3>{
+      {1, -1, (-(fl.X() + fl.Y())).template to<double>()},
+      {1, 1, (fr.X() - fr.Y()).template to<double>()},
+      {1, 1, (rl.X() - rl.Y()).template to<double>()},
+      {1, -1, (-(rr.X() + rr.Y())).template to<double>()}};
 }
diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
index 064fac381c..2d41915169 100644
--- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h
+++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
@@ -10,6 +10,7 @@
 #include <type_traits>
 
 #include <wpi/SymbolExports.h>
+#include <wpi/deprecated.h>
 
 #include "Eigen/Core"
 #include "Eigen/Eigenvalues"
@@ -60,7 +61,7 @@ void WhiteNoiseVectorImpl(Matrix& result, T elem, Ts... elems) {
 template <int States, int Inputs>
 bool IsStabilizableImpl(const Eigen::Matrix<double, States, States>& A,
                         const Eigen::Matrix<double, States, Inputs>& B) {
-  Eigen::EigenSolver<Eigen::Matrix<double, States, States>> es(A);
+  Eigen::EigenSolver<Eigen::Matrix<double, States, States>> es{A};
 
   for (int i = 0; i < States; ++i) {
     if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
@@ -94,10 +95,13 @@ bool IsStabilizableImpl(const Eigen::Matrix<double, States, States>& A,
  *
  * @param elems An array of elements in the matrix.
  * @return A matrix containing the given elements.
+ * @deprecated Use Eigen::Matrix or Eigen::Vector initializer list constructor.
  */
 template <int Rows, int Cols, typename... Ts,
           typename =
               std::enable_if_t<std::conjunction_v<std::is_same<double, Ts>...>>>
+WPI_DEPRECATED(
+    "Use Eigen::Matrix or Eigen::Vector initializer list constructor")
 Eigen::Matrix<double, Rows, Cols> MakeMatrix(Ts... elems) {
   static_assert(
       sizeof...(elems) == Rows * Cols,
@@ -212,12 +216,12 @@ Eigen::Matrix<double, sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
  * @return White noise vector.
  */
 template <int N>
-Eigen::Matrix<double, N, 1> MakeWhiteNoiseVector(
+Eigen::Vector<double, N> MakeWhiteNoiseVector(
     const std::array<double, N>& stdDevs) {
   std::random_device rd;
   std::mt19937 gen{rd()};
 
-  Eigen::Matrix<double, N, 1> result;
+  Eigen::Vector<double, N> result;
   for (int i = 0; i < N; ++i) {
     // Passing a standard deviation of 0.0 to std::normal_distribution is
     // undefined behavior
@@ -239,7 +243,7 @@ Eigen::Matrix<double, N, 1> MakeWhiteNoiseVector(
  * @return The vector.
  */
 WPILIB_DLLEXPORT
-Eigen::Matrix<double, 3, 1> PoseTo3dVector(const Pose2d& pose);
+Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose);
 
 /**
  * Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
@@ -249,7 +253,7 @@ Eigen::Matrix<double, 3, 1> PoseTo3dVector(const Pose2d& pose);
  * @return The vector.
  */
 WPILIB_DLLEXPORT
-Eigen::Matrix<double, 4, 1> PoseTo4dVector(const Pose2d& pose);
+Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose);
 
 /**
  * Returns true if (A, B) is a stabilizable pair.
@@ -287,7 +291,7 @@ WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(
  * @return The vector.
  */
 WPILIB_DLLEXPORT
-Eigen::Matrix<double, 3, 1> PoseToVector(const Pose2d& pose);
+Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose);
 
 /**
  * Clamps input vector between system's minimum and maximum allowable input.
@@ -296,11 +300,11 @@ Eigen::Matrix<double, 3, 1> PoseToVector(const Pose2d& pose);
  * @return Clamped input vector.
  */
 template <int Inputs>
-Eigen::Matrix<double, Inputs, 1> ClampInputMaxMagnitude(
-    const Eigen::Matrix<double, Inputs, 1>& u,
-    const Eigen::Matrix<double, Inputs, 1>& umin,
-    const Eigen::Matrix<double, Inputs, 1>& umax) {
-  Eigen::Matrix<double, Inputs, 1> result;
+Eigen::Vector<double, Inputs> ClampInputMaxMagnitude(
+    const Eigen::Vector<double, Inputs>& u,
+    const Eigen::Vector<double, Inputs>& umin,
+    const Eigen::Vector<double, Inputs>& umax) {
+  Eigen::Vector<double, Inputs> result;
   for (int i = 0; i < Inputs; ++i) {
     result(i) = std::clamp(u(i), umin(i), umax(i));
   }
@@ -317,8 +321,8 @@ Eigen::Matrix<double, Inputs, 1> ClampInputMaxMagnitude(
  * @return The normalizedInput
  */
 template <int Inputs>
-Eigen::Matrix<double, Inputs, 1> NormalizeInputVector(
-    const Eigen::Matrix<double, Inputs, 1>& u, double maxMagnitude) {
+Eigen::Vector<double, Inputs> NormalizeInputVector(
+    const Eigen::Vector<double, Inputs>& u, double maxMagnitude) {
   double maxValue = u.template lpNorm<Eigen::Infinity>();
 
   if (maxValue > maxMagnitude) {
diff --git a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
index 50970284c1..1d60b86789 100644
--- a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
@@ -47,15 +47,15 @@ class ControlAffinePlantInversionFeedforward {
    * @param dt The timestep between calls of calculate().
    */
   ControlAffinePlantInversionFeedforward(
-      std::function<Eigen::Matrix<double, States, 1>(
-          const Eigen::Matrix<double, States, 1>&,
-          const Eigen::Matrix<double, Inputs, 1>&)>
+      std::function<
+          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
+                                        const Eigen::Vector<double, Inputs>&)>
           f,
       units::second_t dt)
       : m_dt(dt), m_f(f) {
     m_B = NumericalJacobianU<States, States, Inputs>(
-        f, Eigen::Matrix<double, States, 1>::Zero(),
-        Eigen::Matrix<double, Inputs, 1>::Zero());
+        f, Eigen::Vector<double, States>::Zero(),
+        Eigen::Vector<double, Inputs>::Zero());
 
     Reset();
   }
@@ -70,14 +70,14 @@ class ControlAffinePlantInversionFeedforward {
    * @param dt The timestep between calls of calculate().
    */
   ControlAffinePlantInversionFeedforward(
-      std::function<Eigen::Matrix<double, States, 1>(
-          const Eigen::Matrix<double, States, 1>&)>
+      std::function<
+          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&)>
           f,
       const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
       : m_B(B), m_dt(dt) {
-    m_f = [=](const Eigen::Matrix<double, States, 1>& x,
-              const Eigen::Matrix<double, Inputs, 1>& u)
-        -> Eigen::Matrix<double, States, 1> { return f(x); };
+    m_f = [=](const Eigen::Vector<double, States>& x,
+              const Eigen::Vector<double, Inputs>& u)
+        -> Eigen::Vector<double, States> { return f(x); };
 
     Reset();
   }
@@ -92,7 +92,7 @@ class ControlAffinePlantInversionFeedforward {
    *
    * @return The calculated feedforward.
    */
-  const Eigen::Matrix<double, Inputs, 1>& Uff() const { return m_uff; }
+  const Eigen::Vector<double, Inputs>& Uff() const { return m_uff; }
 
   /**
    * Returns an element of the previously calculated feedforward.
@@ -108,7 +108,7 @@ class ControlAffinePlantInversionFeedforward {
    *
    * @return The current reference vector.
    */
-  const Eigen::Matrix<double, States, 1>& R() const { return m_r; }
+  const Eigen::Vector<double, States>& R() const { return m_r; }
 
   /**
    * Returns an element of the reference vector r.
@@ -124,7 +124,7 @@ class ControlAffinePlantInversionFeedforward {
    *
    * @param initialState The initial state vector.
    */
-  void Reset(const Eigen::Matrix<double, States, 1>& initialState) {
+  void Reset(const Eigen::Vector<double, States>& initialState) {
     m_r = initialState;
     m_uff.setZero();
   }
@@ -142,16 +142,16 @@ class ControlAffinePlantInversionFeedforward {
    * future reference. This uses the internally stored "current"
    * reference.
    *
-   * If this method is used the initial state of the system is the one
-   * set using Reset(const Eigen::Matrix<double, States, 1>&).
-   * If the initial state is not set it defaults to a zero vector.
+   * If this method is used the initial state of the system is the one set using
+   * Reset(const Eigen::Vector<double, States>&). If the initial state is not
+   * set it defaults to a zero vector.
    *
    * @param nextR The reference state of the future timestep (k + dt).
    *
    * @return The calculated feedforward.
    */
-  Eigen::Matrix<double, Inputs, 1> Calculate(
-      const Eigen::Matrix<double, States, 1>& nextR) {
+  Eigen::Vector<double, Inputs> Calculate(
+      const Eigen::Vector<double, States>& nextR) {
     return Calculate(m_r, nextR);
   }
 
@@ -163,13 +163,13 @@ class ControlAffinePlantInversionFeedforward {
    *
    * @return The calculated feedforward.
    */
-  Eigen::Matrix<double, Inputs, 1> Calculate(
-      const Eigen::Matrix<double, States, 1>& r,
-      const Eigen::Matrix<double, States, 1>& nextR) {
-    Eigen::Matrix<double, States, 1> rDot = (nextR - r) / m_dt.to<double>();
+  Eigen::Vector<double, Inputs> Calculate(
+      const Eigen::Vector<double, States>& r,
+      const Eigen::Vector<double, States>& nextR) {
+    Eigen::Vector<double, States> rDot = (nextR - r) / m_dt.to<double>();
 
     m_uff = m_B.householderQr().solve(
-        rDot - m_f(r, Eigen::Matrix<double, Inputs, 1>::Zero()));
+        rDot - m_f(r, Eigen::Vector<double, Inputs>::Zero()));
 
     m_r = nextR;
     return m_uff;
@@ -183,16 +183,16 @@ class ControlAffinePlantInversionFeedforward {
   /**
    * The model dynamics.
    */
-  std::function<Eigen::Matrix<double, States, 1>(
-      const Eigen::Matrix<double, States, 1>&,
-      const Eigen::Matrix<double, Inputs, 1>&)>
+  std::function<Eigen::Vector<double, States>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, Inputs>&)>
       m_f;
 
   // Current reference
-  Eigen::Matrix<double, States, 1> m_r;
+  Eigen::Vector<double, States> m_r;
 
   // Computed feedforward
-  Eigen::Matrix<double, Inputs, 1> m_uff;
+  Eigen::Vector<double, Inputs> m_uff;
 };
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
index be25a72738..cb4f907d1b 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
@@ -59,7 +59,7 @@ class LinearPlantInversionFeedforward {
    *
    * @return The calculated feedforward.
    */
-  const Eigen::Matrix<double, Inputs, 1>& Uff() const { return m_uff; }
+  const Eigen::Vector<double, Inputs>& Uff() const { return m_uff; }
 
   /**
    * Returns an element of the previously calculated feedforward.
@@ -75,7 +75,7 @@ class LinearPlantInversionFeedforward {
    *
    * @return The current reference vector.
    */
-  const Eigen::Matrix<double, States, 1>& R() const { return m_r; }
+  const Eigen::Vector<double, States>& R() const { return m_r; }
 
   /**
    * Returns an element of the reference vector r.
@@ -91,7 +91,7 @@ class LinearPlantInversionFeedforward {
    *
    * @param initialState The initial state vector.
    */
-  void Reset(const Eigen::Matrix<double, States, 1>& initialState) {
+  void Reset(const Eigen::Vector<double, States>& initialState) {
     m_r = initialState;
     m_uff.setZero();
   }
@@ -109,16 +109,16 @@ class LinearPlantInversionFeedforward {
    * future reference. This uses the internally stored "current"
    * reference.
    *
-   * If this method is used the initial state of the system is the one
-   * set using Reset(const Eigen::Matrix<double, States, 1>&).
-   * If the initial state is not set it defaults to a zero vector.
+   * If this method is used the initial state of the system is the one set using
+   * Reset(const Eigen::Vector<double, States>&). If the initial state is not
+   * set it defaults to a zero vector.
    *
    * @param nextR The reference state of the future timestep (k + dt).
    *
    * @return The calculated feedforward.
    */
-  Eigen::Matrix<double, Inputs, 1> Calculate(
-      const Eigen::Matrix<double, States, 1>& nextR) {
+  Eigen::Vector<double, Inputs> Calculate(
+      const Eigen::Vector<double, States>& nextR) {
     return Calculate(m_r, nextR);  // NOLINT
   }
 
@@ -130,9 +130,9 @@ class LinearPlantInversionFeedforward {
    *
    * @return The calculated feedforward.
    */
-  Eigen::Matrix<double, Inputs, 1> Calculate(
-      const Eigen::Matrix<double, States, 1>& r,
-      const Eigen::Matrix<double, States, 1>& nextR) {
+  Eigen::Vector<double, Inputs> Calculate(
+      const Eigen::Vector<double, States>& r,
+      const Eigen::Vector<double, States>& nextR) {
     m_uff = m_B.householderQr().solve(nextR - (m_A * r));
     m_r = nextR;
     return m_uff;
@@ -145,10 +145,10 @@ class LinearPlantInversionFeedforward {
   units::second_t m_dt;
 
   // Current reference
-  Eigen::Matrix<double, States, 1> m_r;
+  Eigen::Vector<double, States> m_r;
 
   // Computed feedforward
-  Eigen::Matrix<double, Inputs, 1> m_uff;
+  Eigen::Vector<double, Inputs> m_uff;
 };
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
index 424af044d7..c324702b81 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
+++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
@@ -146,7 +146,7 @@ class LinearQuadraticRegulatorImpl {
    *
    * @return The reference vector.
    */
-  const Eigen::Matrix<double, States, 1>& R() const { return m_r; }
+  const Eigen::Vector<double, States>& R() const { return m_r; }
 
   /**
    * Returns an element of the reference vector r.
@@ -162,7 +162,7 @@ class LinearQuadraticRegulatorImpl {
    *
    * @return The control input.
    */
-  const Eigen::Matrix<double, Inputs, 1>& U() const { return m_u; }
+  const Eigen::Vector<double, Inputs>& U() const { return m_u; }
 
   /**
    * Returns an element of the control input vector u.
@@ -186,8 +186,8 @@ class LinearQuadraticRegulatorImpl {
    *
    * @param x The current state x.
    */
-  Eigen::Matrix<double, Inputs, 1> Calculate(
-      const Eigen::Matrix<double, States, 1>& x) {
+  Eigen::Vector<double, Inputs> Calculate(
+      const Eigen::Vector<double, States>& x) {
     m_u = m_K * (m_r - x);
     return m_u;
   }
@@ -198,9 +198,9 @@ class LinearQuadraticRegulatorImpl {
    * @param x     The current state x.
    * @param nextR The next reference vector r.
    */
-  Eigen::Matrix<double, Inputs, 1> Calculate(
-      const Eigen::Matrix<double, States, 1>& x,
-      const Eigen::Matrix<double, States, 1>& nextR) {
+  Eigen::Vector<double, Inputs> Calculate(
+      const Eigen::Vector<double, States>& x,
+      const Eigen::Vector<double, States>& nextR) {
     m_r = nextR;
     return Calculate(x);
   }
@@ -233,10 +233,10 @@ class LinearQuadraticRegulatorImpl {
 
  private:
   // Current reference
-  Eigen::Matrix<double, States, 1> m_r;
+  Eigen::Vector<double, States> m_r;
 
   // Computed controller output
-  Eigen::Matrix<double, Inputs, 1> m_u;
+  Eigen::Vector<double, Inputs> m_u;
 
   // Controller gain
   Eigen::Matrix<double, Inputs, States> m_K;
diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
index da125de18a..a00d343081 100644
--- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
@@ -70,11 +70,8 @@ class SimpleMotorFeedforward {
     auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
     LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
 
-    Eigen::Matrix<double, 1, 1> r;
-    r << currentVelocity.template to<double>();
-
-    Eigen::Matrix<double, 1, 1> nextR;
-    nextR << nextVelocity.template to<double>();
+    Eigen::Vector<double, 1> r{currentVelocity.template to<double>()};
+    Eigen::Vector<double, 1> nextR{nextVelocity.template to<double>()};
 
     return kS * wpi::sgn(currentVelocity.template to<double>()) +
            units::volt_t{feedforward.Calculate(r, nextR)(0)};
diff --git a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
index 9b4a958f27..30d2b41ae3 100644
--- a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
+++ b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
@@ -20,10 +20,10 @@ namespace frc {
  * @param angleStateIdx The row containing angles to be normalized.
  */
 template <int States>
-Eigen::Matrix<double, States, 1> AngleResidual(
-    const Eigen::Matrix<double, States, 1>& a,
-    const Eigen::Matrix<double, States, 1>& b, int angleStateIdx) {
-  Eigen::Matrix<double, States, 1> ret = a - b;
+Eigen::Vector<double, States> AngleResidual(
+    const Eigen::Vector<double, States>& a,
+    const Eigen::Vector<double, States>& b, int angleStateIdx) {
+  Eigen::Vector<double, States> ret = a - b;
   ret[angleStateIdx] =
       AngleModulus(units::radian_t{ret[angleStateIdx]}).to<double>();
   return ret;
@@ -36,9 +36,8 @@ Eigen::Matrix<double, States, 1> AngleResidual(
  * @param angleStateIdx The row containing angles to be normalized.
  */
 template <int States>
-std::function<
-    Eigen::Matrix<double, States, 1>(const Eigen::Matrix<double, States, 1>&,
-                                     const Eigen::Matrix<double, States, 1>&)>
+std::function<Eigen::Vector<double, States>(
+    const Eigen::Vector<double, States>&, const Eigen::Vector<double, States>&)>
 AngleResidual(int angleStateIdx) {
   return [=](auto a, auto b) {
     return AngleResidual<States>(a, b, angleStateIdx);
@@ -54,10 +53,10 @@ AngleResidual(int angleStateIdx) {
  * @param angleStateIdx The row containing angles to be normalized.
  */
 template <int States>
-Eigen::Matrix<double, States, 1> AngleAdd(
-    const Eigen::Matrix<double, States, 1>& a,
-    const Eigen::Matrix<double, States, 1>& b, int angleStateIdx) {
-  Eigen::Matrix<double, States, 1> ret = a + b;
+Eigen::Vector<double, States> AngleAdd(const Eigen::Vector<double, States>& a,
+                                       const Eigen::Vector<double, States>& b,
+                                       int angleStateIdx) {
+  Eigen::Vector<double, States> ret = a + b;
   ret[angleStateIdx] =
       InputModulus(ret[angleStateIdx], -wpi::numbers::pi, wpi::numbers::pi);
   return ret;
@@ -70,9 +69,8 @@ Eigen::Matrix<double, States, 1> AngleAdd(
  * @param angleStateIdx The row containing angles to be normalized.
  */
 template <int States>
-std::function<
-    Eigen::Matrix<double, States, 1>(const Eigen::Matrix<double, States, 1>&,
-                                     const Eigen::Matrix<double, States, 1>&)>
+std::function<Eigen::Vector<double, States>(
+    const Eigen::Vector<double, States>&, const Eigen::Vector<double, States>&)>
 AngleAdd(int angleStateIdx) {
   return [=](auto a, auto b) { return AngleAdd<States>(a, b, angleStateIdx); };
 }
@@ -86,9 +84,9 @@ AngleAdd(int angleStateIdx) {
  * @param angleStateIdx The row containing the angles.
  */
 template <int CovDim, int States>
-Eigen::Matrix<double, CovDim, 1> AngleMean(
+Eigen::Vector<double, CovDim> AngleMean(
     const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
-    const Eigen::Matrix<double, 2 * States + 1, 1>& Wm, int angleStatesIdx) {
+    const Eigen::Vector<double, 2 * States + 1>& Wm, int angleStatesIdx) {
   double sumSin = sigmas.row(angleStatesIdx)
                       .unaryExpr([](auto it) { return std::sin(it); })
                       .sum();
@@ -96,7 +94,7 @@ Eigen::Matrix<double, CovDim, 1> AngleMean(
                       .unaryExpr([](auto it) { return std::cos(it); })
                       .sum();
 
-  Eigen::Matrix<double, CovDim, 1> ret = sigmas * Wm;
+  Eigen::Vector<double, CovDim> ret = sigmas * Wm;
   ret[angleStatesIdx] = std::atan2(sumSin, sumCos);
   return ret;
 }
@@ -108,9 +106,9 @@ Eigen::Matrix<double, CovDim, 1> AngleMean(
  * @param angleStateIdx The row containing the angles.
  */
 template <int CovDim, int States>
-std::function<Eigen::Matrix<double, CovDim, 1>(
+std::function<Eigen::Vector<double, CovDim>(
     const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
-    const Eigen::Matrix<double, 2 * States + 1, 1>&)>
+    const Eigen::Vector<double, 2 * States + 1>&)>
 AngleMean(int angleStateIdx) {
   return [=](auto sigmas, auto Wm) {
     return AngleMean<CovDim, States>(sigmas, Wm, angleStateIdx);
diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
index e59b162e64..4c875b0803 100644
--- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
@@ -209,8 +209,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
   UnscentedKalmanFilter<5, 3, 3> m_observer;
   KalmanFilterLatencyCompensator<5, 3, 3, UnscentedKalmanFilter<5, 3, 3>>
       m_latencyCompensator;
-  std::function<void(const Eigen::Matrix<double, 3, 1>& u,
-                     const Eigen::Matrix<double, 3, 1>& y)>
+  std::function<void(const Eigen::Vector<double, 3>& u,
+                     const Eigen::Vector<double, 3>& y)>
       m_visionCorrect;
 
   Eigen::Matrix<double, 3, 3> m_visionContR;
@@ -223,13 +223,13 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
 
   template <int Dim>
   static wpi::array<double, Dim> StdDevMatrixToArray(
-      const Eigen::Matrix<double, Dim, 1>& stdDevs);
+      const Eigen::Vector<double, Dim>& stdDevs);
 
-  static Eigen::Matrix<double, 5, 1> F(const Eigen::Matrix<double, 5, 1>& x,
-                                       const Eigen::Matrix<double, 3, 1>& u);
-  static Eigen::Matrix<double, 5, 1> FillStateVector(
-      const Pose2d& pose, units::meter_t leftDistance,
-      units::meter_t rightDistance);
+  static Eigen::Vector<double, 5> F(const Eigen::Vector<double, 5>& x,
+                                    const Eigen::Vector<double, 3>& u);
+  static Eigen::Vector<double, 5> FillStateVector(const Pose2d& pose,
+                                                  units::meter_t leftDistance,
+                                                  units::meter_t rightDistance);
 };
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
index 26a8fd4370..985f2c14d9 100644
--- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
@@ -33,13 +33,13 @@ class ExtendedKalmanFilter {
    * @param measurementStdDevs Standard deviations of measurements.
    * @param dt                 Nominal discretization timestep.
    */
-  ExtendedKalmanFilter(std::function<Eigen::Matrix<double, States, 1>(
-                           const Eigen::Matrix<double, States, 1>&,
-                           const Eigen::Matrix<double, Inputs, 1>&)>
+  ExtendedKalmanFilter(std::function<Eigen::Vector<double, States>(
+                           const Eigen::Vector<double, States>&,
+                           const Eigen::Vector<double, Inputs>&)>
                            f,
-                       std::function<Eigen::Matrix<double, Outputs, 1>(
-                           const Eigen::Matrix<double, States, 1>&,
-                           const Eigen::Matrix<double, Inputs, 1>&)>
+                       std::function<Eigen::Vector<double, Outputs>(
+                           const Eigen::Vector<double, States>&,
+                           const Eigen::Vector<double, Inputs>&)>
                            h,
                        const wpi::array<double, States>& stateStdDevs,
                        const wpi::array<double, Outputs>& measurementStdDevs,
@@ -47,10 +47,10 @@ class ExtendedKalmanFilter {
       : m_f(f), m_h(h) {
     m_contQ = MakeCovMatrix(stateStdDevs);
     m_contR = MakeCovMatrix(measurementStdDevs);
-    m_residualFuncY = [](auto a, auto b) -> Eigen::Matrix<double, Outputs, 1> {
+    m_residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Outputs> {
       return a - b;
     };
-    m_addFuncX = [](auto a, auto b) -> Eigen::Matrix<double, States, 1> {
+    m_addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
       return a + b;
     };
     m_dt = dt;
@@ -59,10 +59,10 @@ class ExtendedKalmanFilter {
 
     Eigen::Matrix<double, States, States> contA =
         NumericalJacobianX<States, States, Inputs>(
-            m_f, m_xHat, Eigen::Matrix<double, Inputs, 1>::Zero());
+            m_f, m_xHat, Eigen::Vector<double, Inputs>::Zero());
     Eigen::Matrix<double, Outputs, States> C =
         NumericalJacobianX<Outputs, States, Inputs>(
-            m_h, m_xHat, Eigen::Matrix<double, Inputs, 1>::Zero());
+            m_h, m_xHat, Eigen::Vector<double, Inputs>::Zero());
 
     Eigen::Matrix<double, States, States> discA;
     Eigen::Matrix<double, States, States> discQ;
@@ -97,23 +97,23 @@ class ExtendedKalmanFilter {
    * @param addFuncX           A function that adds two state vectors.
    * @param dt                 Nominal discretization timestep.
    */
-  ExtendedKalmanFilter(std::function<Eigen::Matrix<double, States, 1>(
-                           const Eigen::Matrix<double, States, 1>&,
-                           const Eigen::Matrix<double, Inputs, 1>&)>
+  ExtendedKalmanFilter(std::function<Eigen::Vector<double, States>(
+                           const Eigen::Vector<double, States>&,
+                           const Eigen::Vector<double, Inputs>&)>
                            f,
-                       std::function<Eigen::Matrix<double, Outputs, 1>(
-                           const Eigen::Matrix<double, States, 1>&,
-                           const Eigen::Matrix<double, Inputs, 1>&)>
+                       std::function<Eigen::Vector<double, Outputs>(
+                           const Eigen::Vector<double, States>&,
+                           const Eigen::Vector<double, Inputs>&)>
                            h,
                        const wpi::array<double, States>& stateStdDevs,
                        const wpi::array<double, Outputs>& measurementStdDevs,
-                       std::function<Eigen::Matrix<double, Outputs, 1>(
-                           const Eigen::Matrix<double, Outputs, 1>&,
-                           const Eigen::Matrix<double, Outputs, 1>&)>
+                       std::function<Eigen::Vector<double, Outputs>(
+                           const Eigen::Vector<double, Outputs>&,
+                           const Eigen::Vector<double, Outputs>&)>
                            residualFuncY,
-                       std::function<Eigen::Matrix<double, States, 1>(
-                           const Eigen::Matrix<double, States, 1>&,
-                           const Eigen::Matrix<double, States, 1>&)>
+                       std::function<Eigen::Vector<double, States>(
+                           const Eigen::Vector<double, States>&,
+                           const Eigen::Vector<double, States>&)>
                            addFuncX,
                        units::second_t dt)
       : m_f(f), m_h(h), m_residualFuncY(residualFuncY), m_addFuncX(addFuncX) {
@@ -125,10 +125,10 @@ class ExtendedKalmanFilter {
 
     Eigen::Matrix<double, States, States> contA =
         NumericalJacobianX<States, States, Inputs>(
-            m_f, m_xHat, Eigen::Matrix<double, Inputs, 1>::Zero());
+            m_f, m_xHat, Eigen::Vector<double, Inputs>::Zero());
     Eigen::Matrix<double, Outputs, States> C =
         NumericalJacobianX<Outputs, States, Inputs>(
-            m_h, m_xHat, Eigen::Matrix<double, Inputs, 1>::Zero());
+            m_h, m_xHat, Eigen::Vector<double, Inputs>::Zero());
 
     Eigen::Matrix<double, States, States> discA;
     Eigen::Matrix<double, States, States> discQ;
@@ -172,7 +172,7 @@ class ExtendedKalmanFilter {
   /**
    * Returns the state estimate x-hat.
    */
-  const Eigen::Matrix<double, States, 1>& Xhat() const { return m_xHat; }
+  const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
 
   /**
    * Returns an element of the state estimate x-hat.
@@ -186,7 +186,7 @@ class ExtendedKalmanFilter {
    *
    * @param xHat The state estimate x-hat.
    */
-  void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) { m_xHat = xHat; }
+  void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
 
   /**
    * Set an element of the initial state estimate x-hat.
@@ -210,7 +210,7 @@ class ExtendedKalmanFilter {
    * @param u  New control input from controller.
    * @param dt Timestep for prediction.
    */
-  void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
+  void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
     // Find continuous A
     Eigen::Matrix<double, States, States> contA =
         NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
@@ -234,23 +234,23 @@ class ExtendedKalmanFilter {
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    */
-  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
-               const Eigen::Matrix<double, Outputs, 1>& y) {
+  void Correct(const Eigen::Vector<double, Inputs>& u,
+               const Eigen::Vector<double, Outputs>& y) {
     Correct<Outputs>(u, y, m_h, m_contR, m_residualFuncY, m_addFuncX);
   }
 
   template <int Rows>
-  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
-               const Eigen::Matrix<double, Rows, 1>& y,
-               std::function<Eigen::Matrix<double, Rows, 1>(
-                   const Eigen::Matrix<double, States, 1>&,
-                   const Eigen::Matrix<double, Inputs, 1>&)>
+  void Correct(const Eigen::Vector<double, Inputs>& u,
+               const Eigen::Vector<double, Rows>& y,
+               std::function<Eigen::Vector<double, Rows>(
+                   const Eigen::Vector<double, States>&,
+                   const Eigen::Vector<double, Inputs>&)>
                    h,
                const Eigen::Matrix<double, Rows, Rows>& R) {
-    auto residualFuncY = [](auto a, auto b) -> Eigen::Matrix<double, Rows, 1> {
+    auto residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Rows> {
       return a - b;
     };
-    auto addFuncX = [](auto a, auto b) -> Eigen::Matrix<double, States, 1> {
+    auto addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
       return a + b;
     };
     Correct<Rows>(u, y, h, R, residualFuncY, addFuncX);
@@ -273,20 +273,20 @@ class ExtendedKalmanFilter {
    * @param addFuncX      A function that adds two state vectors.
    */
   template <int Rows>
-  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
-               const Eigen::Matrix<double, Rows, 1>& y,
-               std::function<Eigen::Matrix<double, Rows, 1>(
-                   const Eigen::Matrix<double, States, 1>&,
-                   const Eigen::Matrix<double, Inputs, 1>&)>
+  void Correct(const Eigen::Vector<double, Inputs>& u,
+               const Eigen::Vector<double, Rows>& y,
+               std::function<Eigen::Vector<double, Rows>(
+                   const Eigen::Vector<double, States>&,
+                   const Eigen::Vector<double, Inputs>&)>
                    h,
                const Eigen::Matrix<double, Rows, Rows>& R,
-               std::function<Eigen::Matrix<double, Rows, 1>(
-                   const Eigen::Matrix<double, Rows, 1>&,
-                   const Eigen::Matrix<double, Rows, 1>&)>
+               std::function<Eigen::Vector<double, Rows>(
+                   const Eigen::Vector<double, Rows>&,
+                   const Eigen::Vector<double, Rows>&)>
                    residualFuncY,
-               std::function<Eigen::Matrix<double, States, 1>(
-                   const Eigen::Matrix<double, States, 1>&,
-                   const Eigen::Matrix<double, States, 1>)>
+               std::function<Eigen::Vector<double, States>(
+                   const Eigen::Vector<double, States>&,
+                   const Eigen::Vector<double, States>)>
                    addFuncX) {
     const Eigen::Matrix<double, Rows, States> C =
         NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
@@ -317,23 +317,23 @@ class ExtendedKalmanFilter {
   }
 
  private:
-  std::function<Eigen::Matrix<double, States, 1>(
-      const Eigen::Matrix<double, States, 1>&,
-      const Eigen::Matrix<double, Inputs, 1>&)>
+  std::function<Eigen::Vector<double, States>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, Inputs>&)>
       m_f;
-  std::function<Eigen::Matrix<double, Outputs, 1>(
-      const Eigen::Matrix<double, States, 1>&,
-      const Eigen::Matrix<double, Inputs, 1>&)>
+  std::function<Eigen::Vector<double, Outputs>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, Inputs>&)>
       m_h;
-  std::function<Eigen::Matrix<double, Outputs, 1>(
-      const Eigen::Matrix<double, Outputs, 1>&,
-      const Eigen::Matrix<double, Outputs, 1>)>
+  std::function<Eigen::Vector<double, Outputs>(
+      const Eigen::Vector<double, Outputs>&,
+      const Eigen::Vector<double, Outputs>)>
       m_residualFuncY;
-  std::function<Eigen::Matrix<double, States, 1>(
-      const Eigen::Matrix<double, States, 1>&,
-      const Eigen::Matrix<double, States, 1>)>
+  std::function<Eigen::Vector<double, States>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, States>)>
       m_addFuncX;
-  Eigen::Matrix<double, States, 1> m_xHat;
+  Eigen::Vector<double, States> m_xHat;
   Eigen::Matrix<double, States, States> m_P;
   Eigen::Matrix<double, States, States> m_contQ;
   Eigen::Matrix<double, Outputs, Outputs> m_contR;
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
index 341e43d9bf..363753e5a3 100644
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
@@ -118,7 +118,7 @@ class KalmanFilterImpl {
   /**
    * Returns the state estimate x-hat.
    */
-  const Eigen::Matrix<double, States, 1>& Xhat() const { return m_xHat; }
+  const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
 
   /**
    * Returns an element of the state estimate x-hat.
@@ -132,7 +132,7 @@ class KalmanFilterImpl {
    *
    * @param xHat The state estimate x-hat.
    */
-  void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) { m_xHat = xHat; }
+  void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
 
   /**
    * Set an element of the initial state estimate x-hat.
@@ -153,7 +153,7 @@ class KalmanFilterImpl {
    * @param u  New control input from controller.
    * @param dt Timestep for prediction.
    */
-  void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
+  void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
     m_xHat = m_plant->CalculateX(m_xHat, u, dt);
   }
 
@@ -163,8 +163,8 @@ class KalmanFilterImpl {
    * @param u Same control input used in the last predict step.
    * @param y Measurement vector.
    */
-  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
-               const Eigen::Matrix<double, Outputs, 1>& y) {
+  void Correct(const Eigen::Vector<double, Inputs>& u,
+               const Eigen::Vector<double, Outputs>& y) {
     // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
     m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u));
   }
@@ -180,7 +180,7 @@ class KalmanFilterImpl {
   /**
    * The state estimate.
    */
-  Eigen::Matrix<double, States, 1> m_xHat;
+  Eigen::Vector<double, States> m_xHat;
 };
 
 }  // namespace detail
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
index f7e5588c8b..aabb8ec800 100644
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
@@ -20,14 +20,14 @@ template <int States, int Inputs, int Outputs, typename KalmanFilterType>
 class KalmanFilterLatencyCompensator {
  public:
   struct ObserverSnapshot {
-    Eigen::Matrix<double, States, 1> xHat;
+    Eigen::Vector<double, States> xHat;
     Eigen::Matrix<double, States, States> errorCovariances;
-    Eigen::Matrix<double, Inputs, 1> inputs;
-    Eigen::Matrix<double, Outputs, 1> localMeasurements;
+    Eigen::Vector<double, Inputs> inputs;
+    Eigen::Vector<double, Outputs> localMeasurements;
 
     ObserverSnapshot(const KalmanFilterType& observer,
-                     const Eigen::Matrix<double, Inputs, 1>& u,
-                     const Eigen::Matrix<double, Outputs, 1>& localY)
+                     const Eigen::Vector<double, Inputs>& u,
+                     const Eigen::Vector<double, Outputs>& localY)
         : xHat(observer.Xhat()),
           errorCovariances(observer.P()),
           inputs(u),
@@ -48,8 +48,8 @@ class KalmanFilterLatencyCompensator {
    * @param timestamp The timesnap of the state.
    */
   void AddObserverState(const KalmanFilterType& observer,
-                        Eigen::Matrix<double, Inputs, 1> u,
-                        Eigen::Matrix<double, Outputs, 1> localY,
+                        Eigen::Vector<double, Inputs> u,
+                        Eigen::Vector<double, Outputs> localY,
                         units::second_t timestamp) {
     // Add the new state into the vector.
     m_pastObserverSnapshots.emplace_back(timestamp,
@@ -75,9 +75,9 @@ class KalmanFilterLatencyCompensator {
   template <int Rows>
   void ApplyPastGlobalMeasurement(
       KalmanFilterType* observer, units::second_t nominalDt,
-      Eigen::Matrix<double, Rows, 1> y,
-      std::function<void(const Eigen::Matrix<double, Inputs, 1>& u,
-                         const Eigen::Matrix<double, Rows, 1>& y)>
+      Eigen::Vector<double, Rows> y,
+      std::function<void(const Eigen::Vector<double, Inputs>& u,
+                         const Eigen::Vector<double, Rows>& y)>
           globalMeasurementCorrect,
       units::second_t timestamp) {
     if (m_pastObserverSnapshots.size() == 0) {
diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
index fc58a6acfb..40d7e28436 100644
--- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
@@ -205,8 +205,8 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
   MecanumDriveKinematics m_kinematics;
   KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>>
       m_latencyCompensator;
-  std::function<void(const Eigen::Matrix<double, 3, 1>& u,
-                     const Eigen::Matrix<double, 3, 1>& y)>
+  std::function<void(const Eigen::Vector<double, 3>& u,
+                     const Eigen::Vector<double, 3>& y)>
       m_visionCorrect;
 
   Eigen::Matrix3d m_visionContR;
@@ -219,7 +219,7 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
 
   template <int Dim>
   static wpi::array<double, Dim> StdDevMatrixToArray(
-      const Eigen::Matrix<double, Dim, 1>& vector) {
+      const Eigen::Vector<double, Dim>& vector) {
     wpi::array<double, Dim> array;
     for (size_t i = 0; i < Dim; ++i) {
       array[i] = vector(i);
diff --git a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
index 543f7a9af0..b521ed1f38 100644
--- a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
+++ b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
@@ -63,7 +63,7 @@ class MerweScaledSigmaPoints {
    *
    */
   Eigen::Matrix<double, States, 2 * States + 1> SigmaPoints(
-      const Eigen::Matrix<double, States, 1>& x,
+      const Eigen::Vector<double, States>& x,
       const Eigen::Matrix<double, States, States>& P) {
     double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
     Eigen::Matrix<double, States, States> U =
@@ -84,7 +84,7 @@ class MerweScaledSigmaPoints {
   /**
    * Returns the weight for each sigma point for the mean.
    */
-  const Eigen::Matrix<double, 2 * States + 1, 1>& Wm() const { return m_Wm; }
+  const Eigen::Vector<double, 2 * States + 1>& Wm() const { return m_Wm; }
 
   /**
    * Returns an element of the weight for each sigma point for the mean.
@@ -96,7 +96,7 @@ class MerweScaledSigmaPoints {
   /**
    * Returns the weight for each sigma point for the covariance.
    */
-  const Eigen::Matrix<double, 2 * States + 1, 1>& Wc() const { return m_Wc; }
+  const Eigen::Vector<double, 2 * States + 1>& Wc() const { return m_Wc; }
 
   /**
    * Returns an element of the weight for each sigma point for the covariance.
@@ -106,8 +106,8 @@ class MerweScaledSigmaPoints {
   double Wc(int i) const { return m_Wc(i, 0); }
 
  private:
-  Eigen::Matrix<double, 2 * States + 1, 1> m_Wm;
-  Eigen::Matrix<double, 2 * States + 1, 1> m_Wc;
+  Eigen::Vector<double, 2 * States + 1> m_Wm;
+  Eigen::Vector<double, 2 * States + 1> m_Wc;
   double m_alpha;
   int m_kappa;
 
@@ -120,8 +120,8 @@ class MerweScaledSigmaPoints {
     double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
 
     double c = 0.5 / (States + lambda);
-    m_Wm = Eigen::Matrix<double, 2 * States + 1, 1>::Constant(c);
-    m_Wc = Eigen::Matrix<double, 2 * States + 1, 1>::Constant(c);
+    m_Wm = Eigen::Vector<double, 2 * States + 1>::Constant(c);
+    m_Wc = Eigen::Vector<double, 2 * States + 1>::Constant(c);
 
     m_Wm(0) = lambda / (States + lambda);
     m_Wc(0) = lambda / (States + lambda) + (1 - std::pow(m_alpha, 2) + beta);
diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
index d4a1c3121f..67c955b3a4 100644
--- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
@@ -80,10 +80,10 @@ class SwerveDrivePoseEstimator {
       const wpi::array<double, 1>& localMeasurementStdDevs,
       const wpi::array<double, 3>& visionMeasurementStdDevs,
       units::second_t nominalDt = 0.02_s)
-      : m_observer([](const Eigen::Matrix<double, 3, 1>& x,
-                      const Eigen::Matrix<double, 3, 1>& u) { return u; },
-                   [](const Eigen::Matrix<double, 3, 1>& x,
-                      const Eigen::Matrix<double, 3, 1>& u) {
+      : m_observer([](const Eigen::Vector<double, 3>& x,
+                      const Eigen::Vector<double, 3>& u) { return u; },
+                   [](const Eigen::Vector<double, 3>& x,
+                      const Eigen::Vector<double, 3>& u) {
                      return x.block<1, 1>(2, 0);
                    },
                    stateStdDevs, localMeasurementStdDevs,
@@ -95,12 +95,12 @@ class SwerveDrivePoseEstimator {
     SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
 
     // Create correction mechanism for vision measurements.
-    m_visionCorrect = [&](const Eigen::Matrix<double, 3, 1>& u,
-                          const Eigen::Matrix<double, 3, 1>& y) {
+    m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
+                          const Eigen::Vector<double, 3>& y) {
       m_observer.Correct<3>(
           u, y,
-          [](const Eigen::Matrix<double, 3, 1>& x,
-             const Eigen::Matrix<double, 3, 1>& u) { return x; },
+          [](const Eigen::Vector<double, 3>& x,
+             const Eigen::Vector<double, 3>& u) { return x; },
           m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
           frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
     };
@@ -269,12 +269,11 @@ class SwerveDrivePoseEstimator {
         Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
             .RotateBy(angle);
 
-    auto u =
-        frc::MakeMatrix<3, 1>(fieldRelativeSpeeds.X().template to<double>(),
-                              fieldRelativeSpeeds.Y().template to<double>(),
-                              omega.template to<double>());
+    Eigen::Vector<double, 3> u{fieldRelativeSpeeds.X().template to<double>(),
+                               fieldRelativeSpeeds.Y().template to<double>(),
+                               omega.template to<double>()};
 
-    auto localY = frc::MakeMatrix<1, 1>(angle.Radians().template to<double>());
+    Eigen::Vector<double, 1> localY{angle.Radians().template to<double>()};
     m_previousAngle = angle;
 
     m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
@@ -290,8 +289,8 @@ class SwerveDrivePoseEstimator {
   SwerveDriveKinematics<NumModules>& m_kinematics;
   KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>>
       m_latencyCompensator;
-  std::function<void(const Eigen::Matrix<double, 3, 1>& u,
-                     const Eigen::Matrix<double, 3, 1>& y)>
+  std::function<void(const Eigen::Vector<double, 3>& u,
+                     const Eigen::Vector<double, 3>& y)>
       m_visionCorrect;
 
   Eigen::Matrix3d m_visionContR;
@@ -304,7 +303,7 @@ class SwerveDrivePoseEstimator {
 
   template <int Dim>
   static wpi::array<double, Dim> StdDevMatrixToArray(
-      const Eigen::Matrix<double, Dim, 1>& vector) {
+      const Eigen::Vector<double, Dim>& vector) {
     wpi::array<double, Dim> array;
     for (size_t i = 0; i < Dim; ++i) {
       array[i] = vector(i);
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
index 75a11a60dd..53972a8193 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
@@ -34,13 +34,13 @@ class UnscentedKalmanFilter {
    * @param measurementStdDevs Standard deviations of measurements.
    * @param dt                 Nominal discretization timestep.
    */
-  UnscentedKalmanFilter(std::function<Eigen::Matrix<double, States, 1>(
-                            const Eigen::Matrix<double, States, 1>&,
-                            const Eigen::Matrix<double, Inputs, 1>&)>
+  UnscentedKalmanFilter(std::function<Eigen::Vector<double, States>(
+                            const Eigen::Vector<double, States>&,
+                            const Eigen::Vector<double, Inputs>&)>
                             f,
-                        std::function<Eigen::Matrix<double, Outputs, 1>(
-                            const Eigen::Matrix<double, States, 1>&,
-                            const Eigen::Matrix<double, Inputs, 1>&)>
+                        std::function<Eigen::Vector<double, Outputs>(
+                            const Eigen::Vector<double, States>&,
+                            const Eigen::Vector<double, Inputs>&)>
                             h,
                         const wpi::array<double, States>& stateStdDevs,
                         const wpi::array<double, Outputs>& measurementStdDevs,
@@ -48,20 +48,19 @@ class UnscentedKalmanFilter {
       : m_f(f), m_h(h) {
     m_contQ = MakeCovMatrix(stateStdDevs);
     m_contR = MakeCovMatrix(measurementStdDevs);
-    m_meanFuncX = [](auto sigmas, auto Wm) -> Eigen::Matrix<double, States, 1> {
+    m_meanFuncX = [](auto sigmas, auto Wm) -> Eigen::Vector<double, States> {
       return sigmas * Wm;
     };
-    m_meanFuncY = [](auto sigmas,
-                     auto Wc) -> Eigen::Matrix<double, Outputs, 1> {
+    m_meanFuncY = [](auto sigmas, auto Wc) -> Eigen::Vector<double, Outputs> {
       return sigmas * Wc;
     };
-    m_residualFuncX = [](auto a, auto b) -> Eigen::Matrix<double, States, 1> {
+    m_residualFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
       return a - b;
     };
-    m_residualFuncY = [](auto a, auto b) -> Eigen::Matrix<double, Outputs, 1> {
+    m_residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Outputs> {
       return a - b;
     };
-    m_addFuncX = [](auto a, auto b) -> Eigen::Matrix<double, States, 1> {
+    m_addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
       return a + b;
     };
     m_dt = dt;
@@ -94,35 +93,35 @@ class UnscentedKalmanFilter {
    * @param dt                 Nominal discretization timestep.
    */
   UnscentedKalmanFilter(
-      std::function<Eigen::Matrix<double, States, 1>(
-          const Eigen::Matrix<double, States, 1>&,
-          const Eigen::Matrix<double, Inputs, 1>&)>
+      std::function<
+          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
+                                        const Eigen::Vector<double, Inputs>&)>
           f,
-      std::function<Eigen::Matrix<double, Outputs, 1>(
-          const Eigen::Matrix<double, States, 1>&,
-          const Eigen::Matrix<double, Inputs, 1>&)>
+      std::function<
+          Eigen::Vector<double, Outputs>(const Eigen::Vector<double, States>&,
+                                         const Eigen::Vector<double, Inputs>&)>
           h,
       const wpi::array<double, States>& stateStdDevs,
       const wpi::array<double, Outputs>& measurementStdDevs,
-      std::function<Eigen::Matrix<double, States, 1>(
+      std::function<Eigen::Vector<double, States>(
           const Eigen::Matrix<double, States, 2 * States + 1>&,
-          const Eigen::Matrix<double, 2 * States + 1, 1>&)>
+          const Eigen::Vector<double, 2 * States + 1>&)>
           meanFuncX,
-      std::function<Eigen::Matrix<double, Outputs, 1>(
+      std::function<Eigen::Vector<double, Outputs>(
           const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
-          const Eigen::Matrix<double, 2 * States + 1, 1>&)>
+          const Eigen::Vector<double, 2 * States + 1>&)>
           meanFuncY,
-      std::function<Eigen::Matrix<double, States, 1>(
-          const Eigen::Matrix<double, States, 1>&,
-          const Eigen::Matrix<double, States, 1>&)>
+      std::function<
+          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
+                                        const Eigen::Vector<double, States>&)>
           residualFuncX,
-      std::function<Eigen::Matrix<double, Outputs, 1>(
-          const Eigen::Matrix<double, Outputs, 1>&,
-          const Eigen::Matrix<double, Outputs, 1>&)>
+      std::function<
+          Eigen::Vector<double, Outputs>(const Eigen::Vector<double, Outputs>&,
+                                         const Eigen::Vector<double, Outputs>&)>
           residualFuncY,
-      std::function<Eigen::Matrix<double, States, 1>(
-          const Eigen::Matrix<double, States, 1>&,
-          const Eigen::Matrix<double, States, 1>&)>
+      std::function<
+          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
+                                        const Eigen::Vector<double, States>&)>
           addFuncX,
       units::second_t dt)
       : m_f(f),
@@ -162,7 +161,7 @@ class UnscentedKalmanFilter {
   /**
    * Returns the state estimate x-hat.
    */
-  const Eigen::Matrix<double, States, 1>& Xhat() const { return m_xHat; }
+  const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
 
   /**
    * Returns an element of the state estimate x-hat.
@@ -176,7 +175,7 @@ class UnscentedKalmanFilter {
    *
    * @param xHat The state estimate x-hat.
    */
-  void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) { m_xHat = xHat; }
+  void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
 
   /**
    * Set an element of the initial state estimate x-hat.
@@ -201,7 +200,7 @@ class UnscentedKalmanFilter {
    * @param u  New control input from controller.
    * @param dt Timestep for prediction.
    */
-  void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
+  void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
     m_dt = dt;
 
     // Discretize Q before projecting mean and covariance forward
@@ -215,8 +214,7 @@ class UnscentedKalmanFilter {
         m_pts.SigmaPoints(m_xHat, m_P);
 
     for (int i = 0; i < m_pts.NumSigmas(); ++i) {
-      Eigen::Matrix<double, States, 1> x =
-          sigmas.template block<States, 1>(0, i);
+      Eigen::Vector<double, States> x = sigmas.template block<States, 1>(0, i);
       m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
     }
 
@@ -234,8 +232,8 @@ class UnscentedKalmanFilter {
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    */
-  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
-               const Eigen::Matrix<double, Outputs, 1>& y) {
+  void Correct(const Eigen::Vector<double, Inputs>& u,
+               const Eigen::Vector<double, Outputs>& y) {
     Correct<Outputs>(u, y, m_h, m_contR, m_meanFuncY, m_residualFuncY,
                      m_residualFuncX, m_addFuncX);
   }
@@ -254,25 +252,23 @@ class UnscentedKalmanFilter {
    * @param R Measurement noise covariance matrix (continuous-time).
    */
   template <int Rows>
-  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
-               const Eigen::Matrix<double, Rows, 1>& y,
-               std::function<Eigen::Matrix<double, Rows, 1>(
-                   const Eigen::Matrix<double, States, 1>&,
-                   const Eigen::Matrix<double, Inputs, 1>&)>
+  void Correct(const Eigen::Vector<double, Inputs>& u,
+               const Eigen::Vector<double, Rows>& y,
+               std::function<Eigen::Vector<double, Rows>(
+                   const Eigen::Vector<double, States>&,
+                   const Eigen::Vector<double, Inputs>&)>
                    h,
                const Eigen::Matrix<double, Rows, Rows>& R) {
-    auto meanFuncY = [](auto sigmas,
-                        auto Wc) -> Eigen::Matrix<double, Rows, 1> {
+    auto meanFuncY = [](auto sigmas, auto Wc) -> Eigen::Vector<double, Rows> {
       return sigmas * Wc;
     };
-    auto residualFuncX = [](auto a,
-                            auto b) -> Eigen::Matrix<double, States, 1> {
+    auto residualFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
       return a - b;
     };
-    auto residualFuncY = [](auto a, auto b) -> Eigen::Matrix<double, Rows, 1> {
+    auto residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Rows> {
       return a - b;
     };
-    auto addFuncX = [](auto a, auto b) -> Eigen::Matrix<double, States, 1> {
+    auto addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
       return a + b;
     };
     Correct<Rows>(u, y, h, R, meanFuncY, residualFuncY, residualFuncX,
@@ -300,28 +296,28 @@ class UnscentedKalmanFilter {
    * @param addFuncX      A function that adds two state vectors.
    */
   template <int Rows>
-  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
-               const Eigen::Matrix<double, Rows, 1>& y,
-               std::function<Eigen::Matrix<double, Rows, 1>(
-                   const Eigen::Matrix<double, States, 1>&,
-                   const Eigen::Matrix<double, Inputs, 1>&)>
+  void Correct(const Eigen::Vector<double, Inputs>& u,
+               const Eigen::Vector<double, Rows>& y,
+               std::function<Eigen::Vector<double, Rows>(
+                   const Eigen::Vector<double, States>&,
+                   const Eigen::Vector<double, Inputs>&)>
                    h,
                const Eigen::Matrix<double, Rows, Rows>& R,
-               std::function<Eigen::Matrix<double, Rows, 1>(
+               std::function<Eigen::Vector<double, Rows>(
                    const Eigen::Matrix<double, Rows, 2 * States + 1>&,
-                   const Eigen::Matrix<double, 2 * States + 1, 1>&)>
+                   const Eigen::Vector<double, 2 * States + 1>&)>
                    meanFuncY,
-               std::function<Eigen::Matrix<double, Rows, 1>(
-                   const Eigen::Matrix<double, Rows, 1>&,
-                   const Eigen::Matrix<double, Rows, 1>&)>
+               std::function<Eigen::Vector<double, Rows>(
+                   const Eigen::Vector<double, Rows>&,
+                   const Eigen::Vector<double, Rows>&)>
                    residualFuncY,
-               std::function<Eigen::Matrix<double, States, 1>(
-                   const Eigen::Matrix<double, States, 1>&,
-                   const Eigen::Matrix<double, States, 1>&)>
+               std::function<Eigen::Vector<double, States>(
+                   const Eigen::Vector<double, States>&,
+                   const Eigen::Vector<double, States>&)>
                    residualFuncX,
-               std::function<Eigen::Matrix<double, States, 1>(
-                   const Eigen::Matrix<double, States, 1>&,
-                   const Eigen::Matrix<double, States, 1>)>
+               std::function<Eigen::Vector<double, States>(
+                   const Eigen::Vector<double, States>&,
+                   const Eigen::Vector<double, States>)>
                    addFuncX) {
     const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
 
@@ -367,35 +363,35 @@ class UnscentedKalmanFilter {
   }
 
  private:
-  std::function<Eigen::Matrix<double, States, 1>(
-      const Eigen::Matrix<double, States, 1>&,
-      const Eigen::Matrix<double, Inputs, 1>&)>
+  std::function<Eigen::Vector<double, States>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, Inputs>&)>
       m_f;
-  std::function<Eigen::Matrix<double, Outputs, 1>(
-      const Eigen::Matrix<double, States, 1>&,
-      const Eigen::Matrix<double, Inputs, 1>&)>
+  std::function<Eigen::Vector<double, Outputs>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, Inputs>&)>
       m_h;
-  std::function<Eigen::Matrix<double, States, 1>(
+  std::function<Eigen::Vector<double, States>(
       const Eigen::Matrix<double, States, 2 * States + 1>&,
-      const Eigen::Matrix<double, 2 * States + 1, 1>&)>
+      const Eigen::Vector<double, 2 * States + 1>&)>
       m_meanFuncX;
-  std::function<Eigen::Matrix<double, Outputs, 1>(
+  std::function<Eigen::Vector<double, Outputs>(
       const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
-      const Eigen::Matrix<double, 2 * States + 1, 1>&)>
+      const Eigen::Vector<double, 2 * States + 1>&)>
       m_meanFuncY;
-  std::function<Eigen::Matrix<double, States, 1>(
-      const Eigen::Matrix<double, States, 1>&,
-      const Eigen::Matrix<double, States, 1>&)>
+  std::function<Eigen::Vector<double, States>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, States>&)>
       m_residualFuncX;
-  std::function<Eigen::Matrix<double, Outputs, 1>(
-      const Eigen::Matrix<double, Outputs, 1>&,
-      const Eigen::Matrix<double, Outputs, 1>)>
+  std::function<Eigen::Vector<double, Outputs>(
+      const Eigen::Vector<double, Outputs>&,
+      const Eigen::Vector<double, Outputs>)>
       m_residualFuncY;
-  std::function<Eigen::Matrix<double, States, 1>(
-      const Eigen::Matrix<double, States, 1>&,
-      const Eigen::Matrix<double, States, 1>)>
+  std::function<Eigen::Vector<double, States>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, States>)>
       m_addFuncX;
-  Eigen::Matrix<double, States, 1> m_xHat;
+  Eigen::Vector<double, States> m_xHat;
   Eigen::Matrix<double, States, States> m_P;
   Eigen::Matrix<double, States, States> m_contQ;
   Eigen::Matrix<double, Outputs, Outputs> m_contR;
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
index 3e4592d2a8..b0b86f3d21 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
@@ -27,24 +27,23 @@ namespace frc {
  *         passing through the transform.
  */
 template <int States, int CovDim>
-std::tuple<Eigen::Matrix<double, CovDim, 1>,
-           Eigen::Matrix<double, CovDim, CovDim>>
+std::tuple<Eigen::Vector<double, CovDim>, Eigen::Matrix<double, CovDim, CovDim>>
 UnscentedTransform(const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
-                   const Eigen::Matrix<double, 2 * States + 1, 1>& Wm,
-                   const Eigen::Matrix<double, 2 * States + 1, 1>& Wc,
-                   std::function<Eigen::Matrix<double, CovDim, 1>(
+                   const Eigen::Vector<double, 2 * States + 1>& Wm,
+                   const Eigen::Vector<double, 2 * States + 1>& Wc,
+                   std::function<Eigen::Vector<double, CovDim>(
                        const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
-                       const Eigen::Matrix<double, 2 * States + 1, 1>&)>
+                       const Eigen::Vector<double, 2 * States + 1>&)>
                        meanFunc,
-                   std::function<Eigen::Matrix<double, CovDim, 1>(
-                       const Eigen::Matrix<double, CovDim, 1>&,
-                       const Eigen::Matrix<double, CovDim, 1>&)>
+                   std::function<Eigen::Vector<double, CovDim>(
+                       const Eigen::Vector<double, CovDim>&,
+                       const Eigen::Vector<double, CovDim>&)>
                        residualFunc) {
   // New mean is usually just the sum of the sigmas * weight:
   //       n
   // dot = Σ W[k] Xᵢ[k]
   //      k=1
-  Eigen::Matrix<double, CovDim, 1> x = meanFunc(sigmas, Wm);
+  Eigen::Vector<double, CovDim> x = meanFunc(sigmas, Wm);
 
   // New covariance is the sum of the outer product of the residuals times the
   // weights
diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
index e5b6f1db3e..b6ea33a817 100644
--- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h
+++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
@@ -219,12 +219,12 @@ class LinearFilter {
     }
 
     // Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta
-    Eigen::Matrix<double, Samples, 1> d;
+    Eigen::Vector<double, Samples> d;
     for (int i = 0; i < Samples; ++i) {
       d(i) = (i == Derivative) ? Factorial(Derivative) : 0.0;
     }
 
-    Eigen::Matrix<double, Samples, 1> a =
+    Eigen::Vector<double, Samples> a =
         S.householderQr().solve(d) / std::pow(period.to<double>(), Derivative);
 
     // Reverse gains list
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
index 374db562d7..8601adf097 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -24,27 +24,26 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
   if (centerOfRotation != m_previousCoR) {
     for (size_t i = 0; i < NumModules; i++) {
       // clang-format off
-      m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
-        1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).template to<double>(),
-        0, 1, (+m_modules[i].X() - centerOfRotation.X()).template to<double>();
+      m_inverseKinematics.template block<2, 3>(i * 2, 0) =
+        Eigen::Matrix<double, 2, 3>{
+          {1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).template to<double>()},
+          {0, 1, (+m_modules[i].X() - centerOfRotation.X()).template to<double>()}};
       // clang-format on
     }
     m_previousCoR = centerOfRotation;
   }
 
-  Eigen::Vector3d chassisSpeedsVector;
-  chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
-      chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
+  Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.to<double>(),
+                                      chassisSpeeds.vy.to<double>(),
+                                      chassisSpeeds.omega.to<double>()};
 
   Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix =
       m_inverseKinematics * chassisSpeedsVector;
   wpi::array<SwerveModuleState, NumModules> moduleStates{wpi::empty_array};
 
   for (size_t i = 0; i < NumModules; i++) {
-    units::meters_per_second_t x =
-        units::meters_per_second_t{moduleStatesMatrix(i * 2, 0)};
-    units::meters_per_second_t y =
-        units::meters_per_second_t{moduleStatesMatrix(i * 2 + 1, 0)};
+    units::meters_per_second_t x{moduleStatesMatrix(i * 2, 0)};
+    units::meters_per_second_t y{moduleStatesMatrix(i * 2 + 1, 0)};
 
     auto speed = units::math::hypot(x, y);
     Rotation2d rotation{x.to<double>(), y.to<double>()};
@@ -73,12 +72,12 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
     wpi::array<SwerveModuleState, NumModules> moduleStates) const {
   Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
 
-  for (size_t i = 0; i < NumModules; i++) {
+  for (size_t i = 0; i < NumModules; ++i) {
     SwerveModuleState module = moduleStates[i];
-    moduleStatesMatrix.row(i * 2)
-        << module.speed.to<double>() * module.angle.Cos();
-    moduleStatesMatrix.row(i * 2 + 1)
-        << module.speed.to<double>() * module.angle.Sin();
+    moduleStatesMatrix(i * 2, 0) =
+        module.speed.to<double>() * module.angle.Cos();
+    moduleStatesMatrix(i * 2 + 1, 0) =
+        module.speed.to<double>() * module.angle.Sin();
   }
 
   Eigen::Vector3d chassisSpeedsVector =
diff --git a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
index cfad20c3d5..8126349d8a 100644
--- a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
+++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
@@ -74,13 +74,10 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
     // [ a1 ] = [  0  1  0  0 ][ P(i+1)  ]
     // [ a0 ] = [  1  0  0  0 ][ P'(i+1) ]
 
-    // clang-format off
-    static auto basis = (Eigen::Matrix<double, 4, 4>() <<
-     +2.0, +1.0, -2.0, +1.0,
-     -3.0, -2.0, +3.0, -1.0,
-     +0.0, +1.0, +0.0, +0.0,
-     +1.0, +0.0, +0.0, +0.0).finished();
-    // clang-format on
+    static const Eigen::Matrix<double, 4, 4> basis{{+2.0, +1.0, -2.0, +1.0},
+                                                   {-3.0, -2.0, +3.0, -1.0},
+                                                   {+0.0, +1.0, +0.0, +0.0},
+                                                   {+1.0, +0.0, +0.0, +0.0}};
     return basis;
   }
 
@@ -95,9 +92,8 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
    */
   static Eigen::Vector4d ControlVectorFromArrays(
       wpi::array<double, 2> initialVector, wpi::array<double, 2> finalVector) {
-    return (Eigen::Vector4d() << initialVector[0], initialVector[1],
-            finalVector[0], finalVector[1])
-        .finished();
+    return Eigen::Vector4d{initialVector[0], initialVector[1], finalVector[0],
+                           finalVector[1]};
   }
 };
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
index 04efecde3a..5ba3e2a18f 100644
--- a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
+++ b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
@@ -81,15 +81,13 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
     // [ a1 ] = [   0.0   1.0   0.0   0.0   0.0   0.0 ][ P'(i+1)  ]
     // [ a0 ] = [   1.0   0.0   0.0   0.0   0.0   0.0 ][ P''(i+1) ]
 
-    // clang-format off
-    static const auto basis = (Eigen::Matrix<double, 6, 6>() <<
-      -06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
-      +15.0, +08.0, +01.5, -15.0, +07.0, -01.0,
-      -10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
-      +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
-      +00.0, +01.0, +00.0, +00.0, +00.0, +00.0,
-      +01.0, +00.0, +00.0, +00.0, +00.0, +00.0).finished();
-    // clang-format on
+    static const Eigen::Matrix<double, 6, 6> basis{
+        {-06.0, -03.0, -00.5, +06.0, -03.0, +00.5},
+        {+15.0, +08.0, +01.5, -15.0, +07.0, -01.0},
+        {-10.0, -06.0, -01.5, +10.0, -04.0, +00.5},
+        {+00.0, +00.0, +00.5, +00.0, +00.0, +00.0},
+        {+00.0, +01.0, +00.0, +00.0, +00.0, +00.0},
+        {+01.0, +00.0, +00.0, +00.0, +00.0, +00.0}};
     return basis;
   }
 
@@ -102,11 +100,11 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
    *
    * @return The control vector matrix for a dimension.
    */
-  static Eigen::Matrix<double, 6, 1> ControlVectorFromArrays(
+  static Eigen::Vector<double, 6> ControlVectorFromArrays(
       wpi::array<double, 3> initialVector, wpi::array<double, 3> finalVector) {
-    return (Eigen::Matrix<double, 6, 1>() << initialVector[0], initialVector[1],
-            initialVector[2], finalVector[0], finalVector[1], finalVector[2])
-        .finished();
+    return Eigen::Vector<double, 6>{initialVector[0], initialVector[1],
+                                    initialVector[2], finalVector[0],
+                                    finalVector[1],   finalVector[2]};
   }
 };
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/spline/Spline.h b/wpimath/src/main/native/include/frc/spline/Spline.h
index a04564ac4d..615dffd939 100644
--- a/wpimath/src/main/native/include/frc/spline/Spline.h
+++ b/wpimath/src/main/native/include/frc/spline/Spline.h
@@ -55,7 +55,7 @@ class Spline {
    * @return The pose and curvature at that point.
    */
   PoseWithCurvature GetPoint(double t) const {
-    Eigen::Matrix<double, Degree + 1, 1> polynomialBases;
+    Eigen::Vector<double, Degree + 1> polynomialBases;
 
     // Populate the polynomial bases
     for (int i = 0; i <= Degree; i++) {
@@ -64,7 +64,7 @@ class Spline {
 
     // This simply multiplies by the coefficients. We need to divide out t some
     // n number of times where n is the derivative we want to take.
-    Eigen::Matrix<double, 6, 1> combined = Coefficients() * polynomialBases;
+    Eigen::Vector<double, 6> combined = Coefficients() * polynomialBases;
 
     double dx, dy, ddx, ddy;
 
@@ -109,9 +109,8 @@ class Spline {
    * @return The vector.
    */
   static Eigen::Vector2d ToVector(const Translation2d& translation) {
-    return (Eigen::Vector2d() << translation.X().to<double>(),
-            translation.Y().to<double>())
-        .finished();
+    return Eigen::Vector2d{translation.X().to<double>(),
+                           translation.Y().to<double>()};
   }
 
   /**
diff --git a/wpimath/src/main/native/include/frc/system/LinearSystem.h b/wpimath/src/main/native/include/frc/system/LinearSystem.h
index 23ef462bd9..8b160a2997 100644
--- a/wpimath/src/main/native/include/frc/system/LinearSystem.h
+++ b/wpimath/src/main/native/include/frc/system/LinearSystem.h
@@ -110,10 +110,9 @@ class LinearSystem {
    * @param u  The control input.
    * @param dt Timestep for model update.
    */
-  Eigen::Matrix<double, States, 1> CalculateX(
-      const Eigen::Matrix<double, States, 1>& x,
-      const Eigen::Matrix<double, Inputs, 1>& clampedU,
-      units::second_t dt) const {
+  Eigen::Vector<double, States> CalculateX(
+      const Eigen::Vector<double, States>& x,
+      const Eigen::Vector<double, Inputs>& clampedU, units::second_t dt) const {
     Eigen::Matrix<double, States, States> discA;
     Eigen::Matrix<double, States, Inputs> discB;
     DiscretizeAB<States, Inputs>(m_A, m_B, dt, &discA, &discB);
@@ -130,9 +129,9 @@ class LinearSystem {
    * @param x The current state.
    * @param clampedU The control input.
    */
-  Eigen::Matrix<double, Outputs, 1> CalculateY(
-      const Eigen::Matrix<double, States, 1>& x,
-      const Eigen::Matrix<double, Inputs, 1>& clampedU) const {
+  Eigen::Vector<double, Outputs> CalculateY(
+      const Eigen::Vector<double, States>& x,
+      const Eigen::Vector<double, Inputs>& clampedU) const {
     return m_C * x + m_D * clampedU;
   }
 
diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
index a6ec2a7e39..9c5a01be18 100644
--- a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
+++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
@@ -48,7 +48,7 @@ class LinearSystemLoop {
                    units::volt_t maxVoltage, units::second_t dt)
       : LinearSystemLoop(
             plant, controller, observer,
-            [=](Eigen::Matrix<double, Inputs, 1> u) {
+            [=](const Eigen::Vector<double, Inputs>& u) {
               return frc::NormalizeInputVector<Inputs>(
                   u, maxVoltage.template to<double>());
             },
@@ -69,8 +69,8 @@ class LinearSystemLoop {
   LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
                    LinearQuadraticRegulator<States, Inputs>& controller,
                    KalmanFilter<States, Inputs, Outputs>& observer,
-                   std::function<Eigen::Matrix<double, Inputs, 1>(
-                       const Eigen::Matrix<double, Inputs, 1>&)>
+                   std::function<Eigen::Vector<double, Inputs>(
+                       const Eigen::Vector<double, Inputs>&)>
                        clampFunction,
                    units::second_t dt)
       : LinearSystemLoop(
@@ -94,7 +94,7 @@ class LinearSystemLoop {
       const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
       KalmanFilter<States, Inputs, Outputs>& observer, units::volt_t maxVoltage)
       : LinearSystemLoop(controller, feedforward, observer,
-                         [=](Eigen::Matrix<double, Inputs, 1> u) {
+                         [=](const Eigen::Vector<double, Inputs>& u) {
                            return frc::NormalizeInputVector<Inputs>(
                                u, maxVoltage.template to<double>());
                          }) {}
@@ -113,8 +113,8 @@ class LinearSystemLoop {
       LinearQuadraticRegulator<States, Inputs>& controller,
       const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
       KalmanFilter<States, Inputs, Outputs>& observer,
-      std::function<Eigen::Matrix<double, Inputs, 1>(
-          const Eigen::Matrix<double, Inputs, 1>&)>
+      std::function<
+          Eigen::Vector<double, Inputs>(const Eigen::Vector<double, Inputs>&)>
           clampFunction)
       : m_controller(&controller),
         m_feedforward(feedforward),
@@ -130,7 +130,7 @@ class LinearSystemLoop {
   /**
    * Returns the observer's state estimate x-hat.
    */
-  const Eigen::Matrix<double, States, 1>& Xhat() const {
+  const Eigen::Vector<double, States>& Xhat() const {
     return m_observer->Xhat();
   }
 
@@ -144,7 +144,7 @@ class LinearSystemLoop {
   /**
    * Returns the controller's next reference r.
    */
-  const Eigen::Matrix<double, States, 1>& NextR() const { return m_nextR; }
+  const Eigen::Vector<double, States>& NextR() const { return m_nextR; }
 
   /**
    * Returns an element of the controller's next reference r.
@@ -156,7 +156,7 @@ class LinearSystemLoop {
   /**
    * Returns the controller's calculated control input u.
    */
-  Eigen::Matrix<double, Inputs, 1> U() const {
+  Eigen::Vector<double, Inputs> U() const {
     return ClampInput(m_controller->U() + m_feedforward.Uff());
   }
 
@@ -172,7 +172,7 @@ class LinearSystemLoop {
    *
    * @param xHat The initial state estimate x-hat.
    */
-  void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) {
+  void SetXhat(const Eigen::Vector<double, States>& xHat) {
     m_observer->SetXhat(xHat);
   }
 
@@ -189,9 +189,7 @@ class LinearSystemLoop {
    *
    * @param nextR Next reference.
    */
-  void SetNextR(const Eigen::Matrix<double, States, 1>& nextR) {
-    m_nextR = nextR;
-  }
+  void SetNextR(const Eigen::Vector<double, States>& nextR) { m_nextR = nextR; }
 
   /**
    * Return the controller used internally.
@@ -223,7 +221,7 @@ class LinearSystemLoop {
    *
    * @param initialState The initial state.
    */
-  void Reset(Eigen::Matrix<double, States, 1> initialState) {
+  void Reset(const Eigen::Vector<double, States>& initialState) {
     m_nextR.setZero();
     m_controller->Reset();
     m_feedforward.Reset(initialState);
@@ -233,7 +231,7 @@ class LinearSystemLoop {
   /**
    * Returns difference between reference r and current state x-hat.
    */
-  const Eigen::Matrix<double, States, 1> Error() const {
+  Eigen::Vector<double, States> Error() const {
     return m_controller->R() - m_observer->Xhat();
   }
 
@@ -242,7 +240,7 @@ class LinearSystemLoop {
    *
    * @param y Measurement vector.
    */
-  void Correct(const Eigen::Matrix<double, Outputs, 1>& y) {
+  void Correct(const Eigen::Vector<double, Outputs>& y) {
     m_observer->Correct(U(), y);
   }
 
@@ -256,7 +254,7 @@ class LinearSystemLoop {
    * @param dt Timestep for model update.
    */
   void Predict(units::second_t dt) {
-    Eigen::Matrix<double, Inputs, 1> u =
+    Eigen::Vector<double, Inputs> u =
         ClampInput(m_controller->Calculate(m_observer->Xhat(), m_nextR) +
                    m_feedforward.Calculate(m_nextR));
     m_observer->Predict(u, dt);
@@ -268,8 +266,8 @@ class LinearSystemLoop {
    * @param u Input vector to clamp.
    * @return Clamped input vector.
    */
-  Eigen::Matrix<double, Inputs, 1> ClampInput(
-      const Eigen::Matrix<double, Inputs, 1>& u) const {
+  Eigen::Vector<double, Inputs> ClampInput(
+      const Eigen::Vector<double, Inputs>& u) const {
     return m_clampFunc(u);
   }
 
@@ -281,12 +279,12 @@ class LinearSystemLoop {
   /**
    * Clamping function.
    */
-  std::function<Eigen::Matrix<double, Inputs, 1>(
-      const Eigen::Matrix<double, Inputs, 1>&)>
+  std::function<Eigen::Vector<double, Inputs>(
+      const Eigen::Vector<double, Inputs>&)>
       m_clampFunc;
 
   // Reference to go to in the next cycle (used by feedforward controller).
-  Eigen::Matrix<double, States, 1> m_nextR;
+  Eigen::Vector<double, States> m_nextR;
 
   // These are accessible from non-templated subclasses.
   static constexpr int kStates = States;
diff --git a/wpimath/src/main/native/include/frc/system/NumericalJacobian.h b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
index 560e708391..702a3b815f 100644
--- a/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
+++ b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
@@ -17,16 +17,16 @@ namespace frc {
  * @param x     Vector argument.
  */
 template <int Rows, int Cols, typename F>
-auto NumericalJacobian(F&& f, const Eigen::Matrix<double, Cols, 1>& x) {
+auto NumericalJacobian(F&& f, const Eigen::Vector<double, Cols>& x) {
   constexpr double kEpsilon = 1e-5;
   Eigen::Matrix<double, Rows, Cols> result;
   result.setZero();
 
   // It's more expensive, but +- epsilon will be more accurate
   for (int i = 0; i < Cols; ++i) {
-    Eigen::Matrix<double, Cols, 1> dX_plus = x;
+    Eigen::Vector<double, Cols> dX_plus = x;
     dX_plus(i) += kEpsilon;
-    Eigen::Matrix<double, Cols, 1> dX_minus = x;
+    Eigen::Vector<double, Cols> dX_minus = x;
     dX_minus(i) -= kEpsilon;
     result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
   }
@@ -47,11 +47,12 @@ auto NumericalJacobian(F&& f, const Eigen::Matrix<double, Cols, 1>& x) {
  * @param u        Input vector.
  */
 template <int Rows, int States, int Inputs, typename F, typename... Args>
-auto NumericalJacobianX(F&& f, const Eigen::Matrix<double, States, 1>& x,
-                        const Eigen::Matrix<double, Inputs, 1>& u,
+auto NumericalJacobianX(F&& f, const Eigen::Vector<double, States>& x,
+                        const Eigen::Vector<double, Inputs>& u,
                         Args&&... args) {
   return NumericalJacobian<Rows, States>(
-      [&](Eigen::Matrix<double, States, 1> x) { return f(x, u, args...); }, x);
+      [&](const Eigen::Vector<double, States>& x) { return f(x, u, args...); },
+      x);
 }
 
 /**
@@ -67,11 +68,12 @@ auto NumericalJacobianX(F&& f, const Eigen::Matrix<double, States, 1>& x,
  * @param u        Input vector.
  */
 template <int Rows, int States, int Inputs, typename F, typename... Args>
-auto NumericalJacobianU(F&& f, const Eigen::Matrix<double, States, 1>& x,
-                        const Eigen::Matrix<double, Inputs, 1>& u,
+auto NumericalJacobianU(F&& f, const Eigen::Vector<double, States>& x,
+                        const Eigen::Vector<double, Inputs>& u,
                         Args&&... args) {
   return NumericalJacobian<Rows, Inputs>(
-      [&](Eigen::Matrix<double, Inputs, 1> u) { return f(x, u, args...); }, u);
+      [&](const Eigen::Vector<double, Inputs>& u) { return f(x, u, args...); },
+      u);
 }
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
index 3f8c1973e7..714857f6b0 100644
--- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
+++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
@@ -43,15 +43,15 @@ class WPILIB_DLLEXPORT LinearSystemId {
   static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor,
                                               units::kilogram_t m,
                                               units::meter_t r, double G) {
-    Eigen::Matrix<double, 2, 2> A = frc::MakeMatrix<2, 2>(
-        0.0, 1.0, 0.0,
-        (-std::pow(G, 2) * motor.Kt /
-         (motor.R * units::math::pow<2>(r) * m * motor.Kv))
-            .to<double>());
-    Eigen::Matrix<double, 2, 1> B = frc::MakeMatrix<2, 1>(
-        0.0, (G * motor.Kt / (motor.R * r * m)).to<double>());
-    Eigen::Matrix<double, 1, 2> C = frc::MakeMatrix<1, 2>(1.0, 0.0);
-    Eigen::Matrix<double, 1, 1> D = frc::MakeMatrix<1, 1>(0.0);
+    Eigen::Matrix<double, 2, 2> A{
+        {0.0, 1.0},
+        {0.0, (-std::pow(G, 2) * motor.Kt /
+               (motor.R * units::math::pow<2>(r) * m * motor.Kv))
+                  .to<double>()}};
+    Eigen::Matrix<double, 2, 1> B{
+        0.0, (G * motor.Kt / (motor.R * r * m)).to<double>()};
+    Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
+    Eigen::Matrix<double, 1, 1> D{0.0};
 
     return LinearSystem<2, 1, 1>(A, B, C, D);
   }
@@ -69,13 +69,14 @@ class WPILIB_DLLEXPORT LinearSystemId {
    */
   static LinearSystem<2, 1, 1> SingleJointedArmSystem(
       DCMotor motor, units::kilogram_square_meter_t J, double G) {
-    Eigen::Matrix<double, 2, 2> A = frc::MakeMatrix<2, 2>(
-        0.0, 1.0, 0.0,
-        (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>());
-    Eigen::Matrix<double, 2, 1> B =
-        frc::MakeMatrix<2, 1>(0.0, (G * motor.Kt / (motor.R * J)).to<double>());
-    Eigen::Matrix<double, 1, 2> C = frc::MakeMatrix<1, 2>(1.0, 0.0);
-    Eigen::Matrix<double, 1, 1> D = frc::MakeMatrix<1, 1>(0.0);
+    Eigen::Matrix<double, 2, 2> A{
+        {0.0, 1.0},
+        {0.0,
+         (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>()}};
+    Eigen::Matrix<double, 2, 1> B{0.0,
+                                  (G * motor.Kt / (motor.R * J)).to<double>()};
+    Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
+    Eigen::Matrix<double, 1, 1> D{0.0};
 
     return LinearSystem<2, 1, 1>(A, B, C, D);
   }
@@ -106,12 +107,11 @@ class WPILIB_DLLEXPORT LinearSystemId {
   static LinearSystem<1, 1, 1> IdentifyVelocitySystem(
       decltype(1_V / Velocity_t<Distance>(1)) kV,
       decltype(1_V / Acceleration_t<Distance>(1)) kA) {
-    Eigen::Matrix<double, 1, 1> A = frc::MakeMatrix<1, 1>(
-        -kV.template to<double>() / kA.template to<double>());
-    Eigen::Matrix<double, 1, 1> B =
-        frc::MakeMatrix<1, 1>(1.0 / kA.template to<double>());
-    Eigen::Matrix<double, 1, 1> C = frc::MakeMatrix<1, 1>(1.0);
-    Eigen::Matrix<double, 1, 1> D = frc::MakeMatrix<1, 1>(0.0);
+    Eigen::Matrix<double, 1, 1> A{-kV.template to<double>() /
+                                  kA.template to<double>()};
+    Eigen::Matrix<double, 1, 1> B{1.0 / kA.template to<double>()};
+    Eigen::Matrix<double, 1, 1> C{1.0};
+    Eigen::Matrix<double, 1, 1> D{0.0};
 
     return LinearSystem<1, 1, 1>(A, B, C, D);
   }
@@ -142,12 +142,12 @@ class WPILIB_DLLEXPORT LinearSystemId {
   static LinearSystem<2, 1, 1> IdentifyPositionSystem(
       decltype(1_V / Velocity_t<Distance>(1)) kV,
       decltype(1_V / Acceleration_t<Distance>(1)) kA) {
-    Eigen::Matrix<double, 2, 2> A = frc::MakeMatrix<2, 2>(
-        0.0, 1.0, 0.0, -kV.template to<double>() / kA.template to<double>());
-    Eigen::Matrix<double, 2, 1> B =
-        frc::MakeMatrix<2, 1>(0.0, 1.0 / kA.template to<double>());
-    Eigen::Matrix<double, 1, 2> C = frc::MakeMatrix<1, 2>(1.0, 0.0);
-    Eigen::Matrix<double, 1, 1> D = frc::MakeMatrix<1, 1>(0.0);
+    Eigen::Matrix<double, 2, 2> A{
+        {0.0, 1.0},
+        {0.0, -kV.template to<double>() / kA.template to<double>()}};
+    Eigen::Matrix<double, 2, 1> B{0.0, 1.0 / kA.template to<double>()};
+    Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
+    Eigen::Matrix<double, 1, 1> D{0.0};
 
     return LinearSystem<2, 1, 1>(A, B, C, D);
   }
@@ -177,10 +177,12 @@ class WPILIB_DLLEXPORT LinearSystemId {
     double B1 = 1.0 / kAlinear.to<double>() + 1.0 / kAangular.to<double>();
     double B2 = 1.0 / kAlinear.to<double>() - 1.0 / kAangular.to<double>();
 
-    Eigen::Matrix<double, 2, 2> A = 0.5 * frc::MakeMatrix<2, 2>(A1, A2, A2, A1);
-    Eigen::Matrix<double, 2, 2> B = 0.5 * frc::MakeMatrix<2, 2>(B1, B2, B2, B1);
-    Eigen::Matrix<double, 2, 2> C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
-    Eigen::Matrix<double, 2, 2> D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0);
+    Eigen::Matrix<double, 2, 2> A =
+        0.5 * Eigen::Matrix<double, 2, 2>{{A1, A2}, {A2, A1}};
+    Eigen::Matrix<double, 2, 2> B =
+        0.5 * Eigen::Matrix<double, 2, 2>{{B1, B2}, {B2, B1}};
+    Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
+    Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
 
     return LinearSystem<2, 2, 2>(A, B, C, D);
   }
@@ -236,12 +238,11 @@ class WPILIB_DLLEXPORT LinearSystemId {
   static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor,
                                               units::kilogram_square_meter_t J,
                                               double G) {
-    auto A = frc::MakeMatrix<1, 1>(
-        (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>());
-    Eigen::Matrix<double, 1, 1> B =
-        frc::MakeMatrix<1, 1>((G * motor.Kt / (motor.R * J)).to<double>());
-    Eigen::Matrix<double, 1, 1> C = frc::MakeMatrix<1, 1>(1.0);
-    Eigen::Matrix<double, 1, 1> D = frc::MakeMatrix<1, 1>(0.0);
+    Eigen::Matrix<double, 1, 1> A{
+        (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>()};
+    Eigen::Matrix<double, 1, 1> B{(G * motor.Kt / (motor.R * J)).to<double>()};
+    Eigen::Matrix<double, 1, 1> C{1.0};
+    Eigen::Matrix<double, 1, 1> D{0.0};
 
     return LinearSystem<1, 1, 1>(A, B, C, D);
   }
@@ -267,18 +268,18 @@ class WPILIB_DLLEXPORT LinearSystemId {
               (motor.Kv * motor.R * units::math::pow<2>(r));
     auto C2 = G * motor.Kt / (motor.R * r);
 
-    Eigen::Matrix<double, 2, 2> A = frc::MakeMatrix<2, 2>(
-        ((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>(),
-        ((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>(),
-        ((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>(),
-        ((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>());
-    Eigen::Matrix<double, 2, 2> B = frc::MakeMatrix<2, 2>(
-        ((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>(),
-        ((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>(),
-        ((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>(),
-        ((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>());
-    Eigen::Matrix<double, 2, 2> C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
-    Eigen::Matrix<double, 2, 2> D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0);
+    Eigen::Matrix<double, 2, 2> A{
+        {((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>(),
+         ((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>()},
+        {((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>(),
+         ((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>()}};
+    Eigen::Matrix<double, 2, 2> B{
+        {((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>(),
+         ((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>()},
+        {((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>(),
+         ((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>()}};
+    Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
+    Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
 
     return LinearSystem<2, 2, 2>(A, B, C, D);
   }
diff --git a/wpimath/src/test/native/cpp/EigenTest.cpp b/wpimath/src/test/native/cpp/EigenTest.cpp
index 1ae700532a..7b433552b7 100644
--- a/wpimath/src/test/native/cpp/EigenTest.cpp
+++ b/wpimath/src/test/native/cpp/EigenTest.cpp
@@ -7,48 +7,40 @@
 #include "gtest/gtest.h"
 
 TEST(EigenTest, MultiplicationTest) {
-  Eigen::Matrix<double, 2, 2> m1;
-  m1 << 2, 1, 0, 1;
-
-  Eigen::Matrix<double, 2, 2> m2;
-  m2 << 3, 0, 0, 2.5;
+  Eigen::Matrix<double, 2, 2> m1{{2, 1}, {0, 1}};
+  Eigen::Matrix<double, 2, 2> m2{{3, 0}, {0, 2.5}};
 
   const auto result = m1 * m2;
 
-  Eigen::Matrix<double, 2, 2> expectedResult;
-  expectedResult << 6.0, 2.5, 0.0, 2.5;
+  Eigen::Matrix<double, 2, 2> expectedResult{{6.0, 2.5}, {0.0, 2.5}};
 
   EXPECT_TRUE(expectedResult.isApprox(result));
 
-  Eigen::Matrix<double, 2, 3> m3;
-  m3 << 1.0, 3.0, 0.5, 2.0, 4.3, 1.2;
-
-  Eigen::Matrix<double, 3, 4> m4;
-  m4 << 3.0, 1.5, 2.0, 4.5, 2.3, 1.0, 1.6, 3.1, 5.2, 2.1, 2.0, 1.0;
+  Eigen::Matrix<double, 2, 3> m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}};
+  Eigen::Matrix<double, 3, 4> m4{
+      {3.0, 1.5, 2.0, 4.5}, {2.3, 1.0, 1.6, 3.1}, {5.2, 2.1, 2.0, 1.0}};
 
   const auto result2 = m3 * m4;
 
-  Eigen::Matrix<double, 2, 4> expectedResult2;
-  expectedResult2 << 12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53;
+  Eigen::Matrix<double, 2, 4> expectedResult2{{12.5, 5.55, 7.8, 14.3},
+                                              {22.13, 9.82, 13.28, 23.53}};
 
   EXPECT_TRUE(expectedResult2.isApprox(result2));
 }
 
 TEST(EigenTest, TransposeTest) {
-  Eigen::Matrix<double, 3, 1> vec;
-  vec << 1, 2, 3;
+  Eigen::Vector<double, 3> vec{1, 2, 3};
 
   const auto transpose = vec.transpose();
 
-  Eigen::Matrix<double, 1, 3> expectedTranspose;
-  expectedTranspose << 1, 2, 3;
+  Eigen::RowVector<double, 3> expectedTranspose{1, 2, 3};
 
   EXPECT_TRUE(expectedTranspose.isApprox(transpose));
 }
 
 TEST(EigenTest, InverseTest) {
-  Eigen::Matrix<double, 3, 3> mat;
-  mat << 1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5;
+  Eigen::Matrix<double, 3, 3> mat{
+      {1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}};
 
   const auto inverse = mat.inverse();
   const auto identity = Eigen::MatrixXd::Identity(3, 3);
diff --git a/wpimath/src/test/native/cpp/FormatterTest.cpp b/wpimath/src/test/native/cpp/FormatterTest.cpp
index 644ebef44f..cd7ef5cbe4 100644
--- a/wpimath/src/test/native/cpp/FormatterTest.cpp
+++ b/wpimath/src/test/native/cpp/FormatterTest.cpp
@@ -10,8 +10,7 @@
 #include "units/velocity.h"
 
 TEST(FormatterTest, Eigen) {
-  Eigen::Matrix<double, 3, 2> A;
-  A << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0;
+  Eigen::Matrix<double, 3, 2> A{{1.0, 2.0}, {3.0, 4.0}, {5.0, 6.0}};
   EXPECT_EQ(
       "  1.000000  2.000000\n"
       "  3.000000  4.000000\n"
diff --git a/wpimath/src/test/native/cpp/StateSpaceTest.cpp b/wpimath/src/test/native/cpp/StateSpaceTest.cpp
index cd8b083ba3..c6e2241b55 100644
--- a/wpimath/src/test/native/cpp/StateSpaceTest.cpp
+++ b/wpimath/src/test/native/cpp/StateSpaceTest.cpp
@@ -45,8 +45,8 @@ class StateSpace : public testing::Test {
 
 void Update(const LinearSystem<2, 1, 1>& plant, LinearSystemLoop<2, 1, 1>& loop,
             double noise) {
-  Eigen::Matrix<double, 1, 1> y = plant.CalculateY(loop.Xhat(), loop.U()) +
-                                  Eigen::Matrix<double, 1, 1>(noise);
+  Eigen::Vector<double, 1> y =
+      plant.CalculateY(loop.Xhat(), loop.U()) + Eigen::Vector<double, 1>{noise};
   loop.Correct(y);
   loop.Predict(kDt);
 }
@@ -55,8 +55,7 @@ TEST_F(StateSpace, CorrectPredictLoop) {
   std::default_random_engine generator;
   std::normal_distribution<double> dist{0.0, kPositionStddev};
 
-  Eigen::Matrix<double, 2, 1> references;
-  references << 2.0, 0.0;
+  Eigen::Vector<double, 2> references{2.0, 0.0};
   loop.SetNextR(references);
 
   for (int i = 0; i < 1000; i++) {
diff --git a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
index 89d7c402cd..c1984255cd 100644
--- a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
@@ -12,12 +12,12 @@
 
 TEST(StateSpaceUtilTest, MakeMatrix) {
   // Column vector
-  Eigen::Matrix<double, 2, 1> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0);
+  Eigen::Vector<double, 2> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0);
   EXPECT_NEAR(mat1(0), 1.0, 1e-3);
   EXPECT_NEAR(mat1(1), 2.0, 1e-3);
 
   // Row vector
-  Eigen::Matrix<double, 1, 2> mat2 = frc::MakeMatrix<1, 2>(1.0, 2.0);
+  Eigen::RowVector<double, 2> mat2 = frc::MakeMatrix<1, 2>(1.0, 2.0);
   EXPECT_NEAR(mat2(0), 1.0, 1e-3);
   EXPECT_NEAR(mat2(1), 2.0, 1e-3);
 
@@ -102,44 +102,42 @@ TEST(StateSpaceUtilTest, CovArray) {
 }
 
 TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
-  Eigen::Matrix<double, 2, 1> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
+  Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
   static_cast<void>(vec);
 }
 
 TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) {
-  Eigen::Matrix<double, 2, 1> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
+  Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
   static_cast<void>(vec);
 }
 
 TEST(StateSpaceUtilTest, IsStabilizable) {
-  Eigen::Matrix<double, 2, 2> A;
-  Eigen::Matrix<double, 2, 1> B;
-  B << 0, 1;
+  Eigen::Matrix<double, 2, 1> B{0, 1};
 
   // We separate the result of IsStabilizable from the assertion because
   // templates break gtest.
 
   // First eigenvalue is uncontrollable and unstable.
   // Second eigenvalue is controllable and stable.
-  A << 1.2, 0, 0, 0.5;
-  bool ret = frc::IsStabilizable<2, 1>(A, B);
+  Eigen::Matrix<double, 2, 2> A1{{1.2, 0}, {0, 0.5}};
+  bool ret = frc::IsStabilizable<2, 1>(A1, B);
   EXPECT_FALSE(ret);
 
   // First eigenvalue is uncontrollable and marginally stable.
   // Second eigenvalue is controllable and stable.
-  A << 1, 0, 0, 0.5;
-  ret = frc::IsStabilizable<2, 1>(A, B);
+  Eigen::Matrix<double, 2, 2> A2{{1, 0}, {0, 0.5}};
+  ret = frc::IsStabilizable<2, 1>(A2, B);
   EXPECT_FALSE(ret);
 
   // First eigenvalue is uncontrollable and stable.
   // Second eigenvalue is controllable and stable.
-  A << 0.2, 0, 0, 0.5;
-  ret = frc::IsStabilizable<2, 1>(A, B);
+  Eigen::Matrix<double, 2, 2> A3{{0.2, 0}, {0, 0.5}};
+  ret = frc::IsStabilizable<2, 1>(A3, B);
   EXPECT_TRUE(ret);
 
   // First eigenvalue is uncontrollable and stable.
   // Second eigenvalue is controllable and unstable.
-  A << 0.2, 0, 0, 1.2;
-  ret = frc::IsStabilizable<2, 1>(A, B);
+  Eigen::Matrix<double, 2, 2> A4{{0.2, 0}, {0, 1.2}};
+  ret = frc::IsStabilizable<2, 1>(A4, B);
   EXPECT_TRUE(ret);
 }
diff --git a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
index e3560603ec..354ed18637 100644
--- a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
@@ -7,61 +7,46 @@
 #include <cmath>
 
 #include "Eigen/Core"
-#include "frc/StateSpaceUtil.h"
 #include "frc/controller/ControlAffinePlantInversionFeedforward.h"
 #include "units/time.h"
 
 namespace frc {
 
-Eigen::Matrix<double, 2, 1> Dynamics(const Eigen::Matrix<double, 2, 1>& x,
-                                     const Eigen::Matrix<double, 1, 1>& u) {
-  Eigen::Matrix<double, 2, 1> result;
-
-  result = (frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0) * x) +
-           (frc::MakeMatrix<2, 1>(0.0, 1.0) * u);
-
-  return result;
+Eigen::Vector<double, 2> Dynamics(const Eigen::Vector<double, 2>& x,
+                                  const Eigen::Vector<double, 1>& u) {
+  return Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x +
+         Eigen::Matrix<double, 2, 1>{0.0, 1.0} * u;
 }
 
-Eigen::Matrix<double, 2, 1> StateDynamics(
-    const Eigen::Matrix<double, 2, 1>& x) {
-  Eigen::Matrix<double, 2, 1> result;
-
-  result = (frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0) * x);
-
-  return result;
+Eigen::Vector<double, 2> StateDynamics(const Eigen::Vector<double, 2>& x) {
+  return Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x;
 }
 
 TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
-  std::function<Eigen::Matrix<double, 2, 1>(const Eigen::Matrix<double, 2, 1>&,
-                                            const Eigen::Matrix<double, 1, 1>&)>
+  std::function<Eigen::Vector<double, 2>(const Eigen::Vector<double, 2>&,
+                                         const Eigen::Vector<double, 1>&)>
       modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); };
 
   frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
-      modelDynamics, units::second_t(0.02)};
+      modelDynamics, units::second_t{0.02}};
 
-  Eigen::Matrix<double, 2, 1> r;
-  r << 2, 2;
-  Eigen::Matrix<double, 2, 1> nextR;
-  nextR << 3, 3;
+  Eigen::Vector<double, 2> r{2, 2};
+  Eigen::Vector<double, 2> nextR{3, 3};
 
   EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
 }
 
 TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
-  std::function<Eigen::Matrix<double, 2, 1>(const Eigen::Matrix<double, 2, 1>&)>
+  std::function<Eigen::Vector<double, 2>(const Eigen::Vector<double, 2>&)>
       modelDynamics = [](auto& x) { return StateDynamics(x); };
 
-  Eigen::Matrix<double, 2, 1> B;
-  B << 0, 1;
+  Eigen::Matrix<double, 2, 1> B{0, 1};
 
   frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
       modelDynamics, B, units::second_t(0.02)};
 
-  Eigen::Matrix<double, 2, 1> r;
-  r << 2, 2;
-  Eigen::Matrix<double, 2, 1> nextR;
-  nextR << 3, 3;
+  Eigen::Vector<double, 2> r{2, 2};
+  Eigen::Vector<double, 2> nextR{3, 3};
 
   EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
 }
diff --git a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
index a9d1dd4b69..6e61706ff0 100644
--- a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
@@ -13,19 +13,14 @@
 namespace frc {
 
 TEST(LinearPlantInversionFeedforwardTest, Calculate) {
-  Eigen::Matrix<double, 2, 2> A;
-  A << 1, 0, 0, 1;
-
-  Eigen::Matrix<double, 2, 1> B;
-  B << 0, 1;
+  Eigen::Matrix<double, 2, 2> A{{1, 0}, {0, 1}};
+  Eigen::Matrix<double, 2, 1> B{0, 1};
 
   frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B,
                                                          units::second_t(0.02)};
 
-  Eigen::Matrix<double, 2, 1> r;
-  r << 2, 2;
-  Eigen::Matrix<double, 2, 1> nextR;
-  nextR << 3, 3;
+  Eigen::Vector<double, 2> r{2, 2};
+  Eigen::Vector<double, 2> nextR{3, 3};
 
   EXPECT_NEAR(47.502599, feedforward.Calculate(r, nextR)(0, 0), 0.002);
 }
diff --git a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
index c178ddbb4c..29a789e370 100644
--- a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
@@ -21,21 +21,16 @@ TEST(SimpleMotorFeedforwardTest, Calculate) {
   double Ka = 0.6;
   auto dt = 0.02_s;
 
-  Eigen::Matrix<double, 1, 1> A;
-  A << -Kv / Ka;
-
-  Eigen::Matrix<double, 1, 1> B;
-  B << 1.0 / Ka;
+  Eigen::Matrix<double, 1, 1> A{-Kv / Ka};
+  Eigen::Matrix<double, 1, 1> B{1.0 / Ka};
 
   frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
   frc::SimpleMotorFeedforward<units::meter> simpleMotor{
       units::volt_t{Ks}, units::volt_t{Kv} / 1_mps,
       units::volt_t{Ka} / 1_mps_sq};
 
-  Eigen::Matrix<double, 1, 1> r;
-  r << 2;
-  Eigen::Matrix<double, 1, 1> nextR;
-  nextR << 3;
+  Eigen::Vector<double, 1> r{2.0};
+  Eigen::Vector<double, 1> nextR{3.0};
 
   EXPECT_NEAR(37.524995834325161 + Ks,
               simpleMotor.Calculate(2_mps, 3_mps, dt).to<double>(), 0.002);
diff --git a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
index bd1e30b6d4..5b25a74939 100644
--- a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
@@ -10,11 +10,12 @@
 #include "frc/estimator/AngleStatistics.h"
 
 TEST(AngleStatisticsTest, TestMean) {
-  Eigen::Matrix<double, 3, 3> sigmas;
-  sigmas << 1, 1.2, 0, 359 * wpi::numbers::pi / 180, 3 * wpi::numbers::pi / 180,
-      0, 1, 2, 0;
+  Eigen::Matrix<double, 3, 3> sigmas{
+      {1, 1.2, 0},
+      {359 * wpi::numbers::pi / 180, 3 * wpi::numbers::pi / 180, 0},
+      {1, 2, 0}};
   // Weights need to produce the mean of the sigmas
-  Eigen::Vector3d weights{};
+  Eigen::Vector3d weights;
   weights.fill(1.0 / sigmas.cols());
 
   EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
@@ -22,16 +23,16 @@ TEST(AngleStatisticsTest, TestMean) {
 }
 
 TEST(AngleStatisticsTest, TestResidual) {
-  Eigen::Vector3d a(1, 1 * wpi::numbers::pi / 180, 2);
-  Eigen::Vector3d b(1, 359 * wpi::numbers::pi / 180, 1);
+  Eigen::Vector3d a{1, 1 * wpi::numbers::pi / 180, 2};
+  Eigen::Vector3d b{1, 359 * wpi::numbers::pi / 180, 1};
 
   EXPECT_TRUE(frc::AngleResidual<3>(a, b, 1).isApprox(
-      Eigen::Vector3d(0, 2 * wpi::numbers::pi / 180, 1)));
+      Eigen::Vector3d{0, 2 * wpi::numbers::pi / 180, 1}));
 }
 
 TEST(AngleStatisticsTest, TestAdd) {
-  Eigen::Vector3d a(1, 1 * wpi::numbers::pi / 180, 2);
-  Eigen::Vector3d b(1, 359 * wpi::numbers::pi / 180, 1);
+  Eigen::Vector3d a{1, 1 * wpi::numbers::pi / 180, 2};
+  Eigen::Vector3d b{1, 359 * wpi::numbers::pi / 180, 1};
 
-  EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d(2, 0, 3)));
+  EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
 }
diff --git a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
index f1fbf7d7b4..830895521e 100644
--- a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
@@ -18,8 +18,8 @@
 
 namespace {
 
-Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
-                                     const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
+                                  const Eigen::Vector<double, 2>& u) {
   auto motors = frc::DCMotor::CIM(2);
 
   // constexpr double Glow = 15.32;       // Low gear ratio
@@ -40,36 +40,26 @@ Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
   units::volt_t Vl{u(0)};
   units::volt_t Vr{u(1)};
 
-  Eigen::Matrix<double, 5, 1> result;
   auto v = 0.5 * (vl + vr);
-  result(0) = v.to<double>() * std::cos(x(2));
-  result(1) = v.to<double>() * std::sin(x(2));
-  result(2) = ((vr - vl) / (2.0 * rb)).to<double>();
-  result(3) =
+  return Eigen::Vector<double, 5>{
+      v.to<double>() * std::cos(x(2)), v.to<double>() * std::sin(x(2)),
+      ((vr - vl) / (2.0 * rb)).to<double>(),
       k1.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
-      k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
-  result(4) =
+          k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>()),
       k2.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
-      k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
-  return result;
+          k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>())};
 }
 
-Eigen::Matrix<double, 3, 1> LocalMeasurementModel(
-    const Eigen::Matrix<double, 5, 1>& x,
-    const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 3> LocalMeasurementModel(
+    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
   static_cast<void>(u);
-  Eigen::Matrix<double, 3, 1> y;
-  y << x(2), x(3), x(4);
-  return y;
+  return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
 }
 
-Eigen::Matrix<double, 5, 1> GlobalMeasurementModel(
-    const Eigen::Matrix<double, 5, 1>& x,
-    const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 5> GlobalMeasurementModel(
+    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
   static_cast<void>(u);
-  Eigen::Matrix<double, 5, 1> y;
-  y << x(0), x(1), x(2), x(3), x(4);
-  return y;
+  return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
 }
 }  // namespace
 
@@ -81,8 +71,7 @@ TEST(ExtendedKalmanFilterTest, Init) {
                                               {0.5, 0.5, 10.0, 1.0, 1.0},
                                               {0.0001, 0.01, 0.01},
                                               dt};
-  Eigen::Matrix<double, 2, 1> u;
-  u << 12.0, 12.0;
+  Eigen::Vector<double, 2> u{12.0, 12.0};
   observer.Predict(u, dt);
 
   auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -109,19 +98,17 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
   auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
       waypoints, {8.8_mps, 0.1_mps_sq});
 
-  Eigen::Matrix<double, 5, 1> r = Eigen::Matrix<double, 5, 1>::Zero();
+  Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
+  Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
 
-  Eigen::Matrix<double, 5, 1> nextR;
-  Eigen::Matrix<double, 2, 1> u = Eigen::Matrix<double, 2, 1>::Zero();
+  auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics,
+                                            Eigen::Vector<double, 5>::Zero(),
+                                            Eigen::Vector<double, 2>::Zero());
 
-  auto B = frc::NumericalJacobianU<5, 5, 2>(
-      Dynamics, Eigen::Matrix<double, 5, 1>::Zero(),
-      Eigen::Matrix<double, 2, 1>::Zero());
-
-  observer.SetXhat(frc::MakeMatrix<5, 1>(
+  observer.SetXhat(Eigen::Vector<double, 5>{
       trajectory.InitialPose().Translation().X().to<double>(),
       trajectory.InitialPose().Translation().Y().to<double>(),
-      trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0));
+      trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0});
 
   auto totalTime = trajectory.TotalTime();
   for (size_t i = 0; i < (totalTime / dt).to<double>(); ++i) {
@@ -131,19 +118,18 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
     units::meters_per_second_t vr =
         ref.velocity * (1 + (ref.curvature * rb).to<double>());
 
-    nextR(0) = ref.pose.Translation().X().to<double>();
-    nextR(1) = ref.pose.Translation().Y().to<double>();
-    nextR(2) = ref.pose.Rotation().Radians().to<double>();
-    nextR(3) = vl.to<double>();
-    nextR(4) = vr.to<double>();
+    Eigen::Vector<double, 5> nextR{ref.pose.Translation().X().to<double>(),
+                                   ref.pose.Translation().Y().to<double>(),
+                                   ref.pose.Rotation().Radians().to<double>(),
+                                   vl.to<double>(), vr.to<double>()};
 
     auto localY =
-        LocalMeasurementModel(nextR, Eigen::Matrix<double, 2, 1>::Zero());
+        LocalMeasurementModel(nextR, Eigen::Vector<double, 2>::Zero());
     observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
 
-    Eigen::Matrix<double, 5, 1> rdot = (nextR - r) / dt.to<double>();
-    u = B.householderQr().solve(
-        rdot - Dynamics(r, Eigen::Matrix<double, 2, 1>::Zero()));
+    Eigen::Vector<double, 5> rdot = (nextR - r) / dt.to<double>();
+    u = B.householderQr().solve(rdot -
+                                Dynamics(r, Eigen::Vector<double, 2>::Zero()));
 
     observer.Predict(u, dt);
 
diff --git a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
index 8420d52530..244270fb35 100644
--- a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
@@ -4,32 +4,33 @@
 
 #include <gtest/gtest.h>
 
-#include "frc/StateSpaceUtil.h"
 #include "frc/estimator/MerweScaledSigmaPoints.h"
 
 namespace drake::math {
 namespace {
 TEST(MerweScaledSigmaPointsTest, TestZeroMean) {
   frc::MerweScaledSigmaPoints<2> sigmaPoints;
-  auto points =
-      sigmaPoints.SigmaPoints(frc::MakeMatrix<2, 1>(0.0, 0.0),
-                              frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0));
+  auto points = sigmaPoints.SigmaPoints(
+      Eigen::Vector<double, 2>{0.0, 0.0},
+      Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}});
 
   EXPECT_TRUE(
-      (points - frc::MakeMatrix<2, 5>(0.0, 0.00173205, 0.0, -0.00173205, 0.0,
-                                      0.0, 0.0, 0.00173205, 0.0, -0.00173205))
+      (points -
+       Eigen::Matrix<double, 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);
 }
 
 TEST(MerweScaledSigmaPointsTest, TestNonzeroMean) {
   frc::MerweScaledSigmaPoints<2> sigmaPoints;
-  auto points =
-      sigmaPoints.SigmaPoints(frc::MakeMatrix<2, 1>(1.0, 2.0),
-                              frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 10.0));
+  auto points = sigmaPoints.SigmaPoints(
+      Eigen::Vector<double, 2>{1.0, 2.0},
+      Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 10.0}});
 
   EXPECT_TRUE(
-      (points - frc::MakeMatrix<2, 5>(1.0, 1.00173205, 1.0, 0.998268, 1.0, 2.0,
-                                      2.0, 2.00548, 2.0, 1.99452))
+      (points -
+       Eigen::Matrix<double, 2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
+                                   {2.0, 2.0, 2.00548, 2.0, 1.99452}})
           .norm() < 1e-3);
 }
 }  // namespace
diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
index a659baede2..8c9afc10bb 100644
--- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
@@ -20,8 +20,8 @@
 
 namespace {
 
-Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
-                                     const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
+                                  const Eigen::Vector<double, 2>& u) {
   auto motors = frc::DCMotor::CIM(2);
 
   // constexpr double Glow = 15.32;       // Low gear ratio
@@ -42,49 +42,38 @@ Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
   units::volt_t Vl{u(0)};
   units::volt_t Vr{u(1)};
 
-  Eigen::Matrix<double, 5, 1> result;
   auto v = 0.5 * (vl + vr);
-  result(0) = v.to<double>() * std::cos(x(2));
-  result(1) = v.to<double>() * std::sin(x(2));
-  result(2) = ((vr - vl) / (2.0 * rb)).to<double>();
-  result(3) =
+  return Eigen::Vector<double, 5>{
+      v.to<double>() * std::cos(x(2)), v.to<double>() * std::sin(x(2)),
+      ((vr - vl) / (2.0 * rb)).to<double>(),
       k1.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
-      k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
-  result(4) =
+          k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>()),
       k2.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
-      k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
-  return result;
+          k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>())};
 }
 
-Eigen::Matrix<double, 3, 1> LocalMeasurementModel(
-    const Eigen::Matrix<double, 5, 1>& x,
-    const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 3> LocalMeasurementModel(
+    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
   static_cast<void>(u);
-  Eigen::Matrix<double, 3, 1> y;
-  y << x(2), x(3), x(4);
-  return y;
+  return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
 }
 
-Eigen::Matrix<double, 5, 1> GlobalMeasurementModel(
-    const Eigen::Matrix<double, 5, 1>& x,
-    const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 5> GlobalMeasurementModel(
+    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
   static_cast<void>(u);
-  Eigen::Matrix<double, 5, 1> y;
-  y << x(0), x(1), x(2), x(3), x(4);
-  return y;
+  return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
 }
 }  // namespace
 
 TEST(UnscentedKalmanFilterTest, Init) {
-  constexpr auto dt = 0.00505_s;
+  constexpr auto dt = 5_ms;
 
   frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
                                                LocalMeasurementModel,
                                                {0.5, 0.5, 10.0, 1.0, 1.0},
                                                {0.0001, 0.01, 0.01},
                                                dt};
-  Eigen::Matrix<double, 2, 1> u;
-  u << 12.0, 12.0;
+  Eigen::Vector<double, 2> u{12.0, 12.0};
   observer.Predict(u, dt);
 
   auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -98,7 +87,7 @@ TEST(UnscentedKalmanFilterTest, Init) {
 }
 
 TEST(UnscentedKalmanFilterTest, Convergence) {
-  constexpr auto dt = 0.00505_s;
+  constexpr auto dt = 5_ms;
   constexpr auto rb = 0.8382_m / 2.0;  // Robot radius
 
   frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
@@ -113,19 +102,17 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
   auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
       waypoints, {8.8_mps, 0.1_mps_sq});
 
-  Eigen::Matrix<double, 5, 1> r = Eigen::Matrix<double, 5, 1>::Zero();
+  Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
+  Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
 
-  Eigen::Matrix<double, 5, 1> nextR;
-  Eigen::Matrix<double, 2, 1> u = Eigen::Matrix<double, 2, 1>::Zero();
+  auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics,
+                                            Eigen::Vector<double, 5>::Zero(),
+                                            Eigen::Vector<double, 2>::Zero());
 
-  auto B = frc::NumericalJacobianU<5, 5, 2>(
-      Dynamics, Eigen::Matrix<double, 5, 1>::Zero(),
-      Eigen::Matrix<double, 2, 1>::Zero());
-
-  observer.SetXhat(frc::MakeMatrix<5, 1>(
+  observer.SetXhat(Eigen::Vector<double, 5>{
       trajectory.InitialPose().Translation().X().to<double>(),
       trajectory.InitialPose().Translation().Y().to<double>(),
-      trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0));
+      trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0});
 
   auto trueXhat = observer.Xhat();
 
@@ -137,19 +124,18 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
     units::meters_per_second_t vr =
         ref.velocity * (1 + (ref.curvature * rb).to<double>());
 
-    nextR(0) = ref.pose.Translation().X().to<double>();
-    nextR(1) = ref.pose.Translation().Y().to<double>();
-    nextR(2) = ref.pose.Rotation().Radians().to<double>();
-    nextR(3) = vl.to<double>();
-    nextR(4) = vr.to<double>();
+    Eigen::Vector<double, 5> nextR{ref.pose.Translation().X().to<double>(),
+                                   ref.pose.Translation().Y().to<double>(),
+                                   ref.pose.Rotation().Radians().to<double>(),
+                                   vl.to<double>(), vr.to<double>()};
 
     auto localY =
-        LocalMeasurementModel(trueXhat, Eigen::Matrix<double, 2, 1>::Zero());
+        LocalMeasurementModel(trueXhat, Eigen::Vector<double, 2>::Zero());
     observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
 
-    Eigen::Matrix<double, 5, 1> rdot = (nextR - r) / dt.to<double>();
-    u = B.householderQr().solve(
-        rdot - Dynamics(r, Eigen::Matrix<double, 2, 1>::Zero()));
+    Eigen::Vector<double, 5> rdot = (nextR - r) / dt.to<double>();
+    u = B.householderQr().solve(rdot -
+                                Dynamics(r, Eigen::Vector<double, 2>::Zero()));
 
     observer.Predict(u, dt);
 
diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
index 55862d476a..e24517d4ca 100644
--- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
@@ -15,20 +15,17 @@
 // Check that for a simple second-order system that we can easily analyze
 // analytically,
 TEST(DiscretizationTest, DiscretizeA) {
-  Eigen::Matrix<double, 2, 2> contA;
-  contA << 0, 1, 0, 0;
+  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
 
-  Eigen::Matrix<double, 2, 1> x0;
-  x0 << 1, 1;
+  Eigen::Vector<double, 2> x0{1, 1};
   Eigen::Matrix<double, 2, 2> discA;
 
   frc::DiscretizeA<2>(contA, 1_s, &discA);
-  Eigen::Matrix<double, 2, 1> x1Discrete = discA * x0;
+  Eigen::Vector<double, 2> x1Discrete = discA * x0;
 
   // We now have pos = vel = 1 and accel = 0, which should give us:
-  Eigen::Matrix<double, 2, 1> x1Truth;
-  x1Truth(0) = 1.0 * x0(0) + 1.0 * x0(1);
-  x1Truth(1) = 0.0 * x0(0) + 1.0 * x0(1);
+  Eigen::Vector<double, 2> x1Truth{1.0 * x0(0) + 1.0 * x0(1),
+                                   0.0 * x0(0) + 1.0 * x0(1)};
 
   EXPECT_EQ(x1Truth, x1Discrete);
 }
@@ -36,26 +33,20 @@ TEST(DiscretizationTest, DiscretizeA) {
 // Check that for a simple second-order system that we can easily analyze
 // analytically,
 TEST(DiscretizationTest, DiscretizeAB) {
-  Eigen::Matrix<double, 2, 2> contA;
-  contA << 0, 1, 0, 0;
+  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
+  Eigen::Matrix<double, 2, 1> contB{0, 1};
 
-  Eigen::Matrix<double, 2, 1> contB;
-  contB << 0, 1;
-
-  Eigen::Matrix<double, 2, 1> x0;
-  x0 << 1, 1;
-  Eigen::Matrix<double, 1, 1> u;
-  u << 1;
+  Eigen::Vector<double, 2> x0{1, 1};
+  Eigen::Vector<double, 1> u{1};
   Eigen::Matrix<double, 2, 2> discA;
   Eigen::Matrix<double, 2, 1> discB;
 
   frc::DiscretizeAB<2, 1>(contA, contB, 1_s, &discA, &discB);
-  Eigen::Matrix<double, 2, 1> x1Discrete = discA * x0 + discB * u;
+  Eigen::Vector<double, 2> x1Discrete = discA * x0 + discB * u;
 
   // We now have pos = vel = accel = 1, which should give us:
-  Eigen::Matrix<double, 2, 1> x1Truth;
-  x1Truth(0) = 1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0);
-  x1Truth(1) = 0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0);
+  Eigen::Vector<double, 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)};
 
   EXPECT_EQ(x1Truth, x1Discrete);
 }
@@ -64,11 +55,8 @@ TEST(DiscretizationTest, DiscretizeAB) {
 // Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
 //                                             0
 TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
-  Eigen::Matrix<double, 2, 2> contA;
-  contA << 0, 1, 0, 0;
-
-  Eigen::Matrix<double, 2, 2> contQ;
-  contQ << 1, 0, 0, 1;
+  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
+  Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
 
   constexpr auto dt = 1_s;
 
@@ -97,11 +85,8 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
 // Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
 //                                             0
 TEST(DiscretizationTest, DiscretizeFastModelAQ) {
-  Eigen::Matrix<double, 2, 2> contA;
-  contA << 0, 1, 0, -1406.29;
-
-  Eigen::Matrix<double, 2, 2> contQ;
-  contQ << 0.0025, 0, 0, 1;
+  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, -1406.29}};
+  Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
 
   constexpr auto dt = 5_ms;
 
@@ -128,11 +113,8 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
 
 // Test that the Taylor series discretization produces nearly identical results.
 TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
-  Eigen::Matrix<double, 2, 2> contA;
-  contA << 0, 1, 0, 0;
-
-  Eigen::Matrix<double, 2, 2> contQ;
-  contQ << 1, 0, 0, 1;
+  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
+  Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
 
   constexpr auto dt = 1_s;
 
@@ -141,7 +123,7 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
   Eigen::Matrix<double, 2, 2> discATaylor;
 
   // Continuous Q should be positive semidefinite
-  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont{contQ};
   for (int i = 0; i < contQ.rows(); ++i) {
     EXPECT_GE(esCont.eigenvalues()[i], 0);
   }
@@ -167,7 +149,7 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
   EXPECT_LT((discA - discATaylor).norm(), 1e-10);
 
   // Discrete Q should be positive semidefinite
-  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc{discQTaylor};
   for (int i = 0; i < discQTaylor.rows(); ++i) {
     EXPECT_GE(esDisc.eigenvalues()[i], 0);
   }
@@ -175,11 +157,8 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
 
 // Test that the Taylor series discretization produces nearly identical results.
 TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
-  Eigen::Matrix<double, 2, 2> contA;
-  contA << 0, 1, 0, -1500;
-
-  Eigen::Matrix<double, 2, 2> contQ;
-  contQ << 0.0025, 0, 0, 1;
+  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, -1500}};
+  Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
 
   constexpr auto dt = 5_ms;
 
@@ -222,11 +201,8 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
 
 // Test that DiscretizeR() works
 TEST(DiscretizationTest, DiscretizeR) {
-  Eigen::Matrix<double, 2, 2> contR;
-  contR << 2.0, 0.0, 0.0, 1.0;
-
-  Eigen::Matrix<double, 2, 2> discRTruth;
-  discRTruth << 4.0, 0.0, 0.0, 2.0;
+  Eigen::Matrix<double, 2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};
+  Eigen::Matrix<double, 2, 2> discRTruth{{4.0, 0.0}, {0.0, 2.0}};
 
   Eigen::Matrix<double, 2, 2> discR = frc::DiscretizeR<2>(contR, 500_ms);
 
diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
index 3949c60648..1fa12c2575 100644
--- a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
+++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
@@ -7,7 +7,6 @@
 #include <frc/system/plant/LinearSystemId.h>
 #include <gtest/gtest.h>
 
-#include "frc/StateSpaceUtil.h"
 #include "frc/system/plant/LinearSystemId.h"
 #include "units/length.h"
 #include "units/mass.h"
@@ -17,32 +16,37 @@ TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) {
       frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
 
   ASSERT_TRUE(model.A().isApprox(
-      frc::MakeMatrix<2, 2>(-10.14132, 3.06598, 3.06598, -10.14132), 0.001));
+      Eigen::Matrix<double, 2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}},
+      0.001));
   ASSERT_TRUE(model.B().isApprox(
-      frc::MakeMatrix<2, 2>(4.2590, -1.28762, -1.2876, 4.2590), 0.001));
-  ASSERT_TRUE(
-      model.C().isApprox(frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0), 0.001));
-  ASSERT_TRUE(
-      model.D().isApprox(frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0), 0.001));
+      Eigen::Matrix<double, 2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}},
+      0.001));
+  ASSERT_TRUE(model.C().isApprox(
+      Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
+  ASSERT_TRUE(model.D().isApprox(
+      Eigen::Matrix<double, 2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
 }
 
 TEST(LinearSystemIDTest, ElevatorSystem) {
   auto model = frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), 5_kg,
                                                    0.05_m, 12);
   ASSERT_TRUE(model.A().isApprox(
-      frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -99.05473), 0.001));
-  ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<2, 1>(0.0, 20.8), 0.001));
-  ASSERT_TRUE(model.C().isApprox(frc::MakeMatrix<1, 2>(1.0, 0.0), 0.001));
-  ASSERT_TRUE(model.D().isApprox(frc::MakeMatrix<1, 1>(0.0), 0.001));
+      Eigen::Matrix<double, 2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
+  ASSERT_TRUE(
+      model.B().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 20.8}, 0.001));
+  ASSERT_TRUE(model.C().isApprox(Eigen::Matrix<double, 1, 2>{1.0, 0.0}, 0.001));
+  ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
 }
 
 TEST(LinearSystemIDTest, FlywheelSystem) {
   auto model = frc::LinearSystemId::FlywheelSystem(frc::DCMotor::NEO(2),
                                                    0.00032_kg_sq_m, 1.0);
-  ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<1, 1>(-26.87032), 0.001));
-  ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<1, 1>(1354.166667), 0.001));
-  ASSERT_TRUE(model.C().isApprox(frc::MakeMatrix<1, 1>(1.0), 0.001));
-  ASSERT_TRUE(model.D().isApprox(frc::MakeMatrix<1, 1>(0.0), 0.001));
+  ASSERT_TRUE(
+      model.A().isApprox(Eigen::Matrix<double, 1, 1>{-26.87032}, 0.001));
+  ASSERT_TRUE(
+      model.B().isApprox(Eigen::Matrix<double, 1, 1>{1354.166667}, 0.001));
+  ASSERT_TRUE(model.C().isApprox(Eigen::Matrix<double, 1, 1>{1.0}, 0.001));
+  ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
 }
 
 TEST(LinearSystemIDTest, IdentifyPositionSystem) {
@@ -53,9 +57,10 @@ TEST(LinearSystemIDTest, IdentifyPositionSystem) {
   auto model = frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
       kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
 
-  ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -kv / ka),
-                                 0.001));
-  ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<2, 1>(0.0, 1.0 / ka), 0.001));
+  ASSERT_TRUE(model.A().isApprox(
+      Eigen::Matrix<double, 2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
+  ASSERT_TRUE(
+      model.B().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 1.0 / ka}, 0.001));
 }
 
 TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
@@ -67,6 +72,6 @@ TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
   auto model = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
       kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
 
-  ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<1, 1>(-kv / ka), 0.001));
-  ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<1, 1>(1.0 / ka), 0.001));
+  ASSERT_TRUE(model.A().isApprox(Eigen::Matrix<double, 1, 1>{-kv / ka}, 0.001));
+  ASSERT_TRUE(model.B().isApprox(Eigen::Matrix<double, 1, 1>{1.0 / ka}, 0.001));
 }
diff --git a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
index b644ddc25f..fd9c039ca0 100644
--- a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
@@ -10,14 +10,11 @@
 
 // Tests that integrating dx/dt = e^x works.
 TEST(NumericalIntegrationTest, Exponential) {
-  Eigen::Matrix<double, 1, 1> y0;
-  y0(0) = 0.0;
+  Eigen::Vector<double, 1> y0{0.0};
 
-  Eigen::Matrix<double, 1, 1> y1 = frc::RK4(
-      [](Eigen::Matrix<double, 1, 1> x) {
-        Eigen::Matrix<double, 1, 1> y;
-        y(0) = std::exp(x(0));
-        return y;
+  Eigen::Vector<double, 1> y1 = frc::RK4(
+      [](const Eigen::Vector<double, 1>& x) {
+        return Eigen::Vector<double, 1>{std::exp(x(0))};
       },
       y0, 0.1_s);
   EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
@@ -25,45 +22,36 @@ TEST(NumericalIntegrationTest, Exponential) {
 
 // Tests that integrating dx/dt = e^x works when we provide a U.
 TEST(NumericalIntegrationTest, ExponentialWithU) {
-  Eigen::Matrix<double, 1, 1> y0;
-  y0(0) = 0.0;
+  Eigen::Vector<double, 1> y0{0.0};
 
-  Eigen::Matrix<double, 1, 1> y1 = frc::RK4(
-      [](Eigen::Matrix<double, 1, 1> x, Eigen::Matrix<double, 1, 1> u) {
-        Eigen::Matrix<double, 1, 1> y;
-        y(0) = std::exp(u(0) * x(0));
-        return y;
+  Eigen::Vector<double, 1> y1 = frc::RK4(
+      [](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
+        return Eigen::Vector<double, 1>{std::exp(u(0) * x(0))};
       },
-      y0, (Eigen::Matrix<double, 1, 1>() << 1.0).finished(), 0.1_s);
+      y0, Eigen::Vector<double, 1>{1.0}, 0.1_s);
   EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
 }
 
 // Tests that integrating dx/dt = e^x works with RKF45.
 TEST(NumericalIntegrationTest, ExponentialRKF45) {
-  Eigen::Matrix<double, 1, 1> y0;
-  y0(0) = 0.0;
+  Eigen::Vector<double, 1> y0{0.0};
 
-  Eigen::Matrix<double, 1, 1> y1 = frc::RKF45(
-      [](Eigen::Matrix<double, 1, 1> x, Eigen::Matrix<double, 1, 1> u) {
-        Eigen::Matrix<double, 1, 1> y;
-        y(0) = std::exp(x(0));
-        return y;
+  Eigen::Vector<double, 1> y1 = frc::RKF45(
+      [](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
+        return Eigen::Vector<double, 1>{std::exp(x(0))};
       },
-      y0, (Eigen::Matrix<double, 1, 1>() << 0.0).finished(), 0.1_s);
+      y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
   EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
 }
 
 // Tests that integrating dx/dt = e^x works with RKDP
 TEST(NumericalIntegrationTest, ExponentialRKDP) {
-  Eigen::Matrix<double, 1, 1> y0;
-  y0(0) = 0.0;
+  Eigen::Vector<double, 1> y0{0.0};
 
-  Eigen::Matrix<double, 1, 1> y1 = frc::RKDP(
-      [](Eigen::Matrix<double, 1, 1> x, Eigen::Matrix<double, 1, 1> u) {
-        Eigen::Matrix<double, 1, 1> y;
-        y(0) = std::exp(x(0));
-        return y;
+  Eigen::Vector<double, 1> y1 = frc::RKDP(
+      [](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
+        return Eigen::Vector<double, 1>{std::exp(x(0))};
       },
-      y0, (Eigen::Matrix<double, 1, 1>() << 0.0).finished(), 0.1_s);
+      y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
   EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
 }
diff --git a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
index 66661aaef5..4e64825e12 100644
--- a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
+++ b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
@@ -6,60 +6,53 @@
 
 #include "frc/system/NumericalJacobian.h"
 
-Eigen::Matrix<double, 4, 4> A = (Eigen::Matrix<double, 4, 4>() << 1, 2, 4, 1, 5,
-                                 2, 3, 4, 5, 1, 3, 2, 1, 1, 3, 7)
-                                    .finished();
-
-Eigen::Matrix<double, 4, 2> B =
-    (Eigen::Matrix<double, 4, 2>() << 1, 1, 2, 1, 3, 2, 3, 7).finished();
+Eigen::Matrix<double, 4, 4> A{
+    {1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}};
+Eigen::Matrix<double, 4, 2> B{{1, 1}, {2, 1}, {3, 2}, {3, 7}};
 
 // Function from which to recover A and B
-Eigen::Matrix<double, 4, 1> AxBuFn(const Eigen::Matrix<double, 4, 1>& x,
-                                   const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 4> AxBuFn(const Eigen::Vector<double, 4>& x,
+                                const Eigen::Vector<double, 2>& u) {
   return A * x + B * u;
 }
 
 // Test that we can recover A from AxBuFn() pretty accurately
 TEST(NumericalJacobianTest, Ax) {
-  Eigen::Matrix<double, 4, 4> newA = frc::NumericalJacobianX<4, 4, 2>(
-      AxBuFn, Eigen::Matrix<double, 4, 1>::Zero(),
-      Eigen::Matrix<double, 2, 1>::Zero());
+  Eigen::Matrix<double, 4, 4> newA =
+      frc::NumericalJacobianX<4, 4, 2>(AxBuFn, Eigen::Vector<double, 4>::Zero(),
+                                       Eigen::Vector<double, 2>::Zero());
   EXPECT_TRUE(newA.isApprox(A));
 }
 
 // Test that we can recover B from AxBuFn() pretty accurately
 TEST(NumericalJacobianTest, Bu) {
-  Eigen::Matrix<double, 4, 2> newB = frc::NumericalJacobianU<4, 4, 2>(
-      AxBuFn, Eigen::Matrix<double, 4, 1>::Zero(),
-      Eigen::Matrix<double, 2, 1>::Zero());
+  Eigen::Matrix<double, 4, 2> newB =
+      frc::NumericalJacobianU<4, 4, 2>(AxBuFn, Eigen::Vector<double, 4>::Zero(),
+                                       Eigen::Vector<double, 2>::Zero());
   EXPECT_TRUE(newB.isApprox(B));
 }
 
-Eigen::Matrix<double, 3, 4> C =
-    (Eigen::Matrix<double, 3, 4>() << 1, 2, 4, 1, 5, 2, 3, 4, 5, 1, 3, 2)
-        .finished();
-
-Eigen::Matrix<double, 3, 2> D =
-    (Eigen::Matrix<double, 3, 2>() << 1, 1, 2, 1, 3, 2).finished();
+Eigen::Matrix<double, 3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}};
+Eigen::Matrix<double, 3, 2> D{{1, 1}, {2, 1}, {3, 2}};
 
 // Function from which to recover C and D
-Eigen::Matrix<double, 3, 1> CxDuFn(const Eigen::Matrix<double, 4, 1>& x,
-                                   const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 3> CxDuFn(const Eigen::Vector<double, 4>& x,
+                                const Eigen::Vector<double, 2>& u) {
   return C * x + D * u;
 }
 
 // Test that we can recover C from CxDuFn() pretty accurately
 TEST(NumericalJacobianTest, Cx) {
-  Eigen::Matrix<double, 3, 4> newC = frc::NumericalJacobianX<3, 4, 2>(
-      CxDuFn, Eigen::Matrix<double, 4, 1>::Zero(),
-      Eigen::Matrix<double, 2, 1>::Zero());
+  Eigen::Matrix<double, 3, 4> newC =
+      frc::NumericalJacobianX<3, 4, 2>(CxDuFn, Eigen::Vector<double, 4>::Zero(),
+                                       Eigen::Vector<double, 2>::Zero());
   EXPECT_TRUE(newC.isApprox(C));
 }
 
 // Test that we can recover D from CxDuFn() pretty accurately
 TEST(NumericalJacobianTest, Du) {
-  Eigen::Matrix<double, 3, 2> newD = frc::NumericalJacobianU<3, 4, 2>(
-      CxDuFn, Eigen::Matrix<double, 4, 1>::Zero(),
-      Eigen::Matrix<double, 2, 1>::Zero());
+  Eigen::Matrix<double, 3, 2> newD =
+      frc::NumericalJacobianU<3, 4, 2>(CxDuFn, Eigen::Vector<double, 4>::Zero(),
+                                       Eigen::Vector<double, 2>::Zero());
   EXPECT_TRUE(newD.isApprox(D));
 }
diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
index 334712901c..8a7217618f 100644
--- a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
+++ b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
@@ -9,10 +9,9 @@
 #include "frc/system/RungeKuttaTimeVarying.h"
 
 namespace {
-Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
-  return (Eigen::Matrix<double, 1, 1>()
-          << 12.0 * std::exp(t) / (std::pow(std::exp(t) + 1.0, 2.0)))
-      .finished();
+Eigen::Vector<double, 1> RungeKuttaTimeVaryingSolution(double t) {
+  return Eigen::Vector<double, 1>{12.0 * std::exp(t) /
+                                  (std::pow(std::exp(t) + 1.0, 2.0))};
 }
 }  // namespace
 
@@ -24,13 +23,12 @@ Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
 //
 // x(t) = 12 * e^t / ((e^t + 1)^2)
 TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) {
-  Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
+  Eigen::Vector<double, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
 
-  Eigen::Matrix<double, 1, 1> y1 = frc::RungeKuttaTimeVarying(
-      [](units::second_t t, Eigen::Matrix<double, 1, 1> x) {
-        return (Eigen::Matrix<double, 1, 1>()
-                << x(0) * (2.0 / (std::exp(t.to<double>()) + 1.0) - 1.0))
-            .finished();
+  Eigen::Vector<double, 1> y1 = frc::RungeKuttaTimeVarying(
+      [](units::second_t t, const Eigen::Vector<double, 1>& x) {
+        return Eigen::Vector<double, 1>{
+            x(0) * (2.0 / (std::exp(t.to<double>()) + 1.0) - 1.0)};
       },
       5_s, y0, 1_s);
   EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);