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

[wpimath] Print uncontrollable/unobservable models in LQR and KF (#3694)

Browse Source 
IsDetectable() was added to make the code easier to read.
This commit is contained in:
Tyler Veness
2021-10-29 00:03:02 -07:00
committed by GitHub
parent d5270d113b
commit a939cd9c89
12 changed files with 169 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -18,25 +18,33 @@
using namespace wpi::java;
/**
* Returns true if (A, B) is a stabilizable pair.
*
* (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
* any, have absolute values less than one, where an eigenvalue is
* uncontrollable if rank(λI - A, B) < n where n is the number of states.
*
* @param A System matrix.
* @param B Input matrix.
*/
bool check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B) {
// This function checks if (A,B) is a stabilizable pair.
// (A,B) is stabilizable if and only if the uncontrollable eigenvalues of
// A, if any, have absolute values less than one, where an eigenvalue is
// uncontrollable if Rank[lambda * I - A, B] < n.
int n = B.rows(), m = B.cols();
Eigen::EigenSolver<Eigen::MatrixXd> es(A);
for (int i = 0; i < n; i++) {
int states = B.rows();
int inputs = B.cols();
Eigen::EigenSolver<Eigen::MatrixXd> es{A};
for (int i = 0; i < states; ++i) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
1) {
continue;
}
Eigen::MatrixXcd E(n, n + m);
E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B;
Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr(E);
if (qr.rank() != n) {
Eigen::MatrixXcd E{states, states + inputs};
E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(states, states) - A,
B;
Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
if (qr.rank() < states) {
return false;
}
}
@@ -196,7 +204,7 @@ Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
B{nativeB, states, inputs};
bool isStabilizable = check_stabilizable(A, B); // NOLINT
bool isStabilizable = check_stabilizable(A, B);
env->ReleaseDoubleArrayElements(aSrc, nativeA, 0);
env->ReleaseDoubleArrayElements(bSrc, nativeB, 0);