[wpimath] Clean up Eigen usage

* Replace Matrix<> with Vector<> where vectors are explicitly intended.
  I found these via `rg "Eigen::Matrix<double, \w+, 1>"`.
* Pass all Eigen matrices by const reference. I found these via `rg
  "\(Eigen"` on main (the initializer list constructors make more false
  positives).
* Replace MakeMatrix() and operator<< usage with initializer list
  constructors. I found these via `rg MakeMatrix` and `rg "<<"`
  respectively.
* Deprecate MakeMatrix()

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

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

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;

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)};