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.coeffRef(row, col) = 0.0;
50 [&](
int i,
double tolerance) {
51 if (tolerance == std::numeric_limits<double>::infinity()) {
52 result.coeffRef(i, i) = 0.0;
54 result.coeffRef(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.coeffRef(row, col) = 0.0;
87 [&](
int i,
double stdDev) { result.coeffRef(i, i) = stdDev * stdDev; },
110 for (
int row = 0; row < result.rows(); ++row) {
111 for (
int col = 0; col < result.cols(); ++col) {
113 if (costs[row] == std::numeric_limits<double>::infinity()) {
114 result.coeffRef(row, col) = 0.0;
116 result.coeffRef(row, col) = 1.0 / (costs[row] * costs[row]);
119 result.coeffRef(row, col) = 0.0;
143 for (
int row = 0; row < result.rows(); ++row) {
144 for (
int col = 0; col < result.cols(); ++col) {
146 result.coeffRef(row, col) = stdDevs[row] * stdDevs[row];
148 result.coeffRef(row, col) = 0.0;
156template <std::same_as<
double>... Ts>
158 std::random_device
rd;
159 std::mt19937 gen{
rd()};
163 [&](
int i,
double stdDev) {
169 std::normal_distribution distr{0.0, stdDev};
170 result(i) = distr(gen);
187 std::random_device
rd;
188 std::mt19937 gen{
rd()};
191 for (
size_t i = 0; i < stdDevs.size(); ++i) {
194 if (stdDevs[i] == 0.0) {
197 std::normal_distribution distr{0.0, stdDevs[i]};
198 result(i) = distr(gen);
236template <
int States,
int Inputs>
239 Eigen::EigenSolver<Matrixd<States, States>> es{A,
false};
241 for (
int i = 0; i < A.rows(); ++i) {
242 if (std::norm(es.eigenvalues()[i]) < 1) {
246 if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) {
247 Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
248 E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
249 States>::Identity() -
253 Eigen::ColPivHouseholderQR<
254 Eigen::Matrix<std::complex<double>, States, States + Inputs>>
256 if (qr.rank() < States) {
260 Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
261 E << es.eigenvalues()[i] *
262 Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
266 Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
267 if (qr.rank() < A.rows()) {
280IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
const Eigen::MatrixXd& A,
281 const Eigen::MatrixXd& B);
295template <
int States,
int Outputs>
298 return IsStabilizable<States, Outputs>(A.transpose(), C.transpose());
325 for (
int i = 0; i < Inputs; ++i) {
326 result(i) = std::clamp(u(i), umin(i), umax(i));
342 double maxMagnitude) {
343 double maxValue = u.template lpNorm<Eigen::Infinity>();
345 if (maxValue > maxMagnitude) {
346 return u * maxMagnitude / maxValue;
#define WPILIB_DLLEXPORT
Definition: SymbolExports.h:36
Represents a 2D pose containing translational and rotational elements.
Definition: Pose2d.h:23
Definition: AprilTagDetector_cv.h:11
Eigen::Matrix< double, Rows, Cols, Options, MaxRows, MaxCols > Matrixd
Definition: EigenCore.h:21
WPILIB_DLLEXPORT Eigen::Vector3d PoseToVector(const Pose2d &pose)
Converts a Pose2d into a vector of [x, y, theta].
Eigen::Vector< double, Size > Vectord
Definition: EigenCore.h:12
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:321
template WPILIB_DLLEXPORT bool IsStabilizable< 2, 1 >(const Matrixd< 2, 2 > &A, const Matrixd< 2, 1 > &B)
WPILIB_DLLEXPORT Eigen::Vector3d PoseTo3dVector(const Pose2d &pose)
Converts a Pose2d into a vector of [x, y, theta].
bool IsStabilizable(const Matrixd< States, States > &A, const Matrixd< States, Inputs > &B)
Returns true if (A, B) is a stabilizable pair.
Definition: StateSpaceUtil.h:237
Vectord< sizeof...(Ts)> MakeWhiteNoiseVector(Ts... stdDevs)
Definition: StateSpaceUtil.h:157
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:296
template WPILIB_DLLEXPORT bool IsStabilizable< 1, 1 >(const Matrixd< 1, 1 > &A, const Matrixd< 1, 1 > &B)
Vectord< Inputs > DesaturateInputVector(const Vectord< Inputs > &u, double maxMagnitude)
Renormalize all inputs if any exceeds the maximum magnitude.
Definition: StateSpaceUtil.h:341
WPILIB_DLLEXPORT Eigen::Vector4d PoseTo4dVector(const Pose2d &pose)
Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
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
compound_unit< energy::joules, inverse< mass::kilogram > > rd
Definition: radiation.h:61
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