Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add static matrix support to DARE solver (#5536)

Browse Source 
Using static matrices where possible results in a 2x performance
improvement.
This commit is contained in:
Tyler Veness
2023-08-14 09:15:58 -07:00
committed by GitHub
parent 394cfeadbd
commit 03764dfe93
9 changed files with 354 additions and 436 deletions
Download Patch File Download Diff File Expand all files Collapse all files

284
wpimath/src/main/native/cpp/DARE.cpp
View File

@@ -1,284 +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 "frc/DARE.h"
#include <cassert>
#include <stdexcept>
#include <string>
#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "Eigen/LU"
#include "Eigen/QR"
#include "frc/fmt/Eigen.h"
// Works cited:
//
// [1] E. K.-W. Chu, H.-Y. Fan, W.-W. Lin & C.-S. Wang "Structure-Preserving
// Algorithms for Periodic Discrete-Time Algebraic Riccati Equations",
// International Journal of Control, 77:8, 767-788, 2004.
// DOI: 10.1080/00207170410001714988
namespace frc {
namespace {
/**
* Returns true if (A, B) is a stabilizable pair.
*
* (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
* any, have absolute values less than one, where an eigenvalue is
* uncontrollable if rank([λI - A, B]) < n where n is the number of states.
*
* @param A System matrix.
* @param B Input matrix.
*/
bool IsStabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B) {
Eigen::EigenSolver<Eigen::MatrixXd> es{A, false};
for (int i = 0; i < A.rows(); ++i) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
1) {
continue;
}
Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(A.rows(), A.cols()) -
A,
B;
Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
if (qr.rank() < A.rows()) {
return false;
}
}
return true;
}
/**
* Returns true if (A, C) is a detectable pair.
*
* (A, C) is detectable if and only if the unobservable eigenvalues of A, if
* any, have absolute values less than one, where an eigenvalue is unobservable
* if rank([λI - A; C]) < n where n is the number of states.
*
* @param A System matrix.
* @param C Output matrix.
*/
bool IsDetectable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& C) {
return IsStabilizable(A.transpose(), C.transpose());
}
} // namespace
Eigen::MatrixXd DARE(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) {
// These are unused if assertions aren't compiled in
[[maybe_unused]] int states = A.rows();
[[maybe_unused]] int inputs = B.cols();
// Check argument dimensions
assert(A.rows() == states && A.cols() == states);
assert(B.rows() == states && B.cols() == inputs);
assert(Q.rows() == states && Q.cols() == states);
assert(R.rows() == inputs && R.cols() == inputs);
// Require Q be symmetric
if ((Q - Q.transpose()).norm() > 1e-10) {
std::string msg =
fmt::format("Q isn't symmetric!\n\nQ =\n{}\n", Eigen::MatrixXd{Q});
throw std::invalid_argument(msg);
}
// Require Q be positive semidefinite
//
// If Q is a symmetric matrix with a decomposition LDLᵀ, the number of
// positive, negative, and zero diagonal entries in D equals the number of
// positive, negative, and zero eigenvalues respectively in Q (see
// https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
//
// Therefore, D having no negative diagonal entries is sufficient to prove Q
// is positive semidefinite.
auto Q_ldlt = Q.ldlt();
if (Q_ldlt.info() != Eigen::Success ||
(Q_ldlt.vectorD().array() < 0.0).any()) {
std::string msg = fmt::format("Q isn't positive semidefinite!\n\nQ =\n{}\n",
Eigen::MatrixXd{Q});
throw std::invalid_argument(msg);
}
// Require R be symmetric
if ((R - R.transpose()).norm() > 1e-10) {
std::string msg =
fmt::format("R isn't symmetric!\n\nR =\n{}\n", Eigen::MatrixXd{R});
throw std::invalid_argument(msg);
}
// Require (A, B) pair be stabilizable
if (!IsStabilizable(A, B)) {
std::string msg =
fmt::format("The (A, B) pair isn't stabilizable!\n\nA =\n{}\nB =\n{}\n",
Eigen::MatrixXd{A}, Eigen::MatrixXd{B});
throw std::invalid_argument(msg);
}
// Require (A, C) pair be detectable where Q = CᵀC
{
Eigen::MatrixXd C = Eigen::MatrixXd{Q_ldlt.matrixL()} *
Q_ldlt.vectorD().cwiseSqrt().asDiagonal();
if (!IsDetectable(A, C)) {
std::string msg = fmt::format(
"The (A, C) pair where Q = CᵀC isn't detectable!\n\nA =\n{}\nQ "
"=\n{}\n",
Eigen::MatrixXd{A}, Eigen::MatrixXd{Q});
throw std::invalid_argument(msg);
}
}
return internal::DARE(A, B, Q, R);
}
Eigen::MatrixXd DARE(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) {
// These are unused if assertions aren't compiled in
[[maybe_unused]] int states = A.rows();
[[maybe_unused]] int inputs = B.cols();
// Check argument dimensions
assert(N.rows() == states && N.cols() == inputs);
auto R_llt = R.llt();
if (R_llt.info() != Eigen::Success) {
std::string msg = fmt::format("R isn't positive definite!\n\nR =\n{}\n",
Eigen::MatrixXd{R});
throw std::invalid_argument(msg);
}
// This is a change of variables to make the DARE that includes Q, R, and N
// cost matrices fit the form of the DARE that includes only Q and R cost
// matrices.
//
// This is equivalent to solving the original DARE:
//
// A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
//
// where A₂ and Q₂ are a change of variables:
//
// A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
return DARE(A - B * R_llt.solve(N.transpose()), B,
Q - N * R_llt.solve(N.transpose()), R);
}
namespace internal {
Eigen::MatrixXd DARE(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) {
// Require R be positive definite
auto R_llt = R.llt();
if (R_llt.info() != Eigen::Success) {
std::string msg = fmt::format("R isn't positive definite!\n\nR =\n{}\n",
Eigen::MatrixXd{R});
throw std::invalid_argument(msg);
}
// Implements the SDA algorithm on page 5 of [1].
// A₀ = A
Eigen::MatrixXd A_k = A;
// G₀ = BR⁻¹Bᵀ
//
// See equation (4) of [1].
Eigen::MatrixXd G_k = B * R_llt.solve(B.transpose());
// H₀ = Q
//
// See equation (4) of [1].
Eigen::MatrixXd H_k;
Eigen::MatrixXd H_k1 = Q;
do {
H_k = H_k1;
// W = I + GₖHₖ
Eigen::MatrixXd W =
Eigen::MatrixXd::Identity(H_k.rows(), H_k.cols()) + G_k * H_k;
auto W_solver = W.lu();
// Solve WV₁ = Aₖ for V₁
Eigen::MatrixXd V_1 = W_solver.solve(A_k);
// Solve V₂Wᵀ = Gₖ for V₂
//
// We want to put V₂Wᵀ = Gₖ into Ax = b form so we can solve it more
// efficiently.
//
// V₂Wᵀ = Gₖ
// (V₂Wᵀ)ᵀ = Gₖᵀ
// WV₂ᵀ = Gₖᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// V₂ᵀ = W.solve(Gₖᵀ)
// V₂ = W.solve(Gₖᵀ)ᵀ
Eigen::MatrixXd V_2 = W_solver.solve(G_k.transpose()).transpose();
// Gₖ₊₁ = Gₖ + AₖV₂Aₖᵀ
G_k += A_k * V_2 * A_k.transpose();
// Hₖ₊₁ = Hₖ + V₁ᵀHₖAₖ
H_k1 = H_k + V_1.transpose() * H_k * A_k;
// Aₖ₊₁ = AₖV₁
A_k *= V_1;
// while |Hₖ₊₁ − Hₖ| > ε |Hₖ₊₁|
} while ((H_k1 - H_k).norm() > 1e-10 * H_k1.norm());
return H_k1;
}
Eigen::MatrixXd DARE(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) {
auto R_llt = R.llt();
if (R_llt.info() != Eigen::Success) {
std::string msg = fmt::format("R isn't positive definite!\n\nR =\n{}\n",
Eigen::MatrixXd{R});
throw std::invalid_argument(msg);
}
// This is a change of variables to make the DARE that includes Q, R, and N
// cost matrices fit the form of the DARE that includes only Q and R cost
// matrices.
//
// This is equivalent to solving the original DARE:
//
// A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
//
// where A₂ and Q₂ are a change of variables:
//
// A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
return internal::DARE(A - B * R_llt.solve(N.transpose()), B,
Q - N * R_llt.solve(N.transpose()), R);
}
} // namespace internal
} // namespace frc

6
wpimath/src/main/native/cpp/StateSpaceUtil.cpp
View File

@@ -28,6 +28,12 @@ bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B) {
return detail::IsStabilizableImpl<2, 1>(A, B);
}
template <>
bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B) {
return detail::IsStabilizableImpl<Eigen::Dynamic, Eigen::Dynamic>(A, B);
}
Vectord<3> PoseToVector(const Pose2d& pose) {
return Vectord<3>{pose.X().value(), pose.Y().value(),
pose.Rotation().Radians().value()};

48
wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
View File

@@ -11,7 +11,6 @@
#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "Eigen/QR"
#include "edu_wpi_first_math_WPIMathJNI.h"
#include "frc/DARE.h"
@@ -21,44 +20,6 @@
using namespace wpi::java;
namespace {
/**
* Returns true if (A, B) is a stabilizable pair.
*
* (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
* any, have absolute values less than one, where an eigenvalue is
* uncontrollable if rank([λI - A, B]) < n where n is the number of states.
*
* @param A System matrix.
* @param B Input matrix.
*/
bool IsStabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B) {
Eigen::EigenSolver<Eigen::MatrixXd> es{A, false};
for (int i = 0; i < A.rows(); ++i) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
1) {
continue;
}
Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
A,
B;
Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
if (qr.rank() < A.rows()) {
return false;
}
}
return true;
}
} // namespace
std::vector<double> GetElementsFromTrajectory(
const frc::Trajectory& trajectory) {
std::vector<double> elements;
@@ -130,7 +91,8 @@ Java_edu_wpi_first_math_WPIMathJNI_dareABQR
Rmat{nativeR, inputs, inputs};
try {
Eigen::MatrixXd result = frc::DARE(Amat, Bmat, Qmat, Rmat);
Eigen::MatrixXd result =
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat);
env->ReleaseDoubleArrayElements(A, nativeA, 0);
env->ReleaseDoubleArrayElements(B, nativeB, 0);
@@ -179,7 +141,8 @@ Java_edu_wpi_first_math_WPIMathJNI_dareABQRN
Nmat{nativeN, states, inputs};
try {
Eigen::MatrixXd result = frc::DARE(Amat, Bmat, Qmat, Rmat, Nmat);
Eigen::MatrixXd result =
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat, Nmat);
env->ReleaseDoubleArrayElements(A, nativeA, 0);
env->ReleaseDoubleArrayElements(B, nativeB, 0);
@@ -314,7 +277,8 @@ Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
B{nativeB, states, inputs};
bool isStabilizable = IsStabilizable(A, B);
bool isStabilizable =
frc::IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(A, B);
env->ReleaseDoubleArrayElements(aSrc, nativeA, 0);
env->ReleaseDoubleArrayElements(bSrc, nativeB, 0);

359
wpimath/src/main/native/include/frc/DARE.h
View File

@@ -4,86 +4,88 @@
#pragma once
#include <wpi/SymbolExports.h>
#include <stdexcept>
#include <string>
#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "Eigen/LU"
#include "frc/StateSpaceUtil.h"
#include "frc/fmt/Eigen.h"
// Works cited:
//
// [1] E. K.-W. Chu, H.-Y. Fan, W.-W. Lin & C.-S. Wang "Structure-Preserving
// Algorithms for Periodic Discrete-Time Algebraic Riccati Equations",
// International Journal of Control, 77:8, 767-788, 2004.
// DOI: 10.1080/00207170410001714988
namespace frc {
namespace detail {
/**
* Computes the unique stabilizing solution X to the discrete-time algebraic
* Riccati equation:
*
* AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
* Checks the preconditions of A, B, and Q for the DARE solver.
*
* @tparam States Number of states.
* @tparam Inputs Number of inputs.
* @param A The system matrix.
* @param B The input matrix.
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @throws std::invalid_argument if Q isn't symmetric positive semidefinite.
* @throws std::invalid_argument if R isn't symmetric positive definite.
* @throws std::invalid_argument if the (A, B) pair isn't stabilizable.
* @throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't
* detectable.
*/
WPILIB_DLLEXPORT
Eigen::MatrixXd DARE(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);
template <int States, int Inputs>
void CheckDARE_ABQ(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q) {
// Require Q be symmetric
if ((Q - Q.transpose()).norm() > 1e-10) {
std::string msg = fmt::format("Q isn't symmetric!\n\nQ =\n{}\n", Q);
throw std::invalid_argument(msg);
}
/**
Computes the unique stabilizing solution X to the discrete-time algebraic
Riccati equation:
// Require Q be positive semidefinite
//
// If Q is a symmetric matrix with a decomposition LDLᵀ, the number of
// positive, negative, and zero diagonal entries in D equals the number of
// positive, negative, and zero eigenvalues respectively in Q (see
// https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
//
// Therefore, D having no negative diagonal entries is sufficient to prove Q
// is positive semidefinite.
auto Q_ldlt = Q.ldlt();
if (Q_ldlt.info() != Eigen::Success ||
(Q_ldlt.vectorD().array() < 0.0).any()) {
std::string msg =
fmt::format("Q isn't positive semidefinite!\n\nQ =\n{}\n", Q);
throw std::invalid_argument(msg);
}
AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
// Require (A, B) pair be stabilizable
if (!IsStabilizable<States, Inputs>(A, B)) {
std::string msg = fmt::format(
"The (A, B) pair isn't stabilizable!\n\nA =\n{}\nB =\n{}\n", A, B);
throw std::invalid_argument(msg);
}
This overload of the DARE is useful for finding the control law uₖ that
minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
// Require (A, C) pair be detectable where Q = CᵀC
{
Eigen::Matrix<double, States, States> C =
Eigen::Matrix<double, States, States>{Q_ldlt.matrixL()} *
Q_ldlt.vectorD().cwiseSqrt().asDiagonal();
@verbatim
∞ [xₖ]ᵀ[Q N][xₖ]
J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
k=0
@endverbatim
This is a more general form of the following. The linear-quadratic regulator
is the feedback control law uₖ that minimizes the following cost function
subject to xₖ₊₁ = Axₖ + Buₖ:
@verbatim
∞
J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
k=0
@endverbatim
This can be refactored as:
@verbatim
∞ [xₖ]ᵀ[Q 0][xₖ]
J = Σ [uₖ] [0 R][uₖ] ΔT
k=0
@endverbatim
@param A The system matrix.
@param B The input matrix.
@param Q The state cost matrix.
@param R The input cost matrix.
@param N The state-input cross cost matrix.
@throws std::invalid_argument if Q − NR⁻¹Nᵀ isn't symmetric positive
semidefinite.
@throws std::invalid_argument if R isn't symmetric positive definite.
@throws std::invalid_argument if the (A, B) pair isn't stabilizable.
@throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't detectable.
*/
WPILIB_DLLEXPORT
Eigen::MatrixXd DARE(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);
namespace internal {
if (!IsDetectable<States, States>(A, C)) {
std::string msg = fmt::format(
"The (A, C) pair where Q = CᵀC isn't detectable!\n\nA =\n{}\nQ "
"=\n{}\n",
A, Q);
throw std::invalid_argument(msg);
}
}
}
/**
* Computes the unique stabilizing solution X to the discrete-time algebraic
@@ -101,16 +103,77 @@ namespace internal {
* </ul>
* Only use this function if you're sure the preconditions are met.
*
* @tparam States Number of states.
* @tparam Inputs Number of inputs.
* @param A The system matrix.
* @param B The input matrix.
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @param R_llt The LLT decomposition of the input cost matrix.
*/
WPILIB_DLLEXPORT
Eigen::MatrixXd DARE(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);
template <int States, int Inputs>
Eigen::Matrix<double, States, States> DARE(
const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::LLT<Eigen::Matrix<double, Inputs, Inputs>>& R_llt) {
using StateMatrix = Eigen::Matrix<double, States, States>;
// Implements the SDA algorithm on page 5 of [1].
// A₀ = A
StateMatrix A_k = A;
// G₀ = BR⁻¹Bᵀ
//
// See equation (4) of [1].
StateMatrix G_k = B * R_llt.solve(B.transpose());
// H₀ = Q
//
// See equation (4) of [1].
StateMatrix H_k;
StateMatrix H_k1 = Q;
do {
H_k = H_k1;
// W = I + GₖHₖ
StateMatrix W = StateMatrix::Identity(H_k.rows(), H_k.cols()) + G_k * H_k;
auto W_solver = W.lu();
// Solve WV₁ = Aₖ for V₁
StateMatrix V_1 = W_solver.solve(A_k);
// Solve V₂Wᵀ = Gₖ for V₂
//
// We want to put V₂Wᵀ = Gₖ into Ax = b form so we can solve it more
// efficiently.
//
// V₂Wᵀ = Gₖ
// (V₂Wᵀ)ᵀ = Gₖᵀ
// WV₂ᵀ = Gₖᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// V₂ᵀ = W.solve(Gₖᵀ)
// V₂ = W.solve(Gₖᵀ)ᵀ
StateMatrix V_2 = W_solver.solve(G_k.transpose()).transpose();
// Gₖ₊₁ = Gₖ + AₖV₂Aₖᵀ
G_k += A_k * V_2 * A_k.transpose();
// Hₖ₊₁ = Hₖ + V₁ᵀHₖAₖ
H_k1 = H_k + V_1.transpose() * H_k * A_k;
// Aₖ₊₁ = AₖV₁
A_k *= V_1;
// while |Hₖ₊₁ − Hₖ| > ε |Hₖ₊₁|
} while ((H_k1 - H_k).norm() > 1e-10 * H_k1.norm());
return H_k1;
}
/**
Computes the unique stabilizing solution X to the discrete-time algebraic
@@ -155,18 +218,166 @@ performance. The solver may hang if any of the following occur:
</ul>
Only use this function if you're sure the preconditions are met.
@tparam States Number of states.
@tparam Inputs Number of inputs.
@param A The system matrix.
@param B The input matrix.
@param Q The state cost matrix.
@param R_llt The LLT decomposition of the input cost matrix.
@param N The state-input cross cost matrix.
*/
template <int States, int Inputs>
Eigen::Matrix<double, States, States> DARE(
const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::LLT<Eigen::Matrix<double, Inputs, Inputs>>& R_llt,
const Eigen::Matrix<double, Inputs, Inputs>& N) {
// This is a change of variables to make the DARE that includes Q, R, and N
// cost matrices fit the form of the DARE that includes only Q and R cost
// matrices.
//
// This is equivalent to solving the original DARE:
//
// A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
//
// where A₂ and Q₂ are a change of variables:
//
// A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
return detail::DARE<States, Inputs>(A - B * R_llt.solve(N.transpose()), B,
Q - N * R_llt.solve(N.transpose()),
R_llt);
}
} // namespace detail
/**
* Computes the unique stabilizing solution X to the discrete-time algebraic
* Riccati equation:
*
* AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
*
* @tparam States Number of states.
* @tparam Inputs Number of inputs.
* @param A The system matrix.
* @param B The input matrix.
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @throws std::invalid_argument if Q isn't symmetric positive semidefinite.
* @throws std::invalid_argument if R isn't symmetric positive definite.
* @throws std::invalid_argument if the (A, B) pair isn't stabilizable.
* @throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't
* detectable.
*/
template <int States, int Inputs>
Eigen::Matrix<double, States, States> DARE(
const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R) {
// Require R be symmetric
if ((R - R.transpose()).norm() > 1e-10) {
std::string msg = fmt::format("R isn't symmetric!\n\nR =\n{}\n", R);
throw std::invalid_argument(msg);
}
// Require R be positive definite
auto R_llt = R.llt();
if (R_llt.info() != Eigen::Success) {
std::string msg = fmt::format("R isn't positive definite!\n\nR =\n{}\n", R);
throw std::invalid_argument(msg);
}
detail::CheckDARE_ABQ<States, Inputs>(A, B, Q);
return detail::DARE<States, Inputs>(A, B, Q, R_llt);
}
/**
Computes the unique stabilizing solution X to the discrete-time algebraic
Riccati equation:
AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
This overload of the DARE is useful for finding the control law uₖ that
minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
@verbatim
∞ [xₖ]ᵀ[Q N][xₖ]
J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
k=0
@endverbatim
This is a more general form of the following. The linear-quadratic regulator
is the feedback control law uₖ that minimizes the following cost function
subject to xₖ₊₁ = Axₖ + Buₖ:
@verbatim
∞
J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
k=0
@endverbatim
This can be refactored as:
@verbatim
∞ [xₖ]ᵀ[Q 0][xₖ]
J = Σ [uₖ] [0 R][uₖ] ΔT
k=0
@endverbatim
@tparam States Number of states.
@tparam Inputs Number of inputs.
@param A The system matrix.
@param B The input matrix.
@param Q The state cost matrix.
@param R The input cost matrix.
@param N The state-input cross cost matrix.
@throws std::invalid_argument if Q − NR⁻¹Nᵀ isn't symmetric positive
semidefinite.
@throws std::invalid_argument if R isn't symmetric positive definite.
@throws std::invalid_argument if the (A, B) pair isn't stabilizable.
@throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't detectable.
*/
WPILIB_DLLEXPORT
Eigen::MatrixXd DARE(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);
template <int States, int Inputs>
Eigen::Matrix<double, States, States> DARE(
const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R,
const Eigen::Matrix<double, States, Inputs>& N) {
// Require R be symmetric
if ((R - R.transpose()).norm() > 1e-10) {
std::string msg = fmt::format("R isn't symmetric!\n\nR =\n{}\n", R);
throw std::invalid_argument(msg);
}
// Require R be positive definite
auto R_llt = R.llt();
if (R_llt.info() != Eigen::Success) {
std::string msg = fmt::format("R isn't positive definite!\n\nR =\n{}\n", R);
throw std::invalid_argument(msg);
}
// This is a change of variables to make the DARE that includes Q, R, and N
// cost matrices fit the form of the DARE that includes only Q and R cost
// matrices.
//
// This is equivalent to solving the original DARE:
//
// A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
//
// where A₂ and Q₂ are a change of variables:
//
// A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
Eigen::Matrix<double, States, States> A_2 =
A - B * R_llt.solve(N.transpose());
Eigen::Matrix<double, States, States> Q_2 =
Q - N * R_llt.solve(N.transpose());
detail::CheckDARE_ABQ<States, Inputs>(A_2, B, Q_2);
return detail::DARE<States, Inputs>(A_2, B, Q_2, R_llt);
}
} // namespace internal
} // namespace frc

41
wpimath/src/main/native/include/frc/StateSpaceUtil.h
View File

@@ -58,24 +58,37 @@ bool IsStabilizableImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B) {
Eigen::EigenSolver<Matrixd<States, States>> es{A, false};
for (int i = 0; i < States; ++i) {
for (int i = 0; i < A.rows(); ++i) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
1) {
continue;
}
Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
States>::Identity() -
A,
B;
if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) {
Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
States>::Identity() -
A,
B;
Eigen::ColPivHouseholderQR<
Eigen::Matrix<std::complex<double>, States, States + Inputs>>
qr{E};
if (qr.rank() < States) {
return false;
Eigen::ColPivHouseholderQR<
Eigen::Matrix<std::complex<double>, States, States + Inputs>>
qr{E};
if (qr.rank() < States) {
return false;
}
} else {
Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
E << es.eigenvalues()[i] *
Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
A,
B;
Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
if (qr.rank() < A.rows()) {
return false;
}
}
}
return true;
@@ -274,6 +287,12 @@ template <>
WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
const Matrixd<2, 1>& B);
// Template specializations are used here to make common state-input pairs
// compile faster.
template <>
WPILIB_DLLEXPORT bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
/**
* Converts a Pose2d into a vector of [x, y, theta].
*

4
wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
View File

@@ -51,7 +51,7 @@ LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
throw std::invalid_argument(msg);
}
Matrixd<States, States> S = DARE(discA, discB, Q, R);
Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
m_K = (discB.transpose() * S * discB + R)
@@ -70,7 +70,7 @@ LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
Matrixd<States, States> S = DARE(discA, discB, Q, R, N);
Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
m_K = (discB.transpose() * S * discB + R)

6
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
View File

@@ -43,7 +43,8 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP = DARE(discA.transpose(), C.transpose(), discQ, discR);
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = StateMatrix::Zero();
}
@@ -78,7 +79,8 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP = DARE(discA.transpose(), C.transpose(), discQ, discR);
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = StateMatrix::Zero();
}

2
wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
View File

@@ -48,7 +48,7 @@ KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
}
Matrixd<States, States> P =
DARE(discA.transpose(), C.transpose(), discQ, discR);
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
// S = CPCᵀ + R
Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;

40
wpimath/src/test/native/cpp/DARETest.cpp
View File

@@ -86,7 +86,7 @@ TEST(DARETest, NonInvertibleA_ABQR) {
Eigen::MatrixXd R{1, 1};
R << 0.25;
Eigen::MatrixXd X = frc::DARE(A, B, Q, R);
Eigen::MatrixXd X = frc::DARE<4, 1>(A, B, Q, R);
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, X);
@@ -111,7 +111,7 @@ TEST(DARETest, NonInvertibleA_ABQRN) {
R = B.transpose() * Q * B + R;
Eigen::MatrixXd N = (A - Aref).transpose() * Q * B;
Eigen::MatrixXd X = frc::DARE(A, B, Q, R, N);
Eigen::MatrixXd 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);
@@ -127,7 +127,7 @@ TEST(DARETest, InvertibleA_ABQR) {
Eigen::MatrixXd R{1, 1};
R << 0.3;
Eigen::MatrixXd X = frc::DARE(A, B, Q, R);
Eigen::MatrixXd X = frc::DARE<2, 1>(A, B, Q, R);
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, X);
@@ -149,7 +149,7 @@ TEST(DARETest, InvertibleA_ABQRN) {
R = B.transpose() * Q * B + R;
Eigen::MatrixXd N = (A - Aref).transpose() * Q * B;
Eigen::MatrixXd X = frc::DARE(A, B, Q, R, N);
Eigen::MatrixXd 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);
@@ -167,7 +167,7 @@ TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQR) {
Eigen::MatrixXd R{1, 1};
R << 1;
Eigen::MatrixXd X = frc::DARE(A, B, Q, R);
Eigen::MatrixXd X = frc::DARE<2, 1>(A, B, Q, R);
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, X);
@@ -191,7 +191,7 @@ TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQRN) {
R = B.transpose() * Q * B + R;
Eigen::MatrixXd N = (A - Aref).transpose() * Q * B;
Eigen::MatrixXd X = frc::DARE(A, B, Q, R, N);
Eigen::MatrixXd 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);
@@ -203,7 +203,7 @@ TEST(DARETest, IdentitySystem_ABQR) {
const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()};
Eigen::MatrixXd X = frc::DARE(A, B, Q, R);
Eigen::MatrixXd X = frc::DARE<2, 2>(A, B, Q, R);
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, X);
@@ -216,7 +216,7 @@ TEST(DARETest, IdentitySystem_ABQRN) {
const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd N{Eigen::Matrix2d::Identity()};
Eigen::MatrixXd X = frc::DARE(A, B, Q, R, N);
Eigen::MatrixXd 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);
@@ -228,7 +228,7 @@ TEST(DARETest, MoreInputsThanStates_ABQR) {
const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd R{Eigen::Matrix3d::Identity()};
Eigen::MatrixXd X = frc::DARE(A, B, Q, R);
Eigen::MatrixXd X = frc::DARE<2, 3>(A, B, Q, R);
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, X);
@@ -241,7 +241,7 @@ TEST(DARETest, MoreInputsThanStates_ABQRN) {
const Eigen::MatrixXd R{Eigen::Matrix3d::Identity()};
const Eigen::MatrixXd N{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}};
Eigen::MatrixXd X = frc::DARE(A, B, Q, R, N);
Eigen::MatrixXd 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);
@@ -253,7 +253,7 @@ TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQR) {
const Eigen::MatrixXd Q{-Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()};
EXPECT_THROW(frc::DARE(A, B, Q, R), std::invalid_argument);
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument);
}
TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQRN) {
@@ -263,7 +263,7 @@ TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQRN) {
const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd N{2.0 * Eigen::Matrix2d::Identity()};
EXPECT_THROW(frc::DARE(A, B, Q, R, N), std::invalid_argument);
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
}
TEST(DARETest, RNotSymmetricPositiveDefinite_ABQR) {
@@ -272,10 +272,10 @@ TEST(DARETest, RNotSymmetricPositiveDefinite_ABQR) {
const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd R1{Eigen::Matrix2d::Zero()};
EXPECT_THROW(frc::DARE(A, B, Q, R1), std::invalid_argument);
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R1)), std::invalid_argument);
const Eigen::MatrixXd R2{-Eigen::Matrix2d::Identity()};
EXPECT_THROW(frc::DARE(A, B, Q, R2), std::invalid_argument);
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R2)), std::invalid_argument);
}
TEST(DARETest, RNotSymmetricPositiveDefinite_ABQRN) {
@@ -285,10 +285,10 @@ TEST(DARETest, RNotSymmetricPositiveDefinite_ABQRN) {
const Eigen::MatrixXd N{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd R1{Eigen::Matrix2d::Zero()};
EXPECT_THROW(frc::DARE(A, B, Q, R1, N), std::invalid_argument);
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R1, N)), std::invalid_argument);
const Eigen::MatrixXd R2{-Eigen::Matrix2d::Identity()};
EXPECT_THROW(frc::DARE(A, B, Q, R2, N), std::invalid_argument);
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R2, N)), std::invalid_argument);
}
TEST(DARETest, ABNotStabilizable_ABQR) {
@@ -297,7 +297,7 @@ TEST(DARETest, ABNotStabilizable_ABQR) {
const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()};
EXPECT_THROW(frc::DARE(A, B, Q, R), std::invalid_argument);
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument);
}
TEST(DARETest, ABNotStabilizable_ABQRN) {
@@ -307,7 +307,7 @@ TEST(DARETest, ABNotStabilizable_ABQRN) {
const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd N{Eigen::Matrix2d::Identity()};
EXPECT_THROW(frc::DARE(A, B, Q, R, N), std::invalid_argument);
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
}
TEST(DARETest, ACNotDetectable_ABQR) {
@@ -316,7 +316,7 @@ TEST(DARETest, ACNotDetectable_ABQR) {
const Eigen::MatrixXd Q{Eigen::Matrix2d::Zero()};
const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()};
EXPECT_THROW(frc::DARE(A, B, Q, R), std::invalid_argument);
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument);
}
TEST(DARETest, ACNotDetectable_ABQRN) {
@@ -326,5 +326,5 @@ TEST(DARETest, ACNotDetectable_ABQRN) {
const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd N{Eigen::Matrix2d::Zero()};
EXPECT_THROW(frc::DARE(A, B, Q, R, N), std::invalid_argument);
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
}