Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add static matrix support to DARE solver (#5536)

Browse Source 
Using static matrices where possible results in a 2x performance
improvement.
This commit is contained in:
Tyler Veness
2023-08-14 09:15:58 -07:00
committed by GitHub
parent 394cfeadbd
commit 03764dfe93
9 changed files with 354 additions and 436 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -58,24 +58,37 @@ bool IsStabilizableImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B) {
Eigen::EigenSolver<Matrixd<States, States>> es{A, false};
for (int i = 0; i < States; ++i) {
for (int i = 0; i < A.rows(); ++i) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
1) {
continue;
}
Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
States>::Identity() -
A,
B;
if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) {
Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
States>::Identity() -
A,
B;
Eigen::ColPivHouseholderQR<
Eigen::Matrix<std::complex<double>, States, States + Inputs>>
qr{E};
if (qr.rank() < States) {
return false;
Eigen::ColPivHouseholderQR<
Eigen::Matrix<std::complex<double>, States, States + Inputs>>
qr{E};
if (qr.rank() < States) {
return false;
}
} else {
Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
E << es.eigenvalues()[i] *
Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
A,
B;
Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
if (qr.rank() < A.rows()) {
return false;
}
}
}
return true;
@@ -274,6 +287,12 @@ template <>
WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
const Matrixd<2, 1>& B);
// Template specializations are used here to make common state-input pairs
// compile faster.
template <>
WPILIB_DLLEXPORT bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
/**
* Converts a Pose2d into a vector of [x, y, theta].
*