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

[wpimath] Only compute eigenvalues with EigenSolvers (#5369)

Browse Source 
We don't need the eigenvectors, so we're doing a lot of extra work we
don't need to.
This commit is contained in:
Tyler Veness
2023-06-07 09:47:09 -07:00
committed by GitHub
parent d3c9316a97
commit 5acc5e22aa
5 changed files with 16 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
wpimath/src/main/native/cpp/DARE.cpp
View File

@@ -37,7 +37,7 @@ namespace {
*/
bool IsStabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B) {
Eigen::EigenSolver<Eigen::MatrixXd> es{A};
Eigen::EigenSolver<Eigen::MatrixXd> es{A, false};
for (int i = 0; i < A.rows(); ++i) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
@@ -81,7 +81,7 @@ bool IsDetectable(const Eigen::Ref<const Eigen::MatrixXd>& A,
* @param A The matrix.
*/
bool IsPositiveSemidefinite(const Eigen::Ref<const Eigen::MatrixXd>& A) {
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es{A};
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es{A, Eigen::EigenvaluesOnly};
for (int i = 0; i < A.rows(); ++i) {
if (es.eigenvalues()[i] < 0) {
return false;
@@ -97,7 +97,7 @@ bool IsPositiveSemidefinite(const Eigen::Ref<const Eigen::MatrixXd>& A) {
* @param A The matrix.
*/
bool IsPositiveDefinite(const Eigen::Ref<const Eigen::MatrixXd>& A) {
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es{A};
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es{A, Eigen::EigenvaluesOnly};
for (int i = 0; i < A.rows(); ++i) {
if (es.eigenvalues()[i] <= 0) {
return false;

2
wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
View File

@@ -33,7 +33,7 @@ namespace {
*/
bool IsStabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B) {
Eigen::EigenSolver<Eigen::MatrixXd> es{A};
Eigen::EigenSolver<Eigen::MatrixXd> es{A, false};
for (int i = 0; i < A.rows(); ++i) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +

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

@@ -56,7 +56,7 @@ void WhiteNoiseVectorImpl(Matrix& result, T elem, Ts... elems) {
template <int States, int Inputs>
bool IsStabilizableImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B) {
Eigen::EigenSolver<Matrixd<States, States>> es{A};
Eigen::EigenSolver<Matrixd<States, States>> es{A, false};
for (int i = 0; i < States; ++i) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +

3
wpimath/src/test/native/cpp/DARETest.cpp
View File

@@ -27,7 +27,8 @@ void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs,
}
void ExpectPositiveSemidefinite(const Eigen::Ref<const Eigen::MatrixXd>& X) {
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigX(X);
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigX{X,
Eigen::EigenvaluesOnly};
for (int i = 0; i < X.rows(); ++i) {
EXPECT_GE(eigX.eigenvalues()[i], 0.0);
}

13
wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
View File

@@ -7,6 +7,7 @@
#include <functional>
#include "Eigen/Eigenvalues"
#include "Eigen/src/Core/util/Constants.h"
#include "frc/EigenCore.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
@@ -126,7 +127,8 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
frc::Matrixd<2, 2> discATaylor;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont{contQ};
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont{contQ,
Eigen::EigenvaluesOnly};
for (int i = 0; i < contQ.rows(); ++i) {
EXPECT_GE(esCont.eigenvalues()[i], 0);
}
@@ -154,7 +156,8 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
EXPECT_LT((discA - discATaylor).norm(), 1e-10);
// Discrete Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc{discQTaylor};
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc{discQTaylor,
Eigen::EigenvaluesOnly};
for (int i = 0; i < discQTaylor.rows(); ++i) {
EXPECT_GE(esDisc.eigenvalues()[i], 0);
}
@@ -172,7 +175,8 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
frc::Matrixd<2, 2> discATaylor;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont{contQ,
Eigen::EigenvaluesOnly};
for (int i = 0; i < contQ.rows(); ++i) {
EXPECT_GE(esCont.eigenvalues()[i], 0);
}
@@ -200,7 +204,8 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
EXPECT_LT((discA - discATaylor).norm(), 1e-10);
// Discrete Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc{discQTaylor,
Eigen::EigenvaluesOnly};
for (int i = 0; i < discQTaylor.rows(); ++i) {
EXPECT_GE(esDisc.eigenvalues()[i], 0);
}