mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpimath] Refactor DARE tests to reduce RAM usage at compile time (#5557)
This commit is contained in:
Tyler Veness
@@ -4,89 +4,60 @@
|
||||
|
||||
#include <stdexcept>
|
||||
|
||||
#include <fmt/core.h>
|
||||
|
||||
#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<double, 2, 2> frc::DARE<2, 1>(
|
||||
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
|
||||
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R);
|
||||
extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
|
||||
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
|
||||
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
|
||||
const Eigen::Matrix<double, 2, 1>& 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<double, 4, 4> frc::DARE<4, 1>(
|
||||
const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
|
||||
const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R);
|
||||
extern template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
|
||||
const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
|
||||
const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R,
|
||||
const Eigen::Matrix<double, 4, 1>& N);
|
||||
|
||||
void ExpectPositiveSemidefinite(const Eigen::Ref<const Eigen::MatrixXd>& X) {
|
||||
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigX{X,
|
||||
Eigen::EigenvaluesOnly};
|
||||
for (int i = 0; i < X.rows(); ++i) {
|
||||
EXPECT_GE(eigX.eigenvalues()[i], 0.0);
|
||||
}
|
||||
}
|
||||
// 2x2
|
||||
extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
|
||||
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
|
||||
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R);
|
||||
extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
|
||||
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
|
||||
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
|
||||
const Eigen::Matrix<double, 2, 2>& N);
|
||||
|
||||
void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& B,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& Q,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& R,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& 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<const Eigen::MatrixXd>& A,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& B,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& Q,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& R,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& N,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& 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<double, 2, 2> frc::DARE<2, 3>(
|
||||
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
|
||||
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R);
|
||||
extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
|
||||
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
|
||||
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R,
|
||||
const Eigen::Matrix<double, 2, 3>& 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);
|
||||
}
|
||||
|
||||
72
wpimath/src/test/native/cpp/DARETestUtil.cpp
Normal file
72
wpimath/src/test/native/cpp/DARETestUtil.cpp
Normal file
@@ -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 <fmt/core.h>
|
||||
|
||||
#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<const Eigen::MatrixXd>& X) {
|
||||
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigX{X,
|
||||
Eigen::EigenvaluesOnly};
|
||||
for (int i = 0; i < X.rows(); ++i) {
|
||||
EXPECT_GE(eigX.eigenvalues()[i], 0.0);
|
||||
}
|
||||
}
|
||||
|
||||
void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& B,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& Q,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& R,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& 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<const Eigen::MatrixXd>& A,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& B,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& Q,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& R,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& N,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& 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);
|
||||
}
|
||||
25
wpimath/src/test/native/cpp/DARETestUtil.h
Normal file
25
wpimath/src/test/native/cpp/DARETestUtil.h
Normal file
@@ -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<const Eigen::MatrixXd>& X);
|
||||
|
||||
void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& B,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& Q,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& R,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& X);
|
||||
|
||||
void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& B,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& Q,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& R,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& N,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& X);
|
||||
13
wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp
Normal file
13
wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp
Normal file
@@ -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<double, 2, 2> frc::DARE<2, 1>(
|
||||
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
|
||||
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R);
|
||||
template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
|
||||
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
|
||||
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
|
||||
const Eigen::Matrix<double, 2, 1>& N);
|
||||
13
wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp
Normal file
13
wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp
Normal file
@@ -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<double, 2, 2> frc::DARE<2, 2>(
|
||||
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
|
||||
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R);
|
||||
template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
|
||||
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
|
||||
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
|
||||
const Eigen::Matrix<double, 2, 2>& N);
|
||||
13
wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp
Normal file
13
wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp
Normal file
@@ -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<double, 2, 2> frc::DARE<2, 3>(
|
||||
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
|
||||
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R);
|
||||
template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
|
||||
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
|
||||
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R,
|
||||
const Eigen::Matrix<double, 2, 3>& N);
|
||||
13
wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp
Normal file
13
wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp
Normal file
@@ -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<double, 4, 4> frc::DARE<4, 1>(
|
||||
const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
|
||||
const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R);
|
||||
template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
|
||||
const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
|
||||
const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R,
|
||||
const Eigen::Matrix<double, 4, 1>& N);
|
||||
Reference in New Issue
Block a user