mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
73 lines
2.7 KiB
C++
73 lines
2.7 KiB
C++
// 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 <fmt/core.h>
|
||
#include <gtest/gtest.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()) {
|
||
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);
|
||
}
|