98#ifndef EIGEN_DISCRETE_ALGEBRAIC_RICCATI_SOLVER_H
99#define EIGEN_DISCRETE_ALGEBRAIC_RICCATI_SOLVER_H
102#include "./InternalHeaderCheck.h"
109template <typename MatrixType, bool IsComplex = NumTraits<typename MatrixType::Scalar>::IsComplex>
111 using type = RealQZ<MatrixType>;
113template <
typename MatrixType>
114struct riccati_qz<MatrixType, true> {
115 using type = ComplexQZ<MatrixType>;
119template <
typename Derived>
120typename NumTraits<typename Derived::Scalar>::Real riccati_norm1(
const MatrixBase<Derived>& m) {
121 return m.size() == 0 ?
typename NumTraits<typename Derived::Scalar>::Real(0)
122 : m.cwiseAbs().colwise().sum().maxCoeff();
131template <
typename RealMatrix>
132void riccati_balance_exponents(RealMatrix& M,
VectorXi& exponents) {
133 using RealScalar =
typename RealMatrix::Scalar;
134 const Index n = M.rows();
135 const RealScalar radix(2);
136 const RealScalar factor(0.95);
137 const RealScalar sfmin1 = (std::numeric_limits<RealScalar>::min)() / NumTraits<RealScalar>::epsilon();
138 const RealScalar sfmax1 = RealScalar(1) / sfmin1;
139 const RealScalar sfmin2 = sfmin1 * radix;
140 const RealScalar sfmax2 = RealScalar(1) / sfmin2;
142 exponents.setZero(n);
143 Matrix<RealScalar, Dynamic, 1> scale = Matrix<RealScalar, Dynamic, 1>::Ones(n);
147 for (Index i = 0; i < n; ++i) {
148 RealScalar c = M.col(i).stableNorm();
149 RealScalar r = M.row(i).stableNorm();
150 if (numext::is_exactly_zero(c) || numext::is_exactly_zero(r))
continue;
152 RealScalar ca = M.col(i).maxCoeff();
153 RealScalar ra = M.row(i).maxCoeff();
155 RealScalar g = r / radix;
157 const RealScalar s = c + r;
159 while (c < g && numext::maxi(f, numext::maxi(c, ca)) < sfmax2 && numext::mini(r, numext::mini(g, ra)) > sfmin2) {
169 while (g >= r && numext::maxi(r, ra) < sfmax2 && numext::mini(numext::mini(f, c), numext::mini(g, ca)) > sfmin2) {
179 if (c + r >= factor * s)
continue;
180 if (f < RealScalar(1) && scale(i) < RealScalar(1) && f * scale(i) <= sfmin1)
continue;
181 if (f > RealScalar(1) && scale(i) > RealScalar(1) && scale(i) >= sfmax1 / f)
continue;
192inline int riccati_half_round_even(
int d) {
193 const int q = (d >= 0) ? d / 2 : -((-d + 1) / 2);
194 if (d % 2 == 0)
return q;
195 return (q % 2 == 0) ? q : q + 1;
260template <
typename MatrixType_>
263 using MatrixType = MatrixType_;
264 using Scalar =
typename MatrixType::Scalar;
275 template <
typename InputTypeA,
typename InputTypeB,
typename InputTypeQ,
typename InputTypeR>
287 template <
typename InputTypeA,
typename InputTypeB,
typename InputTypeQ,
typename InputTypeR,
typename InputTypeE,
292 compute(A, B, Q, R, E, S, balance);
308 template <
typename InputTypeA,
typename InputTypeB,
typename InputTypeQ,
typename InputTypeR>
311 bool balance =
true) {
331 template <
typename InputTypeA,
typename InputTypeB,
typename InputTypeQ,
typename InputTypeR,
typename InputTypeE,
336 bool balance =
true) {
337 const DynamicMatrix e = E.
derived();
338 const DynamicMatrix s = S.
derived();
347 eigen_assert(m_isInitialized &&
"DiscreteAlgebraicRiccatiSolver is not initialized.");
348 eigen_assert(m_info ==
Success &&
"The Riccati equation could not be solved.");
363 eigen_assert(m_isInitialized &&
"DiscreteAlgebraicRiccatiSolver is not initialized.");
372 static bool isSelfAdjoint(
const DynamicMatrix& M) {
373 return internal::riccati_norm1(M - M.adjoint()) <=
378 const DynamicMatrix& R,
const DynamicMatrix* E,
const DynamicMatrix* S,
381 static ComputationInfo solvePencil(
const DynamicMatrix& A,
const DynamicMatrix& B,
const DynamicMatrix& Q,
382 const DynamicMatrix& R,
const DynamicMatrix* E,
const DynamicMatrix* S,
383 bool balance, RealScalar rcondThreshold, DynamicMatrix& X, RealVector& balancing);
386 struct BackwardErrorRatios {
391 static BackwardErrorRatios backwardErrorRatios(
const DynamicMatrix& A,
const DynamicMatrix& B,
const DynamicMatrix& Q,
392 const DynamicMatrix& R,
const DynamicMatrix* E,
const DynamicMatrix* S,
393 const DynamicMatrix& X,
const RealVector& balancing);
397 bool m_isInitialized =
false;
400template <
typename MatrixType_>
402 const DynamicMatrix& A,
const DynamicMatrix& B,
const DynamicMatrix& Q,
const DynamicMatrix& R,
403 const DynamicMatrix* E,
const DynamicMatrix* S,
bool balance) {
404 const Index m = B.rows();
405 const Index n = B.cols();
406 EIGEN_ONLY_USED_FOR_DEBUG(n);
407 eigen_assert(A.rows() == m && A.cols() == m &&
"A must be square with as many rows as B.");
408 eigen_assert(Q.rows() == m && Q.cols() == m &&
"Q must have the same size as A.");
409 eigen_assert(R.rows() == n && R.cols() == n &&
"R must be square with as many columns as B.");
410 eigen_assert((E ==
nullptr || (E->rows() == m && E->cols() == m)) &&
"E must have the same size as A.");
411 eigen_assert((S ==
nullptr || (S->rows() == m && S->cols() == n)) &&
"S must have the same size as B.");
413 m_isInitialized =
true;
416 if (!A.allFinite() || !B.allFinite() || !Q.allFinite() || !R.allFinite())
return *
this;
417 if ((E !=
nullptr && !E->allFinite()) || (S !=
nullptr && !S->allFinite()))
return *
this;
418 if (!isSelfAdjoint(Q) || !isSelfAdjoint(R))
return *
this;
430 const RealScalar rcondThreshold = eps;
433 RealVector balancing;
434 m_info = solvePencil(A, B, Q, R, E, S, balance, rcondThreshold, X, balancing);
435 if (balancing.size() > 0) {
446 const BackwardErrorRatios ratios =
447 m_info ==
Success ? backwardErrorRatios(A, B, Q, R, E, S, X, balancing) : BackwardErrorRatios{inf, inf};
448 if (!(ratios.original <= RealScalar(10) && ratios.balanced <= RealScalar(100))) {
451 if (solvePencil(A, B, Q, R, E, S,
false, rcondThreshold, Xu, none) ==
Success &&
452 (m_info !=
Success || backwardErrorRatios(A, B, Q, R, E, S, Xu, balancing).original < ratios.original)) {
458 if (m_info ==
Success) m_X = X;
479template <
typename MatrixType_>
480typename DiscreteAlgebraicRiccatiSolver<MatrixType_>::BackwardErrorRatios
481DiscreteAlgebraicRiccatiSolver<MatrixType_>::backwardErrorRatios(
const DynamicMatrix& A,
const DynamicMatrix& B,
482 const DynamicMatrix& Q,
const DynamicMatrix& R,
483 const DynamicMatrix* E,
const DynamicMatrix* S,
484 const DynamicMatrix& X,
const RealVector& balancing) {
486 const auto finite = [](RealScalar r) {
return (numext::isfinite)(r) ? r :
NumTraits<RealScalar>::infinity(); };
487 const DynamicMatrix XA = X * A;
488 DynamicMatrix AXA = A.adjoint() * XA;
489 DynamicMatrix EXE = (E !=
nullptr) ? DynamicMatrix(E->adjoint() * X * *E) : X;
490 DynamicMatrix G = DynamicMatrix::Zero(A.rows(), A.rows());
492 DynamicMatrix AXBS = XA.adjoint() * B;
493 if (S !=
nullptr) AXBS += *S;
494 G = AXBS * (R + B.adjoint() * X * B).partialPivLu().solve(AXBS.adjoint());
496 DynamicMatrix res = AXA - EXE - G + Q;
497 if (numext::is_exactly_zero(res.norm()))
return BackwardErrorRatios{RealScalar(0), RealScalar(0)};
499 BackwardErrorRatios ratios;
500 const RealScalar normQ = Q.norm(), normEXE = EXE.norm();
501 const RealScalar kappa = (normQ > RealScalar(0) && normEXE > normQ) ? normEXE / normQ : RealScalar(1);
502 ratios.original = finite(res.norm() / (RealScalar(A.rows()) * eps * (AXA.norm() + normEXE + normQ) * kappa));
503 DynamicMatrix Qb = Q;
504 if (balancing.size() > 0) {
505 const RealVector inv = balancing.cwiseInverse();
506 for (DynamicMatrix* M : {&res, &AXA, &EXE, &G, &Qb}) *M = inv.asDiagonal() * *M * inv.asDiagonal();
508 ratios.balanced = finite(res.norm() / (eps * (AXA.norm() + EXE.norm() + Qb.norm() + G.norm())));
516template <
typename MatrixType_>
517ComputationInfo DiscreteAlgebraicRiccatiSolver<MatrixType_>::solvePencil(
const DynamicMatrix& A,
const DynamicMatrix& B,
518 const DynamicMatrix& Q,
const DynamicMatrix& R,
519 const DynamicMatrix* E,
const DynamicMatrix* S,
520 bool balance, RealScalar rcondThreshold,
521 DynamicMatrix& X, RealVector& balancing) {
522 const Index m = B.rows();
523 const Index n = B.cols();
526 const Index p = 2 * m + n;
527 DynamicMatrix H = DynamicMatrix::Zero(p, p);
528 DynamicMatrix J = DynamicMatrix::Zero(p, p);
529 H.topLeftCorner(m, m) = A;
530 H.topRightCorner(m, n) = B;
531 H.block(m, 0, m, m) = -Q;
533 H.block(m, m, m, m) = E->adjoint();
535 H.block(m, m, m, m).setIdentity();
537 H.block(m, 2 * m, m, n) = -*S;
538 H.bottomLeftCorner(n, m) = S->adjoint();
540 H.bottomRightCorner(n, n) = R;
543 J.topLeftCorner(m, m) = *E;
545 J.topLeftCorner(m, m).setIdentity();
546 J.block(m, m, m, m) = A.adjoint();
547 J.block(2 * m, m, n, m) = -B.adjoint();
552 bool balanced =
false;
555 RealDynamicMatrix M = H.cwiseAbs() + J.cwiseAbs();
556 M.diagonal().setZero();
558 internal::riccati_balance_exponents(M, exponents);
559 if (!(exponents.array() == 0).all()) {
562 for (Index i = 0; i < m; ++i) {
563 const int d = internal::riccati_half_round_even(exponents(m + i) - exponents(i));
564 scaling(i) = numext::ldexp(RealScalar(1), d);
565 scaling(m + i) = numext::ldexp(RealScalar(1), -d);
570 for (Index i = 2 * m; i < p; ++i) scaling(i) = numext::ldexp(RealScalar(1), exponents(i));
572 H = scaling.asDiagonal() * H * scaling.cwiseInverse().asDiagonal();
573 J = scaling.asDiagonal() * J * scaling.cwiseInverse().asDiagonal();
575 balancing = scaling.head(m);
580 DynamicMatrix Hd, Jd;
583 Hd = (qr.householderQ().adjoint() * H.leftCols(2 * m)).bottomRows(2 * m);
584 Jd = (qr.householderQ().adjoint() * J.leftCols(2 * m)).bottomRows(2 * m);
591 typename internal::riccati_qz<DynamicMatrix>::type qz(Hd, Jd,
true);
593 DynamicMatrix SS = qz.matrixS();
594 DynamicMatrix TT = qz.matrixT();
595 DynamicMatrix ZZ = qz.matrixZ().adjoint();
597 const DynamicMatrix& QQ = qz.matrixQ();
601 internal::clean_generalized_schur_form(SS, TT);
603 if (!((QQ * SS * ZZ.adjoint() - Hd).norm() <= bound * Hd.norm() &&
604 (QQ * TT * ZZ.adjoint() - Jd).norm() <= bound * Jd.norm()))
609 if (internal::reorder_generalized_schur_inside_unit_circle<DynamicMatrix>(SS, TT,
nullptr, ZZ) != m)
613 DynamicMatrix U = ZZ.leftCols(m);
617 U.topRows(m) = (J.topLeftCorner(m, m) * U.topRows(m)).eval();
620 const auto U1 = U.topRows(m);
621 const auto U2 = U.bottomRows(m);
630 const RealScalar rcondU1 = lu.rcond();
631 if (!(rcondU1 >= rcondThreshold && internal::riccati_norm1(U1) * rcondU1 >= RealScalar(5) * rcondThreshold))
635 const DynamicMatrix uSym = U1.adjoint() * U2;
637 const RealScalar symThreshold =
639 if (!(internal::riccati_norm1(uSym - uSym.adjoint()) <= symThreshold))
return NumericalIssue;
642 X = lu.adjoint().solve(U2.adjoint());
644 if (balanced) X = balancing.asDiagonal() * X * balancing.asDiagonal();
645 X = (X + X.adjoint()).eval() / RealScalar(2);
Solves the discrete-time algebraic Riccati equation (DARE)
Definition DiscreteAlgebraicRiccatiSolver.h:261
DiscreteAlgebraicRiccatiSolver()=default
Default constructor; the solver is initialized by calling compute().
DiscreteAlgebraicRiccatiSolver(const EigenBase< InputTypeA > &A, const EigenBase< InputTypeB > &B, const EigenBase< InputTypeQ > &Q, const EigenBase< InputTypeR > &R, const EigenBase< InputTypeE > &E, const EigenBase< InputTypeS > &S, bool balance=true)
Constructor; solves the generalized discrete-time algebraic Riccati equation.
Definition DiscreteAlgebraicRiccatiSolver.h:289
const MatrixType & solution() const
Definition DiscreteAlgebraicRiccatiSolver.h:346
DiscreteAlgebraicRiccatiSolver & compute(const EigenBase< InputTypeA > &A, const EigenBase< InputTypeB > &B, const EigenBase< InputTypeQ > &Q, const EigenBase< InputTypeR > &R, const EigenBase< InputTypeE > &E, const EigenBase< InputTypeS > &S, bool balance=true)
Solves .
Definition DiscreteAlgebraicRiccatiSolver.h:333
DiscreteAlgebraicRiccatiSolver & compute(const EigenBase< InputTypeA > &A, const EigenBase< InputTypeB > &B, const EigenBase< InputTypeQ > &Q, const EigenBase< InputTypeR > &R, bool balance=true)
Solves .
Definition DiscreteAlgebraicRiccatiSolver.h:309
ComputationInfo info() const
Reports whether the last computation was successful.
Definition DiscreteAlgebraicRiccatiSolver.h:362
DiscreteAlgebraicRiccatiSolver(const EigenBase< InputTypeA > &A, const EigenBase< InputTypeB > &B, const EigenBase< InputTypeQ > &Q, const EigenBase< InputTypeR > &R, bool balance=true)
Constructor; solves the discrete-time algebraic Riccati equation.
Definition DiscreteAlgebraicRiccatiSolver.h:276
HouseholderSequenceType householderQ() const
Matrix< int, Dynamic, 1 > VectorXi
Namespace containing all symbols from the Eigen library.
constexpr Derived & derived()