13#include <Eigen/Eigenvalues>
36template <std::same_as<
double>... Ts>
39 Matrixd<
sizeof...(Ts),
sizeof...(Ts)> result;
41 for (
int row = 0; row < result.rows(); ++row) {
42 for (
int col = 0; col < result.cols(); ++col) {
44 result(row, col) = 0.0;
50 [&](
int i,
double tolerance) {
51 if (tolerance == std::numeric_limits<double>::infinity()) {
54 result(i, i) = 1.0 / (tolerance * tolerance);
74template <std::same_as<
double>... Ts>
76 Matrixd<
sizeof...(Ts),
sizeof...(Ts)> result;
78 for (
int row = 0; row < result.rows(); ++row) {
79 for (
int col = 0; col < result.cols(); ++col) {
81 result(row, col) = 0.0;
86 wpi::for_each([&](
int i,
double stdDev) { result(i, i) = stdDev * stdDev; },
109 for (
int row = 0; row < result.rows(); ++row) {
110 for (
int col = 0; col < result.cols(); ++col) {
112 if (costs[row] == std::numeric_limits<double>::infinity()) {
113 result(row, col) = 0.0;
115 result(row, col) = 1.0 / (costs[row] * costs[row]);
118 result(row, col) = 0.0;
142 for (
int row = 0; row < result.rows(); ++row) {
143 for (
int col = 0; col < result.cols(); ++col) {
145 result(row, col) = stdDevs[row] * stdDevs[row];
147 result(row, col) = 0.0;
155template <std::same_as<
double>... Ts>
157 std::random_device rd;
158 std::mt19937 gen{rd()};
162 [&](
int i,
double stdDev) {
168 std::normal_distribution distr{0.0, stdDev};
169 result(i) = distr(gen);
186 std::random_device rd;
187 std::mt19937 gen{rd()};
190 for (
size_t i = 0; i < stdDevs.size(); ++i) {
193 if (stdDevs[i] == 0.0) {
196 std::normal_distribution distr{0.0, stdDevs[i]};
197 result(i) = distr(gen);
213[[deprecated(
"Use Pose2d.ToMatrix() instead.")]]
230[[deprecated(
"Use Pose2d.ToMatrix() instead.")]]
249template <
int States,
int Inputs>
252 Eigen::EigenSolver<Matrixd<States, States>> es{A,
false};
254 for (
int i = 0; i < A.rows(); ++i) {
255 if (std::norm(es.eigenvalues()[i]) < 1) {
259 if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) {
260 Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
261 E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
262 States>::Identity() -
266 Eigen::ColPivHouseholderQR<
267 Eigen::Matrix<std::complex<double>, States, States + Inputs>>
269 if (qr.rank() < States) {
273 Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
274 E << es.eigenvalues()[i] *
275 Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
279 Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
280 if (qr.rank() < A.rows()) {
294 const Eigen::MatrixXd& B);
308template <
int States,
int Outputs>
324[[deprecated(
"Use Pose2d.ToMatrix() instead.")]]
326 return Eigen::Vector3d{
344 for (
int i = 0; i < Inputs; ++i) {
345 result(i) = std::clamp(u(i), umin(i), umax(i));
361 double maxMagnitude) {
362 double maxValue = u.template lpNorm<Eigen::Infinity>();
364 if (maxValue > maxMagnitude) {
365 return u * maxMagnitude / maxValue;
#define WPILIB_DLLEXPORT
Definition SymbolExports.h:36
Represents a 2D pose containing translational and rotational elements.
Definition Pose2d.h:28
constexpr const Rotation2d & Rotation() const
Returns the underlying rotation.
Definition Pose2d.h:128
constexpr units::meter_t Y() const
Returns the Y component of the pose's translation.
Definition Pose2d.h:121
constexpr units::meter_t X() const
Returns the X component of the pose's translation.
Definition Pose2d.h:114
constexpr const Translation2d & Translation() const
Returns the underlying translation.
Definition Pose2d.h:107
constexpr double Cos() const
Returns the cosine of the rotation.
Definition Rotation2d.h:231
constexpr double Sin() const
Returns the sine of the rotation.
Definition Rotation2d.h:238
constexpr units::radian_t Radians() const
Returns the radian value of the rotation.
Definition Rotation2d.h:216
constexpr units::meter_t X() const
Returns the X component of the translation.
Definition Translation2d.h:83
constexpr units::meter_t Y() const
Returns the Y component of the translation.
Definition Translation2d.h:90
WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d &pose)
Converts a Pose2d into a vector of [x, y, theta].
Definition StateSpaceUtil.h:214
Eigen::Matrix< double, Rows, Cols, Options, MaxRows, MaxCols > Matrixd
Definition EigenCore.h:21
template WPILIB_DLLEXPORT bool IsStabilizable< 2, 1 >(const Matrixd< 2, 2 > &A, const Matrixd< 2, 1 > &B)
Eigen::Vector< double, Size > Vectord
Definition EigenCore.h:12
template WPILIB_DLLEXPORT bool IsStabilizable< Eigen::Dynamic, Eigen::Dynamic >(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B)
bool IsStabilizable(const Matrixd< States, States > &A, const Matrixd< States, Inputs > &B)
Returns true if (A, B) is a stabilizable pair.
Definition StateSpaceUtil.h:250
constexpr Vectord< Inputs > ClampInputMaxMagnitude(const Vectord< Inputs > &u, const Vectord< Inputs > &umin, const Vectord< Inputs > &umax)
Clamps input vector between system's minimum and maximum allowable input.
Definition StateSpaceUtil.h:340
Vectord< sizeof...(Ts)> MakeWhiteNoiseVector(Ts... stdDevs)
Definition StateSpaceUtil.h:156
constexpr Matrixd< sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs)
Creates a covariance matrix from the given vector for use with Kalman filters.
Definition StateSpaceUtil.h:75
bool IsDetectable(const Matrixd< States, States > &A, const Matrixd< Outputs, States > &C)
Returns true if (A, C) is a detectable pair.
Definition StateSpaceUtil.h:309
template WPILIB_DLLEXPORT bool IsStabilizable< 1, 1 >(const Matrixd< 1, 1 > &A, const Matrixd< 1, 1 > &B)
WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseToVector(const Pose2d &pose)
Converts a Pose2d into a vector of [x, y, theta].
Definition StateSpaceUtil.h:325
WPILIB_DLLEXPORT constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d &pose)
Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
Definition StateSpaceUtil.h:231
Vectord< Inputs > DesaturateInputVector(const Vectord< Inputs > &u, double maxMagnitude)
Renormalize all inputs if any exceeds the maximum magnitude.
Definition StateSpaceUtil.h:360
constexpr Matrixd< sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances)
Creates a cost matrix from the given vector for use with LQR.
Definition StateSpaceUtil.h:37
constexpr void for_each(F &&f, Ts &&... elems)
Calls f(i, elem) for each element of elems where i is the index of the element in elems and elem is t...
Definition Algorithm.h:29