mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpimath] Rewrite DARE solver (#5328)
I timed the DARE unit tests, and the new solver is 0 to 100% faster in all cases (that is, it's at least as fast as Drake's and up to 2x faster in some cases). The new solver is also much simpler, takes less time to compile, and drops the libwpimath.so size from 325 MB to 301 MB. I think most of the compilation time is coming from the eigenvalue decompositions used to enforce argument preconditions.
This commit is contained in:
Tyler Veness
220
wpimath/src/test/native/cpp/DARETest.cpp
Normal file
220
wpimath/src/test/native/cpp/DARETest.cpp
Normal file
@@ -0,0 +1,220 @@
|
||||
// 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 <fmt/core.h>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/Eigenvalues"
|
||||
#include "frc/DARE.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);
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
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
|
||||
// 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;
|
||||
|
||||
Eigen::MatrixXd X = frc::DARE(A, B, Q, R);
|
||||
ExpectMatrixEqual(X, X.transpose(), 1e-10);
|
||||
ExpectPositiveSemidefinite(X);
|
||||
ExpectDARESolution(A, B, Q, R, X);
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
Eigen::MatrixXd Aref{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;
|
||||
|
||||
Eigen::MatrixXd X = frc::DARE(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;
|
||||
|
||||
Eigen::MatrixXd X = frc::DARE(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;
|
||||
|
||||
Eigen::MatrixXd Aref{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;
|
||||
|
||||
Eigen::MatrixXd X = frc::DARE(A, B, Q, R, N);
|
||||
ExpectMatrixEqual(X, X.transpose(), 1e-10);
|
||||
ExpectPositiveSemidefinite(X);
|
||||
ExpectDARESolution(A, B, Q, R, N, X);
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
Eigen::MatrixXd X = frc::DARE(A, B, Q, R);
|
||||
ExpectMatrixEqual(X, X.transpose(), 1e-10);
|
||||
ExpectPositiveSemidefinite(X);
|
||||
ExpectDARESolution(A, B, Q, R, X);
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
Eigen::MatrixXd Aref{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;
|
||||
|
||||
Eigen::MatrixXd X = frc::DARE(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()};
|
||||
|
||||
Eigen::MatrixXd X = frc::DARE(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()};
|
||||
|
||||
Eigen::MatrixXd X = frc::DARE(A, B, Q, R, N);
|
||||
ExpectMatrixEqual(X, X.transpose(), 1e-10);
|
||||
ExpectPositiveSemidefinite(X);
|
||||
ExpectDARESolution(A, B, Q, R, N, X);
|
||||
}
|
||||
@@ -1,41 +0,0 @@
|
||||
// TODO(jwnimmer-tri) Write our own formatting logic instead of using Eigen IO,
|
||||
// and add customization flags for how to display the matrix data.
|
||||
#undef EIGEN_NO_IO
|
||||
#include "drake/common/fmt_eigen.h"
|
||||
|
||||
#include <limits>
|
||||
#include <sstream>
|
||||
|
||||
namespace drake {
|
||||
namespace internal {
|
||||
|
||||
template <typename T>
|
||||
using FormatterEigenRef =
|
||||
Eigen::Ref<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>>;
|
||||
|
||||
template <typename T>
|
||||
std::string FormatEigenMatrix(const FormatterEigenRef<T>& matrix) {
|
||||
std::stringstream stream;
|
||||
// We'll print our matrix data using as much precision as we can, so that
|
||||
// console log output and/or error messages paint the full picture. Sadly,
|
||||
// the ostream family of floating-point formatters doesn't know how to do
|
||||
// "shortest round-trip precision". If we set the precision to max_digits,
|
||||
// then simple numbers like "1.1" print as "1.1000000000000001"; instead,
|
||||
// we'll use max_digits - 1 to avoid that problem, with the risk of losing
|
||||
// the last ulps in the printout in case we needed that last digit. This
|
||||
// will all be fixed once we stop using Eigen IO.
|
||||
stream.precision(std::numeric_limits<double>::max_digits10 - 1);
|
||||
stream << matrix;
|
||||
return stream.str();
|
||||
}
|
||||
|
||||
// Explicitly instantiate for the allowed scalar types in our header.
|
||||
template std::string FormatEigenMatrix<double>(
|
||||
const FormatterEigenRef<double>& matrix);
|
||||
template std::string FormatEigenMatrix<float>(
|
||||
const FormatterEigenRef<float>& matrix);
|
||||
template std::string FormatEigenMatrix<std::string>(
|
||||
const FormatterEigenRef<std::string>& matrix);
|
||||
|
||||
} // namespace internal
|
||||
} // namespace drake
|
||||
@@ -1,124 +0,0 @@
|
||||
#include "drake/math/discrete_algebraic_riccati_equation.h"
|
||||
|
||||
#include <Eigen/Eigenvalues>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "drake/common/test_utilities/eigen_matrix_compare.h"
|
||||
// #include "drake/math/autodiff.h"
|
||||
|
||||
using Eigen::MatrixXd;
|
||||
|
||||
namespace drake {
|
||||
namespace math {
|
||||
namespace {
|
||||
void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
|
||||
const Eigen::Ref<const MatrixXd>& B,
|
||||
const Eigen::Ref<const MatrixXd>& Q,
|
||||
const Eigen::Ref<const MatrixXd>& R) {
|
||||
MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R);
|
||||
// Check that X is positive semi-definite.
|
||||
EXPECT_TRUE(
|
||||
CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute));
|
||||
int n = X.rows();
|
||||
Eigen::SelfAdjointEigenSolver<MatrixXd> es(X);
|
||||
for (int i = 0; i < n; i++) {
|
||||
EXPECT_GE(es.eigenvalues()[i], 0);
|
||||
}
|
||||
// Check that X is the solution to the discrete time ARE.
|
||||
// clang-format off
|
||||
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
|
||||
EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10,
|
||||
MatrixCompareType::absolute));
|
||||
}
|
||||
|
||||
void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
|
||||
const Eigen::Ref<const MatrixXd>& B,
|
||||
const Eigen::Ref<const MatrixXd>& Q,
|
||||
const Eigen::Ref<const MatrixXd>& R,
|
||||
const Eigen::Ref<const MatrixXd>& N) {
|
||||
MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R, N);
|
||||
// Check that X is positive semi-definite.
|
||||
EXPECT_TRUE(
|
||||
CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute));
|
||||
int n = X.rows();
|
||||
Eigen::SelfAdjointEigenSolver<MatrixXd> es(X);
|
||||
for (int i = 0; i < n; i++) {
|
||||
EXPECT_GE(es.eigenvalues()[i], 0);
|
||||
}
|
||||
// Check that X is the solution to the discrete time ARE.
|
||||
// clang-format off
|
||||
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
|
||||
EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10,
|
||||
MatrixCompareType::absolute));
|
||||
}
|
||||
|
||||
GTEST_TEST(DARE, SolveDAREandVerify) {
|
||||
// Test 1: non-invertible A
|
||||
// Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
|
||||
// Riccati Equation"
|
||||
int n1 = 4, m1 = 1;
|
||||
MatrixXd A1(n1, n1), B1(n1, m1), Q1(n1, n1), R1(m1, m1);
|
||||
A1 << 0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0;
|
||||
B1 << 0, 0, 0, 1;
|
||||
Q1 << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
|
||||
R1 << 0.25;
|
||||
SolveDAREandVerify(A1, B1, Q1, R1);
|
||||
|
||||
MatrixXd Aref1(n1, n1);
|
||||
Aref1 << 0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0;
|
||||
SolveDAREandVerify(A1, B1, (A1 - Aref1).transpose() * Q1 * (A1 - Aref1),
|
||||
B1.transpose() * Q1 * B1 + R1, (A1 - Aref1).transpose() * Q1 * B1);
|
||||
|
||||
// Test 2: invertible A
|
||||
int n2 = 2, m2 = 1;
|
||||
MatrixXd A2(n2, n2), B2(n2, m2), Q2(n2, n2), R2(m2, m2);
|
||||
A2 << 1, 1, 0, 1;
|
||||
B2 << 0, 1;
|
||||
Q2 << 1, 0, 0, 0;
|
||||
R2 << 0.3;
|
||||
SolveDAREandVerify(A2, B2, Q2, R2);
|
||||
|
||||
MatrixXd Aref2(n2, n2);
|
||||
Aref2 << 0.5, 1, 0, 1;
|
||||
SolveDAREandVerify(A2, B2, (A2 - Aref2).transpose() * Q2 * (A2 - Aref2),
|
||||
B2.transpose() * Q2 * B2 + R2, (A2 - Aref2).transpose() * Q2 * B2);
|
||||
|
||||
// Test 3: the first generalized eigenvalue of (S,T) is stable
|
||||
int n3 = 2, m3 = 1;
|
||||
MatrixXd A3(n3, n3), B3(n3, m3), Q3(n3, n3), R3(m3, m3);
|
||||
A3 << 0, 1, 0, 0;
|
||||
B3 << 0, 1;
|
||||
Q3 << 1, 0, 0, 1;
|
||||
R3 << 1;
|
||||
SolveDAREandVerify(A3, B3, Q3, R3);
|
||||
|
||||
MatrixXd Aref3(n3, n3);
|
||||
Aref3 << 0, 0.5, 0, 0;
|
||||
SolveDAREandVerify(A3, B3, (A3 - Aref3).transpose() * Q3 * (A3 - Aref3),
|
||||
B3.transpose() * Q3 * B3 + R3, (A3 - Aref3).transpose() * Q3 * B3);
|
||||
|
||||
// Test 4: A = B = Q = R = I_2 (2-by-2 identity matrix)
|
||||
const Eigen::MatrixXd A4{Eigen::Matrix2d::Identity()};
|
||||
const Eigen::MatrixXd B4{Eigen::Matrix2d::Identity()};
|
||||
const Eigen::MatrixXd Q4{Eigen::Matrix2d::Identity()};
|
||||
const Eigen::MatrixXd R4{Eigen::Matrix2d::Identity()};
|
||||
SolveDAREandVerify(A4, B4, Q4, R4);
|
||||
|
||||
const Eigen::MatrixXd N4{Eigen::Matrix2d::Identity()};
|
||||
SolveDAREandVerify(A4, B4, Q4, R4, N4);
|
||||
}
|
||||
} // namespace
|
||||
} // namespace math
|
||||
} // namespace drake
|
||||
Reference in New Issue
Block a user