[wpimath] Add typedefs for common types

This makes complex code significantly easier to read.

frc::Vectord<Size> = Eigen::Vector<double, Size>
frc::Matrixd<Rows, Cols> = Eigen::Matrix<double, Rows, Cols>

wpimath/src/main/native/include/frc/estimator/AngleStatistics.h

@@ -6,7 +6,7 @@
 
 #include <wpi/numbers>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/MathUtil.h"
 
 namespace frc {
@@ -21,10 +21,9 @@ namespace frc {
  * @param angleStateIdx The row containing angles to be normalized.
  */
 template <int States>
-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;
+Vectord<States> AngleResidual(const Vectord<States>& a,
+                              const Vectord<States>& b, int angleStateIdx) {
+  Vectord<States> ret = a - b;
   ret[angleStateIdx] =
       AngleModulus(units::radian_t{ret[angleStateIdx]}).value();
   return ret;
@@ -38,8 +37,7 @@ Eigen::Vector<double, States> AngleResidual(
  * @param angleStateIdx The row containing angles to be normalized.
  */
 template <int States>
-std::function<Eigen::Vector<double, States>(
-    const Eigen::Vector<double, States>&, const Eigen::Vector<double, States>&)>
+std::function<Vectord<States>(const Vectord<States>&, const Vectord<States>&)>
 AngleResidual(int angleStateIdx) {
   return [=](auto a, auto b) {
     return AngleResidual<States>(a, b, angleStateIdx);
@@ -56,10 +54,9 @@ AngleResidual(int angleStateIdx) {
  * @param angleStateIdx The row containing angles to be normalized.
  */
 template <int States>
-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;
+Vectord<States> AngleAdd(const Vectord<States>& a, const Vectord<States>& b,
+                         int angleStateIdx) {
+  Vectord<States> ret = a + b;
   ret[angleStateIdx] =
       InputModulus(ret[angleStateIdx], -wpi::numbers::pi, wpi::numbers::pi);
   return ret;
@@ -73,8 +70,7 @@ Eigen::Vector<double, States> AngleAdd(const Eigen::Vector<double, States>& a,
  * @param angleStateIdx The row containing angles to be normalized.
  */
 template <int States>
-std::function<Eigen::Vector<double, States>(
-    const Eigen::Vector<double, States>&, const Eigen::Vector<double, States>&)>
+std::function<Vectord<States>(const Vectord<States>&, const Vectord<States>&)>
 AngleAdd(int angleStateIdx) {
   return [=](auto a, auto b) { return AngleAdd<States>(a, b, angleStateIdx); };
 }
@@ -91,9 +87,9 @@ AngleAdd(int angleStateIdx) {
  * @param angleStatesIdx The row containing the angles.
  */
 template <int CovDim, int States>
-Eigen::Vector<double, CovDim> AngleMean(
-    const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
-    const Eigen::Vector<double, 2 * States + 1>& Wm, int angleStatesIdx) {
+Vectord<CovDim> AngleMean(const Matrixd<CovDim, 2 * States + 1>& sigmas,
+                          const Vectord<2 * States + 1>& Wm,
+                          int angleStatesIdx) {
   double sumSin = sigmas.row(angleStatesIdx)
                       .unaryExpr([](auto it) { return std::sin(it); })
                       .sum();
@@ -101,7 +97,7 @@ Eigen::Vector<double, CovDim> AngleMean(
                       .unaryExpr([](auto it) { return std::cos(it); })
                       .sum();
 
-  Eigen::Vector<double, CovDim> ret = sigmas * Wm;
+  Vectord<CovDim> ret = sigmas * Wm;
   ret[angleStatesIdx] = std::atan2(sumSin, sumCos);
   return ret;
 }
@@ -116,9 +112,8 @@ Eigen::Vector<double, CovDim> AngleMean(
  * @param angleStateIdx The row containing the angles.
  */
 template <int CovDim, int States>
-std::function<Eigen::Vector<double, CovDim>(
-    const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
-    const Eigen::Vector<double, 2 * States + 1>&)>
+std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
+                              const Vectord<2 * States + 1>&)>
 AngleMean(int angleStateIdx) {
   return [=](auto sigmas, auto Wm) {
     return AngleMean<CovDim, States>(sigmas, Wm, angleStateIdx);

wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h

@@ -7,7 +7,7 @@
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/estimator/UnscentedKalmanFilter.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
@@ -223,11 +223,9 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
  private:
   UnscentedKalmanFilter<5, 3, 3> m_observer;
   TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
-  std::function<void(const Eigen::Vector<double, 3>& u,
-                     const Eigen::Vector<double, 3>& y)>
-      m_visionCorrect;
+  std::function<void(const Vectord<3>& u, const Vectord<3>& y)> m_visionCorrect;
 
-  Eigen::Matrix<double, 3, 3> m_visionContR;
+  Matrixd<3, 3> m_visionContR;
 
   units::second_t m_nominalDt;
   units::second_t m_prevTime = -1_s;
@@ -237,13 +235,12 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
 
   template <int Dim>
   static wpi::array<double, Dim> StdDevMatrixToArray(
-      const Eigen::Vector<double, Dim>& stdDevs);
+      const Vectord<Dim>& stdDevs);
 
-  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);
+  static Vectord<5> F(const Vectord<5>& x, const Vectord<3>& u);
+  static Vectord<5> FillStateVector(const Pose2d& pose,
+                                    units::meter_t leftDistance,
+                                    units::meter_t rightDistance);
 };
 
 }  // namespace frc

wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h

@@ -8,7 +8,7 @@
 
 #include <wpi/array.h>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "units/time.h"
 
 namespace frc {
@@ -40,6 +40,15 @@ namespace frc {
 template <int States, int Inputs, int Outputs>
 class ExtendedKalmanFilter {
  public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+  using OutputVector = Vectord<Outputs>;
+
+  using StateArray = wpi::array<double, States>;
+  using OutputArray = wpi::array<double, Outputs>;
+
+  using StateMatrix = Matrixd<States, States>;
+
   /**
    * Constructs an extended Kalman filter.
    *
@@ -51,17 +60,11 @@ class ExtendedKalmanFilter {
    * @param measurementStdDevs Standard deviations of measurements.
    * @param dt                 Nominal discretization timestep.
    */
-  ExtendedKalmanFilter(std::function<Eigen::Vector<double, States>(
-                           const Eigen::Vector<double, States>&,
-                           const Eigen::Vector<double, Inputs>&)>
-                           f,
-                       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,
-                       units::second_t dt);
+  ExtendedKalmanFilter(
+      std::function<StateVector(const StateVector&, const InputVector&)> f,
+      std::function<OutputVector(const StateVector&, const InputVector&)> h,
+      const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+      units::second_t dt);
 
   /**
    * Constructs an extended Kalman filter.
@@ -77,30 +80,20 @@ class ExtendedKalmanFilter {
    * @param addFuncX           A function that adds two state vectors.
    * @param dt                 Nominal discretization timestep.
    */
-  ExtendedKalmanFilter(std::function<Eigen::Vector<double, States>(
-                           const Eigen::Vector<double, States>&,
-                           const Eigen::Vector<double, Inputs>&)>
-                           f,
-                       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::Vector<double, Outputs>(
-                           const Eigen::Vector<double, Outputs>&,
-                           const Eigen::Vector<double, Outputs>&)>
-                           residualFuncY,
-                       std::function<Eigen::Vector<double, States>(
-                           const Eigen::Vector<double, States>&,
-                           const Eigen::Vector<double, States>&)>
-                           addFuncX,
-                       units::second_t dt);
+  ExtendedKalmanFilter(
+      std::function<StateVector(const StateVector&, const InputVector&)> f,
+      std::function<OutputVector(const StateVector&, const InputVector&)> h,
+      const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+      std::function<OutputVector(const OutputVector&, const OutputVector&)>
+          residualFuncY,
+      std::function<StateVector(const StateVector&, const StateVector&)>
+          addFuncX,
+      units::second_t dt);
 
   /**
    * Returns the error covariance matrix P.
    */
-  const Eigen::Matrix<double, States, States>& P() const { return m_P; }
+  const StateMatrix& P() const { return m_P; }
 
   /**
    * Returns an element of the error covariance matrix P.
@@ -115,12 +108,12 @@ class ExtendedKalmanFilter {
    *
    * @param P The error covariance matrix P.
    */
-  void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
+  void SetP(const StateMatrix& P) { m_P = P; }
 
   /**
    * Returns the state estimate x-hat.
    */
-  const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
+  const StateVector& Xhat() const { return m_xHat; }
 
   /**
    * Returns an element of the state estimate x-hat.
@@ -134,7 +127,7 @@ class ExtendedKalmanFilter {
    *
    * @param xHat The state estimate x-hat.
    */
-  void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
+  void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
 
   /**
    * Set an element of the initial state estimate x-hat.
@@ -158,7 +151,7 @@ class ExtendedKalmanFilter {
    * @param u  New control input from controller.
    * @param dt Timestep for prediction.
    */
-  void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt);
+  void Predict(const InputVector& u, units::second_t dt);
 
   /**
    * Correct the state estimate x-hat using the measurements in y.
@@ -166,19 +159,15 @@ class ExtendedKalmanFilter {
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    */
-  void Correct(const Eigen::Vector<double, Inputs>& u,
-               const Eigen::Vector<double, Outputs>& y) {
+  void Correct(const InputVector& u, const OutputVector& y) {
     Correct<Outputs>(u, y, m_h, m_contR, m_residualFuncY, m_addFuncX);
   }
 
   template <int Rows>
-  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);
+  void Correct(
+      const InputVector& u, const Vectord<Rows>& y,
+      std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+      const Matrixd<Rows, Rows>& R);
 
   /**
    * Correct the state estimate x-hat using the measurements in y.
@@ -197,46 +186,28 @@ class ExtendedKalmanFilter {
    * @param addFuncX      A function that adds two state vectors.
    */
   template <int Rows>
-  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::Vector<double, Rows>(
-                   const Eigen::Vector<double, Rows>&,
-                   const Eigen::Vector<double, Rows>&)>
-                   residualFuncY,
-               std::function<Eigen::Vector<double, States>(
-                   const Eigen::Vector<double, States>&,
-                   const Eigen::Vector<double, States>)>
-                   addFuncX);
+  void Correct(
+      const InputVector& u, const Vectord<Rows>& y,
+      std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+      const Matrixd<Rows, Rows>& R,
+      std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
+          residualFuncY,
+      std::function<StateVector(const StateVector&, const StateVector&)>
+          addFuncX);
 
  private:
-  std::function<Eigen::Vector<double, States>(
-      const Eigen::Vector<double, States>&,
-      const Eigen::Vector<double, Inputs>&)>
-      m_f;
-  std::function<Eigen::Vector<double, Outputs>(
-      const Eigen::Vector<double, States>&,
-      const Eigen::Vector<double, Inputs>&)>
-      m_h;
-  std::function<Eigen::Vector<double, Outputs>(
-      const Eigen::Vector<double, Outputs>&,
-      const Eigen::Vector<double, Outputs>)>
+  std::function<StateVector(const StateVector&, const InputVector&)> m_f;
+  std::function<OutputVector(const StateVector&, const InputVector&)> m_h;
+  std::function<OutputVector(const OutputVector&, const OutputVector&)>
       m_residualFuncY;
-  std::function<Eigen::Vector<double, States>(
-      const Eigen::Vector<double, States>&,
-      const Eigen::Vector<double, States>)>
-      m_addFuncX;
-  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;
+  std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
+  StateVector m_xHat;
+  StateMatrix m_P;
+  StateMatrix m_contQ;
+  Matrixd<Outputs, Outputs> m_contR;
   units::second_t m_dt;
 
-  Eigen::Matrix<double, States, States> m_initP;
+  StateMatrix m_initP;
 };
 
 }  // namespace frc

wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc

@@ -16,72 +16,47 @@ namespace frc {
 
 template <int States, int Inputs, int Outputs>
 ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
-    std::function<
-        Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                      const Eigen::Vector<double, Inputs>&)>
-        f,
-    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, units::second_t dt)
+    std::function<StateVector(const StateVector&, const InputVector&)> f,
+    std::function<OutputVector(const StateVector&, const InputVector&)> h,
+    const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+    units::second_t dt)
     : m_f(f), m_h(h) {
   m_contQ = MakeCovMatrix(stateStdDevs);
   m_contR = MakeCovMatrix(measurementStdDevs);
-  m_residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Outputs> {
-    return a - b;
-  };
-  m_addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
-    return a + b;
-  };
+  m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; };
+  m_addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
   m_dt = dt;
 
   Reset();
 
-  Eigen::Matrix<double, States, States> contA =
-      NumericalJacobianX<States, States, Inputs>(
-          m_f, m_xHat, Eigen::Vector<double, Inputs>::Zero());
-  Eigen::Matrix<double, Outputs, States> C =
-      NumericalJacobianX<Outputs, States, Inputs>(
-          m_h, m_xHat, Eigen::Vector<double, Inputs>::Zero());
+  StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
+      m_f, m_xHat, InputVector::Zero());
+  Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
+      m_h, m_xHat, InputVector::Zero());
 
-  Eigen::Matrix<double, States, States> discA;
-  Eigen::Matrix<double, States, States> discQ;
+  StateMatrix discA;
+  StateMatrix discQ;
   DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
 
-  Eigen::Matrix<double, Outputs, Outputs> discR =
-      DiscretizeR<Outputs>(m_contR, dt);
+  Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
 
   if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
     m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
         discA.transpose(), C.transpose(), discQ, discR);
   } else {
-    m_initP = Eigen::Matrix<double, States, States>::Zero();
+    m_initP = StateMatrix::Zero();
   }
   m_P = m_initP;
 }
 
 template <int States, int Inputs, int Outputs>
 ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
-    std::function<
-        Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                      const Eigen::Vector<double, Inputs>&)>
-        f,
-    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::Vector<double, Outputs>(const Eigen::Vector<double, Outputs>&,
-                                       const Eigen::Vector<double, Outputs>&)>
+    std::function<StateVector(const StateVector&, const InputVector&)> f,
+    std::function<OutputVector(const StateVector&, const InputVector&)> h,
+    const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+    std::function<OutputVector(const OutputVector&, const OutputVector&)>
         residualFuncY,
-    std::function<
-        Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                      const Eigen::Vector<double, States>&)>
-        addFuncX,
+    std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
     units::second_t dt)
     : m_f(f), m_h(h), m_residualFuncY(residualFuncY), m_addFuncX(addFuncX) {
   m_contQ = MakeCovMatrix(stateStdDevs);
@@ -90,39 +65,36 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
 
   Reset();
 
-  Eigen::Matrix<double, States, States> contA =
-      NumericalJacobianX<States, States, Inputs>(
-          m_f, m_xHat, Eigen::Vector<double, Inputs>::Zero());
-  Eigen::Matrix<double, Outputs, States> C =
-      NumericalJacobianX<Outputs, States, Inputs>(
-          m_h, m_xHat, Eigen::Vector<double, Inputs>::Zero());
+  StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
+      m_f, m_xHat, InputVector::Zero());
+  Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
+      m_h, m_xHat, InputVector::Zero());
 
-  Eigen::Matrix<double, States, States> discA;
-  Eigen::Matrix<double, States, States> discQ;
+  StateMatrix discA;
+  StateMatrix discQ;
   DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
 
-  Eigen::Matrix<double, Outputs, Outputs> discR =
-      DiscretizeR<Outputs>(m_contR, dt);
+  Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
 
   if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
     m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
         discA.transpose(), C.transpose(), discQ, discR);
   } else {
-    m_initP = Eigen::Matrix<double, States, States>::Zero();
+    m_initP = StateMatrix::Zero();
   }
   m_P = m_initP;
 }
 
 template <int States, int Inputs, int Outputs>
 void ExtendedKalmanFilter<States, Inputs, Outputs>::Predict(
-    const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
+    const InputVector& u, units::second_t dt) {
   // Find continuous A
-  Eigen::Matrix<double, States, States> contA =
+  StateMatrix contA =
       NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
 
   // Find discrete A and Q
-  Eigen::Matrix<double, States, States> discA;
-  Eigen::Matrix<double, States, States> discQ;
+  StateMatrix discA;
+  StateMatrix discQ;
   DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
 
   m_xHat = RK4(m_f, m_xHat, u, dt);
@@ -136,44 +108,29 @@ void ExtendedKalmanFilter<States, Inputs, Outputs>::Predict(
 template <int States, int Inputs, int Outputs>
 template <int Rows>
 void ExtendedKalmanFilter<States, Inputs, Outputs>::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::Vector<double, Rows> {
-    return a - b;
-  };
-  auto addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
-    return a + b;
-  };
+    const InputVector& u, const Vectord<Rows>& y,
+    std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+    const Matrixd<Rows, Rows>& R) {
+  auto residualFuncY = [](auto a, auto b) -> Vectord<Rows> { return a - b; };
+  auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
   Correct<Rows>(u, y, h, R, residualFuncY, addFuncX);
 }
 
 template <int States, int Inputs, int Outputs>
 template <int Rows>
 void ExtendedKalmanFilter<States, Inputs, Outputs>::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::Vector<double, Rows>(
-        const Eigen::Vector<double, Rows>&, const Eigen::Vector<double, Rows>&)>
+    const InputVector& u, const Vectord<Rows>& y,
+    std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+    const Matrixd<Rows, Rows>& R,
+    std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
         residualFuncY,
-    std::function<
-        Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                      const Eigen::Vector<double, States>)>
+    std::function<StateVector(const StateVector&, const StateVector&)>
         addFuncX) {
-  const Eigen::Matrix<double, Rows, States> C =
+  const Matrixd<Rows, States> C =
       NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
-  const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
+  const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
 
-  Eigen::Matrix<double, Rows, Rows> S = C * m_P * C.transpose() + discR;
+  Matrixd<Rows, Rows> S = C * m_P * C.transpose() + discR;
 
   // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
   // efficiently.
@@ -187,7 +144,7 @@ void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
   //
   // Kᵀ = Sᵀ.solve(CPᵀ)
   // K = (Sᵀ.solve(CPᵀ))ᵀ
-  Eigen::Matrix<double, States, Rows> K =
+  Matrixd<States, Rows> K =
       S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
 
   // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
@@ -195,9 +152,8 @@ void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
 
   // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
   // Use Joseph form for numerical stability
-  m_P = (Eigen::Matrix<double, States, States>::Identity() - K * C) * m_P *
-            (Eigen::Matrix<double, States, States>::Identity() - K * C)
-                .transpose() +
+  m_P = (StateMatrix::Identity() - K * C) * m_P *
+            (StateMatrix::Identity() - K * C).transpose() +
         K * discR * K.transpose();
 }
 

wpimath/src/main/native/include/frc/estimator/KalmanFilter.h

@@ -7,7 +7,7 @@
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/system/LinearSystem.h"
 #include "units/time.h"
 
@@ -36,6 +36,13 @@ namespace frc {
 template <int States, int Inputs, int Outputs>
 class KalmanFilter {
  public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+  using OutputVector = Vectord<Outputs>;
+
+  using StateArray = wpi::array<double, States>;
+  using OutputArray = wpi::array<double, Outputs>;
+
   /**
    * Constructs a state-space observer with the given plant.
    *
@@ -45,9 +52,8 @@ class KalmanFilter {
    * @param dt                 Nominal discretization timestep.
    */
   KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
-               const wpi::array<double, States>& stateStdDevs,
-               const wpi::array<double, Outputs>& measurementStdDevs,
-               units::second_t dt);
+               const StateArray& stateStdDevs,
+               const OutputArray& measurementStdDevs, units::second_t dt);
 
   KalmanFilter(KalmanFilter&&) = default;
   KalmanFilter& operator=(KalmanFilter&&) = default;
@@ -55,7 +61,7 @@ class KalmanFilter {
   /**
    * Returns the steady-state Kalman gain matrix K.
    */
-  const Eigen::Matrix<double, States, Outputs>& K() const { return m_K; }
+  const Matrixd<States, Outputs>& K() const { return m_K; }
 
   /**
    * Returns an element of the steady-state Kalman gain matrix K.
@@ -68,7 +74,7 @@ class KalmanFilter {
   /**
    * Returns the state estimate x-hat.
    */
-  const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
+  const StateVector& Xhat() const { return m_xHat; }
 
   /**
    * Returns an element of the state estimate x-hat.
@@ -82,7 +88,7 @@ class KalmanFilter {
    *
    * @param xHat The state estimate x-hat.
    */
-  void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
+  void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
 
   /**
    * Set an element of the initial state estimate x-hat.
@@ -103,7 +109,7 @@ class KalmanFilter {
    * @param u  New control input from controller.
    * @param dt Timestep for prediction.
    */
-  void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt);
+  void Predict(const InputVector& u, units::second_t dt);
 
   /**
    * Correct the state estimate x-hat using the measurements in y.
@@ -111,8 +117,7 @@ class KalmanFilter {
    * @param u Same control input used in the last predict step.
    * @param y Measurement vector.
    */
-  void Correct(const Eigen::Vector<double, Inputs>& u,
-               const Eigen::Vector<double, Outputs>& y);
+  void Correct(const InputVector& u, const OutputVector& y);
 
  private:
   LinearSystem<States, Inputs, Outputs>* m_plant;
@@ -120,12 +125,12 @@ class KalmanFilter {
   /**
    * The steady-state Kalman gain matrix.
    */
-  Eigen::Matrix<double, States, Outputs> m_K;
+  Matrixd<States, Outputs> m_K;
 
   /**
    * The state estimate.
    */
-  Eigen::Vector<double, States> m_xHat;
+  StateVector m_xHat;
 };
 
 extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)

wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc

@@ -21,15 +21,15 @@ namespace frc {
 template <int States, int Inputs, int Outputs>
 KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
     LinearSystem<States, Inputs, Outputs>& plant,
-    const wpi::array<double, States>& stateStdDevs,
-    const wpi::array<double, Outputs>& measurementStdDevs, units::second_t dt) {
+    const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+    units::second_t dt) {
   m_plant = &plant;
 
   auto contQ = MakeCovMatrix(stateStdDevs);
   auto contR = MakeCovMatrix(measurementStdDevs);
 
-  Eigen::Matrix<double, States, States> discA;
-  Eigen::Matrix<double, States, States> discQ;
+  Matrixd<States, States> discA;
+  Matrixd<States, States> discQ;
   DiscretizeAQTaylor<States>(plant.A(), contQ, dt, &discA, &discQ);
 
   auto discR = DiscretizeR<Outputs>(contR, dt);
@@ -46,12 +46,11 @@ KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
     throw std::invalid_argument(msg);
   }
 
-  Eigen::Matrix<double, States, States> P =
-      drake::math::DiscreteAlgebraicRiccatiEquation(
-          discA.transpose(), C.transpose(), discQ, discR);
+  Matrixd<States, States> P = drake::math::DiscreteAlgebraicRiccatiEquation(
+      discA.transpose(), C.transpose(), discQ, discR);
 
   // S = CPCᵀ + R
-  Eigen::Matrix<double, Outputs, Outputs> S = C * P * C.transpose() + discR;
+  Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
 
   // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
   // efficiently.
@@ -71,15 +70,14 @@ KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
 }
 
 template <int States, int Inputs, int Outputs>
-void KalmanFilter<States, Inputs, Outputs>::Predict(
-    const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
+void KalmanFilter<States, Inputs, Outputs>::Predict(const InputVector& u,
+                                                    units::second_t dt) {
   m_xHat = m_plant->CalculateX(m_xHat, u, dt);
 }
 
 template <int States, int Inputs, int Outputs>
-void KalmanFilter<States, Inputs, Outputs>::Correct(
-    const Eigen::Vector<double, Inputs>& u,
-    const Eigen::Vector<double, Outputs>& y) {
+void KalmanFilter<States, Inputs, Outputs>::Correct(const InputVector& u,
+                                                    const OutputVector& y) {
   // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
   m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u));
 }

wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h

@@ -10,7 +10,7 @@
 #include <utility>
 #include <vector>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "units/math.h"
 #include "units/time.h"
 
@@ -20,14 +20,13 @@ template <int States, int Inputs, int Outputs, typename KalmanFilterType>
 class KalmanFilterLatencyCompensator {
  public:
   struct ObserverSnapshot {
-    Eigen::Vector<double, States> xHat;
-    Eigen::Matrix<double, States, States> errorCovariances;
-    Eigen::Vector<double, Inputs> inputs;
-    Eigen::Vector<double, Outputs> localMeasurements;
+    Vectord<States> xHat;
+    Matrixd<States, States> errorCovariances;
+    Vectord<Inputs> inputs;
+    Vectord<Outputs> localMeasurements;
 
-    ObserverSnapshot(const KalmanFilterType& observer,
-                     const Eigen::Vector<double, Inputs>& u,
-                     const Eigen::Vector<double, Outputs>& localY)
+    ObserverSnapshot(const KalmanFilterType& observer, const Vectord<Inputs>& u,
+                     const Vectord<Outputs>& localY)
         : xHat(observer.Xhat()),
           errorCovariances(observer.P()),
           inputs(u),
@@ -47,10 +46,8 @@ class KalmanFilterLatencyCompensator {
    * @param localY    The local output at the timestamp
    * @param timestamp The timesnap of the state.
    */
-  void AddObserverState(const KalmanFilterType& observer,
-                        Eigen::Vector<double, Inputs> u,
-                        Eigen::Vector<double, Outputs> localY,
-                        units::second_t timestamp) {
+  void AddObserverState(const KalmanFilterType& observer, Vectord<Inputs> u,
+                        Vectord<Outputs> localY, units::second_t timestamp) {
     // Add the new state into the vector.
     m_pastObserverSnapshots.emplace_back(timestamp,
                                          ObserverSnapshot{observer, u, localY});
@@ -74,10 +71,8 @@ class KalmanFilterLatencyCompensator {
    */
   template <int Rows>
   void ApplyPastGlobalMeasurement(
-      KalmanFilterType* observer, units::second_t nominalDt,
-      Eigen::Vector<double, Rows> y,
-      std::function<void(const Eigen::Vector<double, Inputs>& u,
-                         const Eigen::Vector<double, Rows>& y)>
+      KalmanFilterType* observer, units::second_t nominalDt, Vectord<Rows> y,
+      std::function<void(const Vectord<Inputs>& u, const Vectord<Rows>& y)>
           globalMeasurementCorrect,
       units::second_t timestamp) {
     if (m_pastObserverSnapshots.size() == 0) {

wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h

@@ -9,7 +9,7 @@
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/estimator/UnscentedKalmanFilter.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
@@ -213,9 +213,7 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
   UnscentedKalmanFilter<3, 3, 1> m_observer;
   MecanumDriveKinematics m_kinematics;
   TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
-  std::function<void(const Eigen::Vector<double, 3>& u,
-                     const Eigen::Vector<double, 3>& y)>
-      m_visionCorrect;
+  std::function<void(const Vectord<3>& u, const Vectord<3>& y)> m_visionCorrect;
 
   Eigen::Matrix3d m_visionContR;
 
@@ -227,7 +225,7 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
 
   template <int Dim>
   static wpi::array<double, Dim> StdDevMatrixToArray(
-      const Eigen::Vector<double, Dim>& vector) {
+      const Vectord<Dim>& vector) {
     wpi::array<double, Dim> array;
     for (size_t i = 0; i < Dim; ++i) {
       array[i] = vector(i);

wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h

@@ -7,7 +7,7 @@
 #include <cmath>
 
 #include "Eigen/Cholesky"
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 
 namespace frc {
 
@@ -62,14 +62,12 @@ class MerweScaledSigmaPoints {
    *         Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
    *
    */
-  Eigen::Matrix<double, States, 2 * States + 1> SigmaPoints(
-      const Eigen::Vector<double, States>& x,
-      const Eigen::Matrix<double, States, States>& P) {
+  Matrixd<States, 2 * States + 1> SigmaPoints(
+      const Vectord<States>& x, const Matrixd<States, States>& P) {
     double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
-    Eigen::Matrix<double, States, States> U =
-        ((lambda + States) * P).llt().matrixL();
+    Matrixd<States, States> U = ((lambda + States) * P).llt().matrixL();
 
-    Eigen::Matrix<double, States, 2 * States + 1> sigmas;
+    Matrixd<States, 2 * States + 1> sigmas;
     sigmas.template block<States, 1>(0, 0) = x;
     for (int k = 0; k < States; ++k) {
       sigmas.template block<States, 1>(0, k + 1) =
@@ -84,7 +82,7 @@ class MerweScaledSigmaPoints {
   /**
    * Returns the weight for each sigma point for the mean.
    */
-  const Eigen::Vector<double, 2 * States + 1>& Wm() const { return m_Wm; }
+  const Vectord<2 * States + 1>& Wm() const { return m_Wm; }
 
   /**
    * Returns an element of the weight for each sigma point for the mean.
@@ -96,7 +94,7 @@ class MerweScaledSigmaPoints {
   /**
    * Returns the weight for each sigma point for the covariance.
    */
-  const Eigen::Vector<double, 2 * States + 1>& Wc() const { return m_Wc; }
+  const Vectord<2 * States + 1>& Wc() const { return m_Wc; }
 
   /**
    * Returns an element of the weight for each sigma point for the covariance.
@@ -106,8 +104,8 @@ class MerweScaledSigmaPoints {
   double Wc(int i) const { return m_Wc(i, 0); }
 
  private:
-  Eigen::Vector<double, 2 * States + 1> m_Wm;
-  Eigen::Vector<double, 2 * States + 1> m_Wc;
+  Vectord<2 * States + 1> m_Wm;
+  Vectord<2 * States + 1> m_Wc;
   double m_alpha;
   int m_kappa;
 
@@ -120,8 +118,8 @@ class MerweScaledSigmaPoints {
     double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
 
     double c = 0.5 / (States + lambda);
-    m_Wm = Eigen::Vector<double, 2 * States + 1>::Constant(c);
-    m_Wc = Eigen::Vector<double, 2 * States + 1>::Constant(c);
+    m_Wm = Vectord<2 * States + 1>::Constant(c);
+    m_Wc = Vectord<2 * States + 1>::Constant(c);
 
     m_Wm(0) = lambda / (States + lambda);
     m_Wc(0) = lambda / (States + lambda) + (1 - std::pow(m_alpha, 2) + beta);

wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h

@@ -10,7 +10,7 @@
 #include <wpi/array.h>
 #include <wpi/timestamp.h>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/AngleStatistics.h"
 #include "frc/estimator/UnscentedKalmanFilter.h"
@@ -83,10 +83,8 @@ 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::Vector<double, 3>& x,
-                      const Eigen::Vector<double, 3>& u) { return u; },
-                   [](const Eigen::Vector<double, 3>& x,
-                      const Eigen::Vector<double, 3>& u) {
+      : m_observer([](const Vectord<3>& x, const Vectord<3>& u) { return u; },
+                   [](const Vectord<3>& x, const Vectord<3>& u) {
                      return x.block<1, 1>(2, 0);
                    },
                    stateStdDevs, localMeasurementStdDevs,
@@ -98,12 +96,9 @@ class SwerveDrivePoseEstimator {
     SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
 
     // Create correction mechanism for vision measurements.
-    m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
-                          const Eigen::Vector<double, 3>& y) {
+    m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) {
       m_observer.Correct<3>(
-          u, y,
-          [](const Eigen::Vector<double, 3>& x,
-             const Eigen::Vector<double, 3>& u) { return x; },
+          u, y, [](const Vectord<3>& x, const Vectord<3>& u) { return x; },
           m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
           frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
     };
@@ -190,7 +185,7 @@ class SwerveDrivePoseEstimator {
   void AddVisionMeasurement(const Pose2d& visionRobotPose,
                             units::second_t timestamp) {
     if (auto sample = m_poseBuffer.Sample(timestamp)) {
-      m_visionCorrect(Eigen::Vector<double, 3>::Zero(),
+      m_visionCorrect(Vectord<3>::Zero(),
                       PoseTo3dVector(GetEstimatedPosition().TransformBy(
                           visionRobotPose - sample.value())));
     }
@@ -280,10 +275,10 @@ class SwerveDrivePoseEstimator {
         Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
             .RotateBy(angle);
 
-    Eigen::Vector<double, 3> u{fieldRelativeSpeeds.X().value(),
-                               fieldRelativeSpeeds.Y().value(), omega.value()};
+    Vectord<3> u{fieldRelativeSpeeds.X().value(),
+                 fieldRelativeSpeeds.Y().value(), omega.value()};
 
-    Eigen::Vector<double, 1> localY{angle.Radians().value()};
+    Vectord<1> localY{angle.Radians().value()};
     m_previousAngle = angle;
 
     m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());
@@ -298,9 +293,7 @@ class SwerveDrivePoseEstimator {
   UnscentedKalmanFilter<3, 3, 1> m_observer;
   SwerveDriveKinematics<NumModules>& m_kinematics;
   TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
-  std::function<void(const Eigen::Vector<double, 3>& u,
-                     const Eigen::Vector<double, 3>& y)>
-      m_visionCorrect;
+  std::function<void(const Vectord<3>& u, const Vectord<3>& y)> m_visionCorrect;
 
   Eigen::Matrix3d m_visionContR;
 
@@ -312,7 +305,7 @@ class SwerveDrivePoseEstimator {
 
   template <int Dim>
   static wpi::array<double, Dim> StdDevMatrixToArray(
-      const Eigen::Vector<double, Dim>& vector) {
+      const Vectord<Dim>& vector) {
     wpi::array<double, Dim> array;
     for (size_t i = 0; i < Dim; ++i) {
       array[i] = vector(i);

wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h

@@ -9,7 +9,7 @@
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/estimator/MerweScaledSigmaPoints.h"
 #include "units/time.h"
 
@@ -42,6 +42,15 @@ namespace frc {
 template <int States, int Inputs, int Outputs>
 class UnscentedKalmanFilter {
  public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+  using OutputVector = Vectord<Outputs>;
+
+  using StateArray = wpi::array<double, States>;
+  using OutputArray = wpi::array<double, Outputs>;
+
+  using StateMatrix = Matrixd<States, States>;
+
   /**
    * Constructs an unscented Kalman filter.
    *
@@ -53,17 +62,11 @@ class UnscentedKalmanFilter {
    * @param measurementStdDevs Standard deviations of measurements.
    * @param dt                 Nominal discretization timestep.
    */
-  UnscentedKalmanFilter(std::function<Eigen::Vector<double, States>(
-                            const Eigen::Vector<double, States>&,
-                            const Eigen::Vector<double, Inputs>&)>
-                            f,
-                        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,
-                        units::second_t dt);
+  UnscentedKalmanFilter(
+      std::function<StateVector(const StateVector&, const InputVector&)> f,
+      std::function<OutputVector(const StateVector&, const InputVector&)> h,
+      const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+      units::second_t dt);
 
   /**
    * Constructs an unscented Kalman filter with custom mean, residual, and
@@ -90,42 +93,27 @@ class UnscentedKalmanFilter {
    * @param dt                 Nominal discretization timestep.
    */
   UnscentedKalmanFilter(
-      std::function<
-          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                        const Eigen::Vector<double, Inputs>&)>
-          f,
-      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::Vector<double, States>(
-          const Eigen::Matrix<double, States, 2 * States + 1>&,
-          const Eigen::Vector<double, 2 * States + 1>&)>
+      std::function<StateVector(const StateVector&, const InputVector&)> f,
+      std::function<OutputVector(const StateVector&, const InputVector&)> h,
+      const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+      std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
+                                const Vectord<2 * States + 1>&)>
           meanFuncX,
-      std::function<Eigen::Vector<double, Outputs>(
-          const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
-          const Eigen::Vector<double, 2 * States + 1>&)>
+      std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
+                                 const Vectord<2 * States + 1>&)>
           meanFuncY,
-      std::function<
-          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                        const Eigen::Vector<double, States>&)>
+      std::function<StateVector(const StateVector&, const StateVector&)>
           residualFuncX,
-      std::function<
-          Eigen::Vector<double, Outputs>(const Eigen::Vector<double, Outputs>&,
-                                         const Eigen::Vector<double, Outputs>&)>
+      std::function<OutputVector(const OutputVector&, const OutputVector&)>
           residualFuncY,
-      std::function<
-          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                        const Eigen::Vector<double, States>&)>
+      std::function<StateVector(const StateVector&, const StateVector&)>
           addFuncX,
       units::second_t dt);
 
   /**
    * Returns the error covariance matrix P.
    */
-  const Eigen::Matrix<double, States, States>& P() const { return m_P; }
+  const StateMatrix& P() const { return m_P; }
 
   /**
    * Returns an element of the error covariance matrix P.
@@ -140,12 +128,12 @@ class UnscentedKalmanFilter {
    *
    * @param P The error covariance matrix P.
    */
-  void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
+  void SetP(const StateMatrix& P) { m_P = P; }
 
   /**
    * Returns the state estimate x-hat.
    */
-  const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
+  const StateVector& Xhat() const { return m_xHat; }
 
   /**
    * Returns an element of the state estimate x-hat.
@@ -159,7 +147,7 @@ class UnscentedKalmanFilter {
    *
    * @param xHat The state estimate x-hat.
    */
-  void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
+  void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
 
   /**
    * Set an element of the initial state estimate x-hat.
@@ -184,7 +172,7 @@ class UnscentedKalmanFilter {
    * @param u  New control input from controller.
    * @param dt Timestep for prediction.
    */
-  void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt);
+  void Predict(const InputVector& u, units::second_t dt);
 
   /**
    * Correct the state estimate x-hat using the measurements in y.
@@ -192,8 +180,7 @@ class UnscentedKalmanFilter {
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    */
-  void Correct(const Eigen::Vector<double, Inputs>& u,
-               const Eigen::Vector<double, Outputs>& y) {
+  void Correct(const InputVector& u, const OutputVector& y) {
     Correct<Outputs>(u, y, m_h, m_contR, m_meanFuncY, m_residualFuncY,
                      m_residualFuncX, m_addFuncX);
   }
@@ -212,13 +199,10 @@ class UnscentedKalmanFilter {
    * @param R Measurement noise covariance matrix (continuous-time).
    */
   template <int Rows>
-  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);
+  void Correct(
+      const InputVector& u, const Vectord<Rows>& y,
+      std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+      const Matrixd<Rows, Rows>& R);
 
   /**
    * Correct the state estimate x-hat using the measurements in y.
@@ -241,64 +225,39 @@ class UnscentedKalmanFilter {
    * @param addFuncX      A function that adds two state vectors.
    */
   template <int Rows>
-  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::Vector<double, Rows>(
-                   const Eigen::Matrix<double, Rows, 2 * States + 1>&,
-                   const Eigen::Vector<double, 2 * States + 1>&)>
-                   meanFuncY,
-               std::function<Eigen::Vector<double, Rows>(
-                   const Eigen::Vector<double, Rows>&,
-                   const Eigen::Vector<double, Rows>&)>
-                   residualFuncY,
-               std::function<Eigen::Vector<double, States>(
-                   const Eigen::Vector<double, States>&,
-                   const Eigen::Vector<double, States>&)>
-                   residualFuncX,
-               std::function<Eigen::Vector<double, States>(
-                   const Eigen::Vector<double, States>&,
-                   const Eigen::Vector<double, States>)>
-                   addFuncX);
+  void Correct(
+      const InputVector& u, const Vectord<Rows>& y,
+      std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+      const Matrixd<Rows, Rows>& R,
+      std::function<Vectord<Rows>(const Matrixd<Rows, 2 * States + 1>&,
+                                  const Vectord<2 * States + 1>&)>
+          meanFuncY,
+      std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
+          residualFuncY,
+      std::function<StateVector(const StateVector&, const StateVector&)>
+          residualFuncX,
+      std::function<StateVector(const StateVector&, const StateVector&)>
+          addFuncX);
 
  private:
-  std::function<Eigen::Vector<double, States>(
-      const Eigen::Vector<double, States>&,
-      const Eigen::Vector<double, Inputs>&)>
-      m_f;
-  std::function<Eigen::Vector<double, Outputs>(
-      const Eigen::Vector<double, States>&,
-      const Eigen::Vector<double, Inputs>&)>
-      m_h;
-  std::function<Eigen::Vector<double, States>(
-      const Eigen::Matrix<double, States, 2 * States + 1>&,
-      const Eigen::Vector<double, 2 * States + 1>&)>
+  std::function<StateVector(const StateVector&, const InputVector&)> m_f;
+  std::function<OutputVector(const StateVector&, const InputVector&)> m_h;
+  std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
+                            const Vectord<2 * States + 1>&)>
       m_meanFuncX;
-  std::function<Eigen::Vector<double, Outputs>(
-      const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
-      const Eigen::Vector<double, 2 * States + 1>&)>
+  std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
+                             const Vectord<2 * States + 1>&)>
       m_meanFuncY;
-  std::function<Eigen::Vector<double, States>(
-      const Eigen::Vector<double, States>&,
-      const Eigen::Vector<double, States>&)>
+  std::function<StateVector(const StateVector&, const StateVector&)>
       m_residualFuncX;
-  std::function<Eigen::Vector<double, Outputs>(
-      const Eigen::Vector<double, Outputs>&,
-      const Eigen::Vector<double, Outputs>)>
+  std::function<OutputVector(const OutputVector&, const OutputVector&)>
       m_residualFuncY;
-  std::function<Eigen::Vector<double, States>(
-      const Eigen::Vector<double, States>&,
-      const Eigen::Vector<double, States>)>
-      m_addFuncX;
-  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;
-  Eigen::Matrix<double, States, 2 * States + 1> m_sigmasF;
+  std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
+  StateVector m_xHat;
+  StateMatrix m_P;
+  StateMatrix m_contQ;
+  Matrixd<Outputs, Outputs> m_contR;
+  Matrixd<States, 2 * States + 1> m_sigmasF;
   units::second_t m_dt;
 
   MerweScaledSigmaPoints<States> m_pts;

wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc

@@ -16,34 +16,20 @@ namespace frc {
 
 template <int States, int Inputs, int Outputs>
 UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
-    std::function<
-        Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                      const Eigen::Vector<double, Inputs>&)>
-        f,
-    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, units::second_t dt)
+    std::function<StateVector(const StateVector&, const InputVector&)> f,
+    std::function<OutputVector(const StateVector&, const InputVector&)> h,
+    const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+    units::second_t dt)
     : m_f(f), m_h(h) {
   m_contQ = MakeCovMatrix(stateStdDevs);
   m_contR = MakeCovMatrix(measurementStdDevs);
-  m_meanFuncX = [](auto sigmas, auto Wm) -> Eigen::Vector<double, States> {
-    return sigmas * Wm;
-  };
-  m_meanFuncY = [](auto sigmas, auto Wc) -> Eigen::Vector<double, Outputs> {
+  m_meanFuncX = [](auto sigmas, auto Wm) -> StateVector { return sigmas * Wm; };
+  m_meanFuncY = [](auto sigmas, auto Wc) -> OutputVector {
     return sigmas * Wc;
   };
-  m_residualFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
-    return a - b;
-  };
-  m_residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Outputs> {
-    return a - b;
-  };
-  m_addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
-    return a + b;
-  };
+  m_residualFuncX = [](auto a, auto b) -> StateVector { return a - b; };
+  m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; };
+  m_addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
   m_dt = dt;
 
   Reset();
@@ -51,36 +37,20 @@ UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
 
 template <int States, int Inputs, int Outputs>
 UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
-    std::function<
-        Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                      const Eigen::Vector<double, Inputs>&)>
-        f,
-    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::Vector<double, States>(
-        const Eigen::Matrix<double, States, 2 * States + 1>&,
-        const Eigen::Vector<double, 2 * States + 1>&)>
+    std::function<StateVector(const StateVector&, const InputVector&)> f,
+    std::function<OutputVector(const StateVector&, const InputVector&)> h,
+    const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+    std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
+                              const Vectord<2 * States + 1>&)>
         meanFuncX,
-    std::function<Eigen::Vector<double, Outputs>(
-        const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
-        const Eigen::Vector<double, 2 * States + 1>&)>
+    std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
+                               const Vectord<2 * States + 1>&)>
         meanFuncY,
-    std::function<
-        Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                      const Eigen::Vector<double, States>&)>
+    std::function<StateVector(const StateVector&, const StateVector&)>
         residualFuncX,
-    std::function<
-        Eigen::Vector<double, Outputs>(const Eigen::Vector<double, Outputs>&,
-                                       const Eigen::Vector<double, Outputs>&)>
+    std::function<OutputVector(const OutputVector&, const OutputVector&)>
         residualFuncY,
-    std::function<
-        Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                      const Eigen::Vector<double, States>&)>
-        addFuncX,
+    std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
     units::second_t dt)
     : m_f(f),
       m_h(h),
@@ -98,21 +68,20 @@ UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
 
 template <int States, int Inputs, int Outputs>
 void UnscentedKalmanFilter<States, Inputs, Outputs>::Predict(
-    const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
+    const InputVector& u, units::second_t dt) {
   m_dt = dt;
 
   // Discretize Q before projecting mean and covariance forward
-  Eigen::Matrix<double, States, States> contA =
+  StateMatrix contA =
       NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
-  Eigen::Matrix<double, States, States> discA;
-  Eigen::Matrix<double, States, States> discQ;
+  StateMatrix discA;
+  StateMatrix discQ;
   DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
 
-  Eigen::Matrix<double, States, 2 * States + 1> sigmas =
-      m_pts.SigmaPoints(m_xHat, m_P);
+  Matrixd<States, 2 * States + 1> sigmas = m_pts.SigmaPoints(m_xHat, m_P);
 
   for (int i = 0; i < m_pts.NumSigmas(); ++i) {
-    Eigen::Vector<double, States> x = sigmas.template block<States, 1>(0, i);
+    StateVector x = sigmas.template block<States, 1>(0, i);
     m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
   }
 
@@ -127,59 +96,38 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Predict(
 template <int States, int Inputs, int Outputs>
 template <int Rows>
 void UnscentedKalmanFilter<States, Inputs, Outputs>::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::Vector<double, Rows> {
+    const InputVector& u, const Vectord<Rows>& y,
+    std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+    const Matrixd<Rows, Rows>& R) {
+  auto meanFuncY = [](auto sigmas, auto Wc) -> Vectord<Rows> {
     return sigmas * Wc;
   };
-  auto residualFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
-    return a - b;
-  };
-  auto residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Rows> {
-    return a - b;
-  };
-  auto addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
-    return a + b;
-  };
+  auto residualFuncX = [](auto a, auto b) -> StateVector { return a - b; };
+  auto residualFuncY = [](auto a, auto b) -> Vectord<Rows> { return a - b; };
+  auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
   Correct<Rows>(u, y, h, R, meanFuncY, residualFuncY, residualFuncX, addFuncX);
 }
 
 template <int States, int Inputs, int Outputs>
 template <int Rows>
 void UnscentedKalmanFilter<States, Inputs, Outputs>::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::Vector<double, Rows>(
-        const Eigen::Matrix<double, Rows, 2 * States + 1>&,
-        const Eigen::Vector<double, 2 * States + 1>&)>
+    const InputVector& u, const Vectord<Rows>& y,
+    std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+    const Matrixd<Rows, Rows>& R,
+    std::function<Vectord<Rows>(const Matrixd<Rows, 2 * States + 1>&,
+                                const Vectord<2 * States + 1>&)>
         meanFuncY,
-    std::function<Eigen::Vector<double, Rows>(
-        const Eigen::Vector<double, Rows>&, const Eigen::Vector<double, Rows>&)>
+    std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
         residualFuncY,
-    std::function<
-        Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                      const Eigen::Vector<double, States>&)>
+    std::function<StateVector(const StateVector&, const StateVector&)>
         residualFuncX,
-    std::function<
-        Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                      const Eigen::Vector<double, States>)>
+    std::function<StateVector(const StateVector&, const StateVector&)>
         addFuncX) {
-  const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
+  const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
 
   // Transform sigma points into measurement space
-  Eigen::Matrix<double, Rows, 2 * States + 1> sigmasH;
-  Eigen::Matrix<double, States, 2 * States + 1> sigmas =
-      m_pts.SigmaPoints(m_xHat, m_P);
+  Matrixd<Rows, 2 * States + 1> sigmasH;
+  Matrixd<States, 2 * States + 1> sigmas = m_pts.SigmaPoints(m_xHat, m_P);
   for (int i = 0; i < m_pts.NumSigmas(); ++i) {
     sigmasH.template block<Rows, 1>(0, i) =
         h(sigmas.template block<States, 1>(0, i), u);
@@ -191,7 +139,7 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
   Py += discR;
 
   // Compute cross covariance of the state and the measurements
-  Eigen::Matrix<double, States, Rows> Pxy;
+  Matrixd<States, Rows> Pxy;
   Pxy.setZero();
   for (int i = 0; i < m_pts.NumSigmas(); ++i) {
     // Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
@@ -206,7 +154,7 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
   // P_yᵀKᵀ = P_{xy}ᵀ
   // Kᵀ = P_yᵀ.solve(P_{xy}ᵀ)
   // K = (P_yᵀ.solve(P_{xy}ᵀ)ᵀ
-  Eigen::Matrix<double, States, Rows> K =
+  Matrixd<States, Rows> K =
       Py.transpose().ldlt().solve(Pxy.transpose()).transpose();
 
   // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)

wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h

@@ -6,7 +6,7 @@
 
 #include <tuple>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 
 namespace frc {
 
@@ -31,33 +31,30 @@ namespace frc {
  *         passing through the transform.
  */
 template <int CovDim, int States>
-std::tuple<Eigen::Vector<double, CovDim>, Eigen::Matrix<double, CovDim, CovDim>>
-UnscentedTransform(const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
-                   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::Vector<double, 2 * States + 1>&)>
-                       meanFunc,
-                   std::function<Eigen::Vector<double, CovDim>(
-                       const Eigen::Vector<double, CovDim>&,
-                       const Eigen::Vector<double, CovDim>&)>
-                       residualFunc) {
+std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>> UnscentedTransform(
+    const Matrixd<CovDim, 2 * States + 1>& sigmas,
+    const Vectord<2 * States + 1>& Wm, const Vectord<2 * States + 1>& Wc,
+    std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
+                                  const Vectord<2 * States + 1>&)>
+        meanFunc,
+    std::function<Vectord<CovDim>(const Vectord<CovDim>&,
+                                  const Vectord<CovDim>&)>
+        residualFunc) {
   // New mean is usually just the sum of the sigmas * weight:
   //       n
   // dot = Σ W[k] Xᵢ[k]
   //      k=1
-  Eigen::Vector<double, CovDim> x = meanFunc(sigmas, Wm);
+  Vectord<CovDim> x = meanFunc(sigmas, Wm);
 
   // New covariance is the sum of the outer product of the residuals times the
   // weights
-  Eigen::Matrix<double, CovDim, 2 * States + 1> y;
+  Matrixd<CovDim, 2 * States + 1> y;
   for (int i = 0; i < 2 * States + 1; ++i) {
     // y[:, i] = sigmas[:, i] - x
     y.template block<CovDim, 1>(0, i) =
         residualFunc(sigmas.template block<CovDim, 1>(0, i), x);
   }
-  Eigen::Matrix<double, CovDim, CovDim> P =
+  Matrixd<CovDim, CovDim> P =
       y * Eigen::DiagonalMatrix<double, 2 * States + 1>(Wc) * y.transpose();
 
   return std::make_tuple(x, P);