9#include <Eigen/Cholesky>
42 return "Q was not symmetric.";
44 return "Q was not positive semidefinite.";
46 return "R was not symmetric.";
48 return "R was not positive definite.";
50 return "(A, B) pair was not stabilizable.";
52 return "(A, C) pair where Q = CᵀC was not detectable.";
84template <
int States,
int Inputs>
85Eigen::Matrix<double, States, States>
DARE(
86 const Eigen::Matrix<double, States, States>& A,
87 const Eigen::Matrix<double, States, Inputs>& B,
88 const Eigen::Matrix<double, States, States>& Q,
89 const Eigen::LLT<Eigen::Matrix<double, Inputs, Inputs>>& R_llt) {
90 using StateMatrix = Eigen::Matrix<double, States, States>;
103 StateMatrix G_k = B * R_llt.solve(B.transpose());
105 StateMatrix H_k1 = Q;
111 StateMatrix W = StateMatrix::Identity(H_k.rows(), H_k.cols()) + G_k * H_k;
113 auto W_solver = W.lu();
116 StateMatrix V_1 = W_solver.solve(A_k);
131 StateMatrix V_2 = W_solver.solve(G_k.transpose()).transpose();
136 G_k += A_k * V_2 * A_k.transpose();
137 H_k1 = H_k + V_1.transpose() * H_k * A_k;
141 }
while ((H_k1 - H_k).norm() > 1e-10 * H_k1.norm());
164template <
int States,
int Inputs>
166 const Eigen::Matrix<double, States, States>& A,
167 const Eigen::Matrix<double, States, Inputs>& B,
168 const Eigen::Matrix<double, States, States>& Q,
169 const Eigen::Matrix<double, Inputs, Inputs>& R,
170 bool checkPreconditions =
true) {
171 if (checkPreconditions) {
173 if ((R - R.transpose()).norm() > 1e-10) {
179 auto R_llt = R.llt();
180 if (R_llt.info() != Eigen::Success) {
184 if (checkPreconditions) {
186 if ((Q - Q.transpose()).norm() > 1e-10) {
199 auto Q_ldlt = Q.ldlt();
200 if (Q_ldlt.info() != Eigen::Success ||
201 (Q_ldlt.vectorD().array() < 0.0).any()) {
214 Eigen::Matrix<double, States, States> C =
215 Q_ldlt.vectorD().cwiseSqrt().asDiagonal() *
216 Eigen::Matrix<double, States, States>{Q_ldlt.matrixL().transpose()} *
217 Q_ldlt.transpositionsP();
279template <
int States,
int Inputs>
281 const Eigen::Matrix<double, States, States>& A,
282 const Eigen::Matrix<double, States, Inputs>& B,
283 const Eigen::Matrix<double, States, States>& Q,
284 const Eigen::Matrix<double, Inputs, Inputs>& R,
285 const Eigen::Matrix<double, States, Inputs>& N,
286 bool checkPreconditions =
true) {
287 if (checkPreconditions) {
289 if ((R - R.transpose()).norm() > 1e-10) {
295 auto R_llt = R.llt();
296 if (R_llt.info() != Eigen::Success) {
311 Eigen::Matrix<double, States, States> A_2 =
312 A - B * R_llt.solve(N.transpose());
313 Eigen::Matrix<double, States, States> Q_2 =
314 Q - N * R_llt.solve(N.transpose());
316 if (checkPreconditions) {
318 if ((Q_2 - Q_2.transpose()).norm() > 1e-10) {
331 auto Q_ldlt = Q_2.ldlt();
332 if (Q_ldlt.info() != Eigen::Success ||
333 (Q_ldlt.vectorD().array() < 0.0).any()) {
346 Eigen::Matrix<double, States, States> C =
347 Q_ldlt.vectorD().cwiseSqrt().asDiagonal() *
348 Eigen::Matrix<double, States, States>{Q_ldlt.matrixL().transpose()} *
349 Q_ldlt.transpositionsP();
An expected<T, E> object is an object that contains the storage for another object and manages the li...
Definition expected:1250
detail namespace with internal helper functions
Definition input_adapters.h:32
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)
Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation:
Definition DARE.h:85
bool IsStabilizable(const Matrixd< States, States > &A, const Matrixd< States, Inputs > &B)
Returns true if (A, B) is a stabilizable pair.
Definition StateSpaceUtil.h:250
wpi::expected< Eigen::Matrix< double, States, States >, DAREError > 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, bool checkPreconditions=true)
Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation:
Definition DARE.h:165
constexpr std::string_view to_string(const DAREError &error)
Converts the given DAREError enum to a string.
Definition DARE.h:39
DAREError
Errors the DARE solver can encounter.
Definition DARE.h:21
@ QNotPositiveSemidefinite
Q was not positive semidefinite.
@ RNotSymmetric
R was not symmetric.
@ QNotSymmetric
Q was not symmetric.
@ ACNotDetectable
(A, C) pair where Q = CᵀC was not detectable.
@ RNotPositiveDefinite
R was not positive definite.
@ ABNotStabilizable
(A, B) pair was not stabilizable.
bool IsDetectable(const Matrixd< States, States > &A, const Matrixd< Outputs, States > &C)
Returns true if (A, C) is a detectable pair.
Definition StateSpaceUtil.h:309