Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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

24
wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
View File

@@ -134,3 +134,27 @@ TEST(StateSpaceUtilTest, IsStabilizable) {
EXPECT_TRUE((frc::IsStabilizable<2, 1>(
Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, B)));
}
TEST(StateSpaceUtilTest, IsDetectable) {
Eigen::Matrix<double, 1, 2> C{0, 1};
// First eigenvalue is unobservable and unstable.
// Second eigenvalue is observable and stable.
EXPECT_FALSE((frc::IsDetectable<2, 1>(
Eigen::Matrix<double, 2, 2>{{1.2, 0}, {0, 0.5}}, C)));
// First eigenvalue is unobservable and marginally stable.
// Second eigenvalue is observable and stable.
EXPECT_FALSE((frc::IsDetectable<2, 1>(
Eigen::Matrix<double, 2, 2>{{1, 0}, {0, 0.5}}, C)));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and stable.
EXPECT_TRUE((frc::IsDetectable<2, 1>(
Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 0.5}}, C)));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and unstable.
EXPECT_TRUE((frc::IsDetectable<2, 1>(
Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, C)));
}