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

[wpimath] Fix DARE Q decomposition (#5611)

Browse Source
This commit is contained in:
Tyler Veness
2023-09-05 10:48:43 -07:00
committed by GitHub
parent 9b3f7fb548
commit 1a6df6fec6
5 changed files with 49 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -279,3 +279,21 @@ TEST(DARETest, ACNotDetectable_ABQRN) {
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
}
TEST(DARETest, QDecomposition) {
// Ensures the decomposition of Q into CᵀC is correct
const Eigen::Matrix2d A{{1.0, 0.0}, {0.0, 0.0}};
const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
// (A, C₁) should be detectable pair
const Eigen::Matrix2d C_1{{0.0, 0.0}, {1.0, 0.0}};
const Eigen::Matrix2d Q_1 = C_1.transpose() * C_1;
EXPECT_NO_THROW((frc::DARE<2, 2>(A, B, Q_1, R)));
// (A, C₂) shouldn't be detectable pair
const Eigen::Matrix2d C_2 = C_1.transpose();
const Eigen::Matrix2d Q_2 = C_2.transpose() * C_2;
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q_2, R)), std::invalid_argument);
}

4
wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
View File

@@ -158,10 +158,10 @@ TEST(LinearQuadraticRegulatorTest, MatrixOverloadsWithDoubleIntegrator) {
EXPECT_NEAR(0.51182128351092726, K(0, 1), 1e-10);
// QRN overload
Matrixd<2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}};
Matrixd<2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 5.0)}};
Matrixd<1, 2> Kimf = GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms);
EXPECT_NEAR(0.0, Kimf(0, 0), 1e-10);
EXPECT_NEAR(-5.367540084534802e-05, Kimf(0, 1), 1e-10);
EXPECT_NEAR(-6.9190500116751458e-05, Kimf(0, 1), 1e-10);
}
TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {