Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Clean up Eigen usage

Browse Source 
* 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()
This commit is contained in:
Tyler Veness
2021-08-19 00:23:48 -07:00
committed by Peter Johnson
parent 72716f51ce
commit 9359431bad
63 changed files with 821 additions and 955 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
wpimath/src/main/native/include/frc/StateSpaceUtil.h
View File

@@ -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) {

56
wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
View File

@@ -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

26
wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
View File

@@ -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

18
wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
View File

@@ -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;

7
wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View File

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

36
wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
View File

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

16
wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
View File

@@ -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

120
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
View File

@@ -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;

12
wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
View File

@@ -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

20
wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
View File

@@ -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) {

6
wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
View File

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

14
wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
View File

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

31
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

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

166
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
View File

@@ -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;

19
wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
View File

@@ -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

4
wpimath/src/main/native/include/frc/filter/LinearFilter.h
View File

@@ -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

29
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
View File

@@ -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 =

16
wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
View File

@@ -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

24
wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
View File

@@ -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

9
wpimath/src/main/native/include/frc/spline/Spline.h
View File

@@ -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>()};
}
/**

13
wpimath/src/main/native/include/frc/system/LinearSystem.h
View File

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

42
wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
View File

@@ -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;

20
wpimath/src/main/native/include/frc/system/NumericalJacobian.h
View File

@@ -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

101
wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
View File

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