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] 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

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

@@ -78,7 +78,7 @@ bool IsStabilizableImpl(const Eigen::Matrix<double, States, States>& A,
Eigen::ColPivHouseholderQR<
Eigen::Matrix<std::complex<double>, States, States + Inputs>>
qr(E);
qr{E};
if (qr.rank() < States) {
return false;
}
@@ -258,9 +258,9 @@ Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose);
/**
* Returns true if (A, B) is a stabilizable pair.
*
* (A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
* (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 number of states.
* uncontrollable if rank(λI - A, B) < n where n is the number of states.
*
* @param A System matrix.
* @param B Input matrix.
@@ -271,6 +271,23 @@ bool IsStabilizable(const Eigen::Matrix<double, States, States>& A,
return detail::IsStabilizableImpl<States, Inputs>(A, B);
}
/**
* Returns true if (A, C) is a detectable pair.
*
* (A, C) is detectable if and only if the unobservable eigenvalues of A, if
* any, have absolute values less than one, where an eigenvalue is unobservable
* if rank(λI - A; C) < n where n is the number of states.
*
* @param A System matrix.
* @param C Output matrix.
*/
template <int States, int Outputs>
bool IsDetectable(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, Outputs, States>& C) {
return detail::IsStabilizableImpl<States, Outputs>(A.transpose(),
C.transpose());
}
// Template specializations are used here to make common state-input pairs
// compile faster.
template <>