From 7c20fa1b1841ef83377faa0deb1dd898d23a66cc Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 23 Aug 2023 10:46:50 -0700 Subject: [PATCH] [wpimath] Refactor DARE tests to reduce RAM usage at compile time (#5557) --- wpimath/src/test/native/cpp/DARETest.cpp | 316 ++++++++---------- wpimath/src/test/native/cpp/DARETestUtil.cpp | 72 ++++ wpimath/src/test/native/cpp/DARETestUtil.h | 25 ++ .../src/test/native/cpp/DARETest_Inst_2x1.cpp | 13 + .../src/test/native/cpp/DARETest_Inst_2x2.cpp | 13 + .../src/test/native/cpp/DARETest_Inst_2x3.cpp | 13 + .../src/test/native/cpp/DARETest_Inst_4x1.cpp | 13 + 7 files changed, 282 insertions(+), 183 deletions(-) create mode 100644 wpimath/src/test/native/cpp/DARETestUtil.cpp create mode 100644 wpimath/src/test/native/cpp/DARETestUtil.h create mode 100644 wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp create mode 100644 wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp create mode 100644 wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp create mode 100644 wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp diff --git a/wpimath/src/test/native/cpp/DARETest.cpp b/wpimath/src/test/native/cpp/DARETest.cpp index b7b783eda3..fb266ba801 100644 --- a/wpimath/src/test/native/cpp/DARETest.cpp +++ b/wpimath/src/test/native/cpp/DARETest.cpp @@ -4,89 +4,60 @@ #include -#include - +#include "DARETestUtil.h" #include "Eigen/Core" -#include "Eigen/Eigenvalues" #include "frc/DARE.h" +#include "frc/EigenCore.h" #include "frc/fmt/Eigen.h" #include "gtest/gtest.h" -void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, - double tolerance) { - for (int row = 0; row < lhs.rows(); ++row) { - for (int col = 0; col < lhs.cols(); ++col) { - EXPECT_NEAR(lhs(row, col), rhs(row, col), tolerance) - << fmt::format("row = {}, col = {}", row, col); - } - } +// 2x1 +extern template Eigen::Matrix frc::DARE<2, 1>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R); +extern template Eigen::Matrix frc::DARE<2, 1>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R, + const Eigen::Matrix& N); - if (::testing::Test::HasFailure()) { - fmt::print("lhs =\n{}\n", lhs); - fmt::print("rhs =\n{}\n", rhs); - fmt::print("delta =\n{}\n", Eigen::MatrixXd{lhs - rhs}); - } -} +// 4x1 +extern template Eigen::Matrix frc::DARE<4, 1>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R); +extern template Eigen::Matrix frc::DARE<4, 1>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R, + const Eigen::Matrix& N); -void ExpectPositiveSemidefinite(const Eigen::Ref& X) { - Eigen::SelfAdjointEigenSolver eigX{X, - Eigen::EigenvaluesOnly}; - for (int i = 0; i < X.rows(); ++i) { - EXPECT_GE(eigX.eigenvalues()[i], 0.0); - } -} +// 2x2 +extern template Eigen::Matrix frc::DARE<2, 2>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R); +extern template Eigen::Matrix frc::DARE<2, 2>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R, + const Eigen::Matrix& N); -void ExpectDARESolution(const Eigen::Ref& A, - const Eigen::Ref& B, - const Eigen::Ref& Q, - const Eigen::Ref& R, - const Eigen::Ref& X) { - // Check that X is the solution to the DARE - // Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0 - // clang-format off - Eigen::MatrixXd Y = - A.transpose() * X * A - - X - - (A.transpose() * X * B * (B.transpose() * X * B + R).inverse() - * B.transpose() * X * A) - + Q; - // clang-format on - ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10); -} - -void ExpectDARESolution(const Eigen::Ref& A, - const Eigen::Ref& B, - const Eigen::Ref& Q, - const Eigen::Ref& R, - const Eigen::Ref& N, - const Eigen::Ref& X) { - // Check that X is the solution to the DARE - // Y = AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0 - // clang-format off - Eigen::MatrixXd Y = - A.transpose() * X * A - - X - - ((A.transpose() * X * B + N) * (B.transpose() * X * B + R).inverse() - * (B.transpose() * X * A + N.transpose())) - + Q; - // clang-format on - ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10); -} +// 2x3 +extern template Eigen::Matrix frc::DARE<2, 3>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R); +extern template Eigen::Matrix frc::DARE<2, 3>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R, + const Eigen::Matrix& N); TEST(DARETest, NonInvertibleA_ABQR) { // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic // Riccati Equation" - Eigen::MatrixXd A{4, 4}; - A << 0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0; - Eigen::MatrixXd B{4, 1}; - B << 0, 0, 0, 1; - Eigen::MatrixXd Q{4, 4}; - Q << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; - Eigen::MatrixXd R{1, 1}; - R << 0.25; + frc::Matrixd<4, 4> A{ + {0.5, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}}; + frc::Matrixd<4, 1> B{{0}, {0}, {0}, {1}}; + frc::Matrixd<4, 4> Q{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}; + frc::Matrixd<1, 1> R{0.25}; - Eigen::MatrixXd X = frc::DARE<4, 1>(A, B, Q, R); + frc::Matrixd<4, 4> X = frc::DARE<4, 1>(A, B, Q, R); ExpectMatrixEqual(X, X.transpose(), 1e-10); ExpectPositiveSemidefinite(X); ExpectDARESolution(A, B, Q, R, X); @@ -96,60 +67,48 @@ TEST(DARETest, NonInvertibleA_ABQRN) { // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic // Riccati Equation" - Eigen::MatrixXd A{4, 4}; - A << 0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0; - Eigen::MatrixXd B{4, 1}; - B << 0, 0, 0, 1; - Eigen::MatrixXd Q{4, 4}; - Q << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; - Eigen::MatrixXd R{1, 1}; - R << 0.25; + frc::Matrixd<4, 4> A{ + {0.5, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}}; + frc::Matrixd<4, 1> B{{0}, {0}, {0}, {1}}; + frc::Matrixd<4, 4> Q{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}; + frc::Matrixd<1, 1> R{0.25}; - Eigen::MatrixXd Aref{4, 4}; - Aref << 0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0; + frc::Matrixd<4, 4> Aref{ + {0.25, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}}; Q = (A - Aref).transpose() * Q * (A - Aref); R = B.transpose() * Q * B + R; - Eigen::MatrixXd N = (A - Aref).transpose() * Q * B; + frc::Matrixd<4, 1> N = (A - Aref).transpose() * Q * B; - Eigen::MatrixXd X = frc::DARE<4, 1>(A, B, Q, R, N); + frc::Matrixd<4, 4> X = frc::DARE<4, 1>(A, B, Q, R, N); ExpectMatrixEqual(X, X.transpose(), 1e-10); ExpectPositiveSemidefinite(X); ExpectDARESolution(A, B, Q, R, N, X); } TEST(DARETest, InvertibleA_ABQR) { - Eigen::MatrixXd A{2, 2}; - A << 1, 1, 0, 1; - Eigen::MatrixXd B{2, 1}; - B << 0, 1; - Eigen::MatrixXd Q{2, 2}; - Q << 1, 0, 0, 0; - Eigen::MatrixXd R{1, 1}; - R << 0.3; + frc::Matrixd<2, 2> A{{1, 1}, {0, 1}}; + frc::Matrixd<2, 1> B{{0}, {1}}; + frc::Matrixd<2, 2> Q{{1, 0}, {0, 0}}; + frc::Matrixd<1, 1> R{{0.3}}; - Eigen::MatrixXd X = frc::DARE<2, 1>(A, B, Q, R); + frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R); ExpectMatrixEqual(X, X.transpose(), 1e-10); ExpectPositiveSemidefinite(X); ExpectDARESolution(A, B, Q, R, X); } TEST(DARETest, InvertibleA_ABQRN) { - Eigen::MatrixXd A{2, 2}; - A << 1, 1, 0, 1; - Eigen::MatrixXd B{2, 1}; - B << 0, 1; - Eigen::MatrixXd Q{2, 2}; - Q << 1, 0, 0, 0; - Eigen::MatrixXd R{1, 1}; - R << 0.3; + frc::Matrixd<2, 2> A{{1, 1}, {0, 1}}; + frc::Matrixd<2, 1> B{{0}, {1}}; + frc::Matrixd<2, 2> Q{{1, 0}, {0, 0}}; + frc::Matrixd<1, 1> R{0.3}; - Eigen::MatrixXd Aref{2, 2}; - Aref << 0.5, 1, 0, 1; + frc::Matrixd<2, 2> Aref{{0.5, 1}, {0, 1}}; Q = (A - Aref).transpose() * Q * (A - Aref); R = B.transpose() * Q * B + R; - Eigen::MatrixXd N = (A - Aref).transpose() * Q * B; + frc::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B; - Eigen::MatrixXd X = frc::DARE<2, 1>(A, B, Q, R, N); + frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R, N); ExpectMatrixEqual(X, X.transpose(), 1e-10); ExpectPositiveSemidefinite(X); ExpectDARESolution(A, B, Q, R, N, X); @@ -158,16 +117,12 @@ TEST(DARETest, InvertibleA_ABQRN) { TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQR) { // The first generalized eigenvalue of (S, T) is stable - Eigen::MatrixXd A{2, 2}; - A << 0, 1, 0, 0; - Eigen::MatrixXd B{2, 1}; - B << 0, 1; - Eigen::MatrixXd Q{2, 2}; - Q << 1, 0, 0, 1; - Eigen::MatrixXd R{1, 1}; - R << 1; + frc::Matrixd<2, 2> A{{0, 1}, {0, 0}}; + frc::Matrixd<2, 1> B{{0}, {1}}; + frc::Matrixd<2, 2> Q{{1, 0}, {0, 1}}; + frc::Matrixd<1, 1> R{1}; - Eigen::MatrixXd X = frc::DARE<2, 1>(A, B, Q, R); + frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R); ExpectMatrixEqual(X, X.transpose(), 1e-10); ExpectPositiveSemidefinite(X); ExpectDARESolution(A, B, Q, R, X); @@ -176,155 +131,150 @@ TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQR) { TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQRN) { // The first generalized eigenvalue of (S, T) is stable - Eigen::MatrixXd A{2, 2}; - A << 0, 1, 0, 0; - Eigen::MatrixXd B{2, 1}; - B << 0, 1; - Eigen::MatrixXd Q{2, 2}; - Q << 1, 0, 0, 1; - Eigen::MatrixXd R{1, 1}; - R << 1; + frc::Matrixd<2, 2> A{{0, 1}, {0, 0}}; + frc::Matrixd<2, 1> B{{0}, {1}}; + frc::Matrixd<2, 2> Q{{1, 0}, {0, 1}}; + frc::Matrixd<1, 1> R{1}; - Eigen::MatrixXd Aref{2, 2}; - Aref << 0, 0.5, 0, 0; + frc::Matrixd<2, 2> Aref{{0, 0.5}, {0, 0}}; Q = (A - Aref).transpose() * Q * (A - Aref); R = B.transpose() * Q * B + R; - Eigen::MatrixXd N = (A - Aref).transpose() * Q * B; + frc::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B; - Eigen::MatrixXd X = frc::DARE<2, 1>(A, B, Q, R, N); + frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R, N); ExpectMatrixEqual(X, X.transpose(), 1e-10); ExpectPositiveSemidefinite(X); ExpectDARESolution(A, B, Q, R, N, X); } TEST(DARETest, IdentitySystem_ABQR) { - const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()}; - Eigen::MatrixXd X = frc::DARE<2, 2>(A, B, Q, R); + Eigen::Matrix2d X = frc::DARE<2, 2>(A, B, Q, R); ExpectMatrixEqual(X, X.transpose(), 1e-10); ExpectPositiveSemidefinite(X); ExpectDARESolution(A, B, Q, R, X); } TEST(DARETest, IdentitySystem_ABQRN) { - const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd N{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()}; - Eigen::MatrixXd X = frc::DARE<2, 2>(A, B, Q, R, N); + Eigen::Matrix2d X = frc::DARE<2, 2>(A, B, Q, R, N); ExpectMatrixEqual(X, X.transpose(), 1e-10); ExpectPositiveSemidefinite(X); ExpectDARESolution(A, B, Q, R, N, X); } TEST(DARETest, MoreInputsThanStates_ABQR) { - const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}}; - const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd R{Eigen::Matrix3d::Identity()}; + const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()}; + const frc::Matrixd<2, 3> B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}}; + const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix3d R{Eigen::Matrix3d::Identity()}; - Eigen::MatrixXd X = frc::DARE<2, 3>(A, B, Q, R); + Eigen::Matrix2d X = frc::DARE<2, 3>(A, B, Q, R); ExpectMatrixEqual(X, X.transpose(), 1e-10); ExpectPositiveSemidefinite(X); ExpectDARESolution(A, B, Q, R, X); } TEST(DARETest, MoreInputsThanStates_ABQRN) { - const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}}; - const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd R{Eigen::Matrix3d::Identity()}; - const Eigen::MatrixXd N{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}}; + const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()}; + const frc::Matrixd<2, 3> B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}}; + const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix3d R{Eigen::Matrix3d::Identity()}; + const frc::Matrixd<2, 3> N{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}}; - Eigen::MatrixXd X = frc::DARE<2, 3>(A, B, Q, R, N); + Eigen::Matrix2d X = frc::DARE<2, 3>(A, B, Q, R, N); ExpectMatrixEqual(X, X.transpose(), 1e-10); ExpectPositiveSemidefinite(X); ExpectDARESolution(A, B, Q, R, N, X); } TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQR) { - const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd Q{-Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d Q{-Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()}; EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument); } TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQRN) { - const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd N{2.0 * Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d N{2.0 * Eigen::Matrix2d::Identity()}; EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument); } TEST(DARETest, RNotSymmetricPositiveDefinite_ABQR) { - const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd R1{Eigen::Matrix2d::Zero()}; + const Eigen::Matrix2d R1{Eigen::Matrix2d::Zero()}; EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R1)), std::invalid_argument); - const Eigen::MatrixXd R2{-Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d R2{-Eigen::Matrix2d::Identity()}; EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R2)), std::invalid_argument); } TEST(DARETest, RNotSymmetricPositiveDefinite_ABQRN) { - const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd N{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd R1{Eigen::Matrix2d::Zero()}; + const Eigen::Matrix2d R1{Eigen::Matrix2d::Zero()}; EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R1, N)), std::invalid_argument); - const Eigen::MatrixXd R2{-Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d R2{-Eigen::Matrix2d::Identity()}; EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R2, N)), std::invalid_argument); } TEST(DARETest, ABNotStabilizable_ABQR) { - const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd B{Eigen::Matrix2d::Zero()}; - const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d B{Eigen::Matrix2d::Zero()}; + const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()}; EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument); } TEST(DARETest, ABNotStabilizable_ABQRN) { - const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd B{Eigen::Matrix2d::Zero()}; - const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd N{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d B{Eigen::Matrix2d::Zero()}; + const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()}; EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument); } TEST(DARETest, ACNotDetectable_ABQR) { - const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd Q{Eigen::Matrix2d::Zero()}; - const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d Q{Eigen::Matrix2d::Zero()}; + const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()}; EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument); } TEST(DARETest, ACNotDetectable_ABQRN) { - const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd Q{Eigen::Matrix2d::Zero()}; - const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd N{Eigen::Matrix2d::Zero()}; + const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d Q{Eigen::Matrix2d::Zero()}; + const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()}; + const Eigen::Matrix2d N{Eigen::Matrix2d::Zero()}; EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument); } diff --git a/wpimath/src/test/native/cpp/DARETestUtil.cpp b/wpimath/src/test/native/cpp/DARETestUtil.cpp new file mode 100644 index 0000000000..acb023641f --- /dev/null +++ b/wpimath/src/test/native/cpp/DARETestUtil.cpp @@ -0,0 +1,72 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "DARETestUtil.h" + +#include + +#include "Eigen/Eigenvalues" +#include "frc/fmt/Eigen.h" +#include "gtest/gtest.h" + +void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, + double tolerance) { + for (int row = 0; row < lhs.rows(); ++row) { + for (int col = 0; col < lhs.cols(); ++col) { + EXPECT_NEAR(lhs(row, col), rhs(row, col), tolerance) + << fmt::format("row = {}, col = {}", row, col); + } + } + + if (::testing::Test::HasFailure()) { + fmt::print("lhs =\n{}\n", lhs); + fmt::print("rhs =\n{}\n", rhs); + fmt::print("delta =\n{}\n", Eigen::MatrixXd{lhs - rhs}); + } +} + +void ExpectPositiveSemidefinite(const Eigen::Ref& X) { + Eigen::SelfAdjointEigenSolver eigX{X, + Eigen::EigenvaluesOnly}; + for (int i = 0; i < X.rows(); ++i) { + EXPECT_GE(eigX.eigenvalues()[i], 0.0); + } +} + +void ExpectDARESolution(const Eigen::Ref& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R, + const Eigen::Ref& X) { + // Check that X is the solution to the DARE + // Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0 + // clang-format off + Eigen::MatrixXd Y = + A.transpose() * X * A + - X + - (A.transpose() * X * B * (B.transpose() * X * B + R).inverse() + * B.transpose() * X * A) + + Q; + // clang-format on + ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10); +} + +void ExpectDARESolution(const Eigen::Ref& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R, + const Eigen::Ref& N, + const Eigen::Ref& X) { + // Check that X is the solution to the DARE + // Y = AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0 + // clang-format off + Eigen::MatrixXd Y = + A.transpose() * X * A + - X + - ((A.transpose() * X * B + N) * (B.transpose() * X * B + R).inverse() + * (B.transpose() * X * A + N.transpose())) + + Q; + // clang-format on + ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10); +} diff --git a/wpimath/src/test/native/cpp/DARETestUtil.h b/wpimath/src/test/native/cpp/DARETestUtil.h new file mode 100644 index 0000000000..70edd25ccf --- /dev/null +++ b/wpimath/src/test/native/cpp/DARETestUtil.h @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "Eigen/Core" + +void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, + double tolerance); + +void ExpectPositiveSemidefinite(const Eigen::Ref& X); + +void ExpectDARESolution(const Eigen::Ref& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R, + const Eigen::Ref& X); + +void ExpectDARESolution(const Eigen::Ref& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R, + const Eigen::Ref& N, + const Eigen::Ref& X); diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp new file mode 100644 index 0000000000..3ba3d71abc --- /dev/null +++ b/wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp @@ -0,0 +1,13 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/DARE.h" + +template Eigen::Matrix frc::DARE<2, 1>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R); +template Eigen::Matrix frc::DARE<2, 1>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R, + const Eigen::Matrix& N); diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp new file mode 100644 index 0000000000..75b17b8d67 --- /dev/null +++ b/wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp @@ -0,0 +1,13 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/DARE.h" + +template Eigen::Matrix frc::DARE<2, 2>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R); +template Eigen::Matrix frc::DARE<2, 2>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R, + const Eigen::Matrix& N); diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp new file mode 100644 index 0000000000..8d98553271 --- /dev/null +++ b/wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp @@ -0,0 +1,13 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/DARE.h" + +template Eigen::Matrix frc::DARE<2, 3>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R); +template Eigen::Matrix frc::DARE<2, 3>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R, + const Eigen::Matrix& N); diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp new file mode 100644 index 0000000000..840285cabd --- /dev/null +++ b/wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp @@ -0,0 +1,13 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/DARE.h" + +template Eigen::Matrix frc::DARE<4, 1>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R); +template Eigen::Matrix frc::DARE<4, 1>( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R, + const Eigen::Matrix& N);