mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpimath] DARE: Use wpi::expected instead of exceptions (#7312)
This commit is contained in:
Tyler Veness
@@ -5,48 +5,127 @@
|
||||
#include <stdexcept>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Eigenvalues>
|
||||
#include <gtest/gtest.h>
|
||||
#include <wpi/expected>
|
||||
#include <wpi/print.h>
|
||||
|
||||
#include "DARETestUtil.h"
|
||||
#include "frc/DARE.h"
|
||||
#include "frc/EigenCore.h"
|
||||
#include "frc/fmt/Eigen.h"
|
||||
|
||||
// 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);
|
||||
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
|
||||
// 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);
|
||||
extern template wpi::expected<Eigen::Matrix<double, 4, 4>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
extern template wpi::expected<Eigen::Matrix<double, 4, 4>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
|
||||
// 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);
|
||||
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
|
||||
// 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);
|
||||
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
|
||||
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()) {
|
||||
wpi::print("lhs =\n{}\n", lhs);
|
||||
wpi::print("rhs =\n{}\n", rhs);
|
||||
wpi::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);
|
||||
}
|
||||
|
||||
TEST(DARETest, NonInvertibleA_ABQR) {
|
||||
// Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
|
||||
@@ -58,7 +137,10 @@ TEST(DARETest, NonInvertibleA_ABQR) {
|
||||
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};
|
||||
|
||||
frc::Matrixd<4, 4> X = frc::DARE<4, 1>(A, B, Q, R);
|
||||
auto ret = frc::DARE<4, 1>(A, B, Q, R);
|
||||
EXPECT_TRUE(ret);
|
||||
auto X = ret.value();
|
||||
|
||||
ExpectMatrixEqual(X, X.transpose(), 1e-10);
|
||||
ExpectPositiveSemidefinite(X);
|
||||
ExpectDARESolution(A, B, Q, R, X);
|
||||
@@ -80,7 +162,10 @@ TEST(DARETest, NonInvertibleA_ABQRN) {
|
||||
R = B.transpose() * Q * B + R;
|
||||
frc::Matrixd<4, 1> N = (A - Aref).transpose() * Q * B;
|
||||
|
||||
frc::Matrixd<4, 4> X = frc::DARE<4, 1>(A, B, Q, R, N);
|
||||
auto ret = frc::DARE<4, 1>(A, B, Q, R, N);
|
||||
EXPECT_TRUE(ret);
|
||||
auto X = ret.value();
|
||||
|
||||
ExpectMatrixEqual(X, X.transpose(), 1e-10);
|
||||
ExpectPositiveSemidefinite(X);
|
||||
ExpectDARESolution(A, B, Q, R, N, X);
|
||||
@@ -92,7 +177,10 @@ TEST(DARETest, InvertibleA_ABQR) {
|
||||
frc::Matrixd<2, 2> Q{{1, 0}, {0, 0}};
|
||||
frc::Matrixd<1, 1> R{{0.3}};
|
||||
|
||||
frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R);
|
||||
auto ret = frc::DARE<2, 1>(A, B, Q, R);
|
||||
EXPECT_TRUE(ret);
|
||||
auto X = ret.value();
|
||||
|
||||
ExpectMatrixEqual(X, X.transpose(), 1e-10);
|
||||
ExpectPositiveSemidefinite(X);
|
||||
ExpectDARESolution(A, B, Q, R, X);
|
||||
@@ -109,7 +197,10 @@ TEST(DARETest, InvertibleA_ABQRN) {
|
||||
R = B.transpose() * Q * B + R;
|
||||
frc::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B;
|
||||
|
||||
frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R, N);
|
||||
auto ret = frc::DARE<2, 1>(A, B, Q, R, N);
|
||||
EXPECT_TRUE(ret);
|
||||
auto X = ret.value();
|
||||
|
||||
ExpectMatrixEqual(X, X.transpose(), 1e-10);
|
||||
ExpectPositiveSemidefinite(X);
|
||||
ExpectDARESolution(A, B, Q, R, N, X);
|
||||
@@ -123,7 +214,10 @@ TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQR) {
|
||||
frc::Matrixd<2, 2> Q{{1, 0}, {0, 1}};
|
||||
frc::Matrixd<1, 1> R{1};
|
||||
|
||||
frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R);
|
||||
auto ret = frc::DARE<2, 1>(A, B, Q, R);
|
||||
EXPECT_TRUE(ret);
|
||||
auto X = ret.value();
|
||||
|
||||
ExpectMatrixEqual(X, X.transpose(), 1e-10);
|
||||
ExpectPositiveSemidefinite(X);
|
||||
ExpectDARESolution(A, B, Q, R, X);
|
||||
@@ -142,7 +236,10 @@ TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQRN) {
|
||||
R = B.transpose() * Q * B + R;
|
||||
frc::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B;
|
||||
|
||||
frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R, N);
|
||||
auto ret = frc::DARE<2, 1>(A, B, Q, R, N);
|
||||
EXPECT_TRUE(ret);
|
||||
auto X = ret.value();
|
||||
|
||||
ExpectMatrixEqual(X, X.transpose(), 1e-10);
|
||||
ExpectPositiveSemidefinite(X);
|
||||
ExpectDARESolution(A, B, Q, R, N, X);
|
||||
@@ -154,7 +251,10 @@ TEST(DARETest, IdentitySystem_ABQR) {
|
||||
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
|
||||
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
|
||||
|
||||
Eigen::Matrix2d X = frc::DARE<2, 2>(A, B, Q, R);
|
||||
auto ret = frc::DARE<2, 2>(A, B, Q, R);
|
||||
EXPECT_TRUE(ret);
|
||||
auto X = ret.value();
|
||||
|
||||
ExpectMatrixEqual(X, X.transpose(), 1e-10);
|
||||
ExpectPositiveSemidefinite(X);
|
||||
ExpectDARESolution(A, B, Q, R, X);
|
||||
@@ -167,7 +267,10 @@ TEST(DARETest, IdentitySystem_ABQRN) {
|
||||
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
|
||||
const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
|
||||
|
||||
Eigen::Matrix2d X = frc::DARE<2, 2>(A, B, Q, R, N);
|
||||
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
|
||||
EXPECT_TRUE(ret);
|
||||
auto X = ret.value();
|
||||
|
||||
ExpectMatrixEqual(X, X.transpose(), 1e-10);
|
||||
ExpectPositiveSemidefinite(X);
|
||||
ExpectDARESolution(A, B, Q, R, N, X);
|
||||
@@ -179,7 +282,10 @@ TEST(DARETest, MoreInputsThanStates_ABQR) {
|
||||
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
|
||||
const Eigen::Matrix3d R{Eigen::Matrix3d::Identity()};
|
||||
|
||||
Eigen::Matrix2d X = frc::DARE<2, 3>(A, B, Q, R);
|
||||
auto ret = frc::DARE<2, 3>(A, B, Q, R);
|
||||
EXPECT_TRUE(ret);
|
||||
auto X = ret.value();
|
||||
|
||||
ExpectMatrixEqual(X, X.transpose(), 1e-10);
|
||||
ExpectPositiveSemidefinite(X);
|
||||
ExpectDARESolution(A, B, Q, R, X);
|
||||
@@ -192,54 +298,115 @@ TEST(DARETest, MoreInputsThanStates_ABQRN) {
|
||||
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::Matrix2d X = frc::DARE<2, 3>(A, B, Q, R, N);
|
||||
auto ret = frc::DARE<2, 3>(A, B, Q, R, N);
|
||||
EXPECT_TRUE(ret);
|
||||
auto X = ret.value();
|
||||
|
||||
ExpectMatrixEqual(X, X.transpose(), 1e-10);
|
||||
ExpectPositiveSemidefinite(X);
|
||||
ExpectDARESolution(A, B, Q, R, N, X);
|
||||
}
|
||||
|
||||
TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQR) {
|
||||
TEST(DARETest, QNotSymmetric_ABQR) {
|
||||
const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
|
||||
const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
|
||||
const Eigen::Matrix2d Q{{1.0, 1.0}, {0.0, 1.0}};
|
||||
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
|
||||
|
||||
auto ret = frc::DARE<2, 2>(A, B, Q, R);
|
||||
EXPECT_FALSE(ret);
|
||||
EXPECT_EQ(ret.error(), frc::DAREError::QNotSymmetric);
|
||||
}
|
||||
|
||||
TEST(DARETest, QNotPositiveSemidefinite_ABQR) {
|
||||
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);
|
||||
auto ret = frc::DARE<2, 2>(A, B, Q, R);
|
||||
EXPECT_FALSE(ret);
|
||||
EXPECT_EQ(ret.error(), frc::DAREError::QNotPositiveSemidefinite);
|
||||
}
|
||||
|
||||
TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQRN) {
|
||||
TEST(DARETest, QNotSymmetric_ABQRN) {
|
||||
const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
|
||||
const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
|
||||
const Eigen::Matrix2d Q{{1.0, 1.0}, {0.0, 1.0}};
|
||||
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
|
||||
const Eigen::Matrix2d N{2.0 * Eigen::Matrix2d::Identity()};
|
||||
|
||||
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
|
||||
EXPECT_FALSE(ret);
|
||||
EXPECT_EQ(ret.error(), frc::DAREError::QNotSymmetric);
|
||||
}
|
||||
|
||||
TEST(DARETest, QNotPositiveSemidefinite_ABQRN) {
|
||||
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);
|
||||
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
|
||||
EXPECT_FALSE(ret);
|
||||
EXPECT_EQ(ret.error(), frc::DAREError::QNotPositiveSemidefinite);
|
||||
}
|
||||
|
||||
TEST(DARETest, RNotSymmetricPositiveDefinite_ABQR) {
|
||||
TEST(DARETest, RNotSymmetric_ABQR) {
|
||||
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{{1.0, 1.0}, {0.0, 1.0}};
|
||||
|
||||
auto ret = frc::DARE<2, 2>(A, B, Q, R);
|
||||
EXPECT_FALSE(ret);
|
||||
EXPECT_EQ(ret.error(), frc::DAREError::RNotSymmetric);
|
||||
}
|
||||
|
||||
TEST(DARETest, RNotPositiveDefinite_ABQR) {
|
||||
const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
|
||||
const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
|
||||
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
|
||||
|
||||
const Eigen::Matrix2d R1{Eigen::Matrix2d::Zero()};
|
||||
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R1)), std::invalid_argument);
|
||||
auto ret1 = frc::DARE<2, 2>(A, B, Q, R1);
|
||||
EXPECT_FALSE(ret1);
|
||||
EXPECT_EQ(ret1.error(), frc::DAREError::RNotPositiveDefinite);
|
||||
|
||||
const Eigen::Matrix2d R2{-Eigen::Matrix2d::Identity()};
|
||||
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R2)), std::invalid_argument);
|
||||
auto ret2 = frc::DARE<2, 2>(A, B, Q, R2);
|
||||
EXPECT_FALSE(ret2);
|
||||
EXPECT_EQ(ret2.error(), frc::DAREError::RNotPositiveDefinite);
|
||||
}
|
||||
|
||||
TEST(DARETest, RNotSymmetricPositiveDefinite_ABQRN) {
|
||||
TEST(DARETest, RNotSymmetric_ABQRN) {
|
||||
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::Matrix2d R{{1.0, 1.0}, {0.0, 1.0}};
|
||||
|
||||
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
|
||||
EXPECT_FALSE(ret);
|
||||
EXPECT_EQ(ret.error(), frc::DAREError::RNotSymmetric);
|
||||
}
|
||||
|
||||
TEST(DARETest, RNotPositiveDefinite_ABQRN) {
|
||||
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::Matrix2d R1{Eigen::Matrix2d::Zero()};
|
||||
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R1, N)), std::invalid_argument);
|
||||
auto ret1 = frc::DARE<2, 2>(A, B, Q, R1, N);
|
||||
EXPECT_FALSE(ret1);
|
||||
EXPECT_EQ(ret1.error(), frc::DAREError::RNotPositiveDefinite);
|
||||
|
||||
const Eigen::Matrix2d R2{-Eigen::Matrix2d::Identity()};
|
||||
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R2, N)), std::invalid_argument);
|
||||
auto ret2 = frc::DARE<2, 2>(A, B, Q, R2, N);
|
||||
EXPECT_FALSE(ret2);
|
||||
EXPECT_EQ(ret2.error(), frc::DAREError::RNotPositiveDefinite);
|
||||
}
|
||||
|
||||
TEST(DARETest, ABNotStabilizable_ABQR) {
|
||||
@@ -248,7 +415,9 @@ TEST(DARETest, ABNotStabilizable_ABQR) {
|
||||
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);
|
||||
auto ret = frc::DARE<2, 2>(A, B, Q, R);
|
||||
EXPECT_FALSE(ret);
|
||||
EXPECT_EQ(ret.error(), frc::DAREError::ABNotStabilizable);
|
||||
}
|
||||
|
||||
TEST(DARETest, ABNotStabilizable_ABQRN) {
|
||||
@@ -258,7 +427,9 @@ TEST(DARETest, ABNotStabilizable_ABQRN) {
|
||||
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);
|
||||
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
|
||||
EXPECT_FALSE(ret);
|
||||
EXPECT_EQ(ret.error(), frc::DAREError::ABNotStabilizable);
|
||||
}
|
||||
|
||||
TEST(DARETest, ACNotDetectable_ABQR) {
|
||||
@@ -267,7 +438,9 @@ TEST(DARETest, ACNotDetectable_ABQR) {
|
||||
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);
|
||||
auto ret = frc::DARE<2, 2>(A, B, Q, R);
|
||||
EXPECT_FALSE(ret);
|
||||
EXPECT_EQ(ret.error(), frc::DAREError::ACNotDetectable);
|
||||
}
|
||||
|
||||
TEST(DARETest, ACNotDetectable_ABQRN) {
|
||||
@@ -277,7 +450,9 @@ TEST(DARETest, ACNotDetectable_ABQRN) {
|
||||
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);
|
||||
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
|
||||
EXPECT_FALSE(ret);
|
||||
EXPECT_EQ(ret.error(), frc::DAREError::ACNotDetectable);
|
||||
}
|
||||
|
||||
TEST(DARETest, QDecomposition) {
|
||||
@@ -290,10 +465,12 @@ TEST(DARETest, QDecomposition) {
|
||||
// (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)));
|
||||
auto ret1 = frc::DARE<2, 2>(A, B, Q_1, R);
|
||||
EXPECT_TRUE(ret1);
|
||||
|
||||
// (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);
|
||||
auto ret2 = frc::DARE<2, 2>(A, B, Q_2, R);
|
||||
EXPECT_FALSE(ret2);
|
||||
}
|
||||
|
||||
@@ -1,72 +0,0 @@
|
||||
// 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 <Eigen/Eigenvalues>
|
||||
#include <gtest/gtest.h>
|
||||
#include <wpi/print.h>
|
||||
|
||||
#include "frc/fmt/Eigen.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()) {
|
||||
wpi::print("lhs =\n{}\n", lhs);
|
||||
wpi::print("rhs =\n{}\n", rhs);
|
||||
wpi::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);
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
// 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);
|
||||
@@ -4,10 +4,14 @@
|
||||
|
||||
#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);
|
||||
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
|
||||
@@ -4,10 +4,14 @@
|
||||
|
||||
#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);
|
||||
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
|
||||
@@ -4,10 +4,14 @@
|
||||
|
||||
#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);
|
||||
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
|
||||
@@ -2,12 +2,18 @@
|
||||
// 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 <wpi/expected>
|
||||
|
||||
#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);
|
||||
template wpi::expected<Eigen::Matrix<double, 4, 4>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
template wpi::expected<Eigen::Matrix<double, 4, 4>, frc::DAREError>
|
||||
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, bool checkPreconditions);
|
||||
|
||||
Reference in New Issue
Block a user