Eigen-Contrib  5.0.1-dev+master
 
Loading...
Searching...
No Matches
DiscreteAlgebraicRiccatiSolver.h
1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// This Source Code Form is subject to the terms of the Mozilla
5// Public License v. 2.0. If a copy of the MPL was not distributed
6// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
7// SPDX-FileCopyrightText: The Eigen Authors
8// SPDX-FileCopyrightText: 2001-2002 Enthought, Inc.
9// SPDX-FileCopyrightText: 2003 SciPy Developers
10// SPDX-FileCopyrightText: 1992-2025 The University of Tennessee and The University of Tennessee Research Foundation
11// SPDX-FileCopyrightText: 2000-2025 The University of California Berkeley
12// SPDX-FileCopyrightText: 2006-2025 The University of Colorado Denver
13// SPDX-License-Identifier: MPL-2.0 AND BSD-3-Clause AND BSD-3-Clause-Open-MPI
14//
15// This file incorporates work covered by the following copyright and
16// permission notices:
17//
18// DiscreteAlgebraicRiccatiSolver is adapted from scipy.linalg.solve_discrete_are
19// (scipy/linalg/_solvers.py, SciPy 1.18.0):
20//
21// Copyright (c) 2001-2002 Enthought, Inc. 2003, SciPy Developers.
22// All rights reserved.
23//
24// Redistribution and use in source and binary forms, with or without
25// modification, are permitted provided that the following conditions
26// are met:
27//
28// 1. Redistributions of source code must retain the above copyright
29// notice, this list of conditions and the following disclaimer.
30//
31// 2. Redistributions in binary form must reproduce the above
32// copyright notice, this list of conditions and the following
33// disclaimer in the documentation and/or other materials provided
34// with the distribution.
35//
36// 3. Neither the name of the copyright holder nor the names of its
37// contributors may be used to endorse or promote products derived
38// from this software without specific prior written permission.
39//
40// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
41// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
42// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
43// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
44// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
45// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
46// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
47// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
48// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
49// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
50// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
51//
52// internal::riccati_balance_exponents() is adapted from LAPACK's xGEBAL
53// (https://github.com/Reference-LAPACK/lapack):
54//
55// Copyright (c) 1992-2025 The University of Tennessee and The University
56// of Tennessee Research Foundation. All rights
57// reserved.
58// Copyright (c) 2000-2025 The University of California Berkeley. All
59// rights reserved.
60// Copyright (c) 2006-2025 The University of Colorado Denver. All rights
61// reserved.
62//
63// Redistribution and use in source and binary forms, with or without
64// modification, are permitted provided that the following conditions are
65// met:
66//
67// - Redistributions of source code must retain the above copyright
68// notice, this list of conditions and the following disclaimer.
69//
70// - Redistributions in binary form must reproduce the above copyright
71// notice, this list of conditions and the following disclaimer listed
72// in this license in the documentation and/or other materials
73// provided with the distribution.
74//
75// - Neither the name of the copyright holders nor the names of its
76// contributors may be used to endorse or promote products derived from
77// this software without specific prior written permission.
78//
79// The copyright holders provide no reassurances that the source code
80// provided does not infringe any patent, copyright, or any other
81// intellectual property rights of third parties. The copyright holders
82// disclaim any liability to any recipient for claims brought against
83// recipient by any third party for infringement of that parties
84// intellectual property rights.
85//
86// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
87// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
88// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
89// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
90// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
91// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
92// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
93// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
94// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
95// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
96// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
97
98#ifndef EIGEN_DISCRETE_ALGEBRAIC_RICCATI_SOLVER_H
99#define EIGEN_DISCRETE_ALGEBRAIC_RICCATI_SOLVER_H
100
101// IWYU pragma: private
102#include "./InternalHeaderCheck.h"
103
104namespace Eigen {
105
106namespace internal {
107
109template <typename MatrixType, bool IsComplex = NumTraits<typename MatrixType::Scalar>::IsComplex>
110struct riccati_qz {
111 using type = RealQZ<MatrixType>;
112};
113template <typename MatrixType>
114struct riccati_qz<MatrixType, true> {
115 using type = ComplexQZ<MatrixType>;
116};
117
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();
123}
124
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;
141
142 exponents.setZero(n);
143 Matrix<RealScalar, Dynamic, 1> scale = Matrix<RealScalar, Dynamic, 1>::Ones(n);
144 bool noconv = true;
145 while (noconv) {
146 noconv = false;
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;
151
152 RealScalar ca = M.col(i).maxCoeff();
153 RealScalar ra = M.row(i).maxCoeff();
154
155 RealScalar g = r / radix;
156 RealScalar f(1);
157 const RealScalar s = c + r;
158 int e = 0;
159 while (c < g && numext::maxi(f, numext::maxi(c, ca)) < sfmax2 && numext::mini(r, numext::mini(g, ra)) > sfmin2) {
160 f *= radix;
161 c *= radix;
162 ca *= radix;
163 r /= radix;
164 g /= radix;
165 ra /= radix;
166 ++e;
167 }
168 g = c / radix;
169 while (g >= r && numext::maxi(r, ra) < sfmax2 && numext::mini(numext::mini(f, c), numext::mini(g, ca)) > sfmin2) {
170 f /= radix;
171 c /= radix;
172 g /= radix;
173 ca /= radix;
174 r *= radix;
175 ra *= radix;
176 --e;
177 }
178
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;
182 scale(i) *= f;
183 exponents(i) += e;
184 noconv = true;
185 M.row(i) /= f;
186 M.col(i) *= f;
187 }
188 }
189}
190
192inline int riccati_half_round_even(int d) {
193 const int q = (d >= 0) ? d / 2 : -((-d + 1) / 2); // floor(d / 2)
194 if (d % 2 == 0) return q;
195 return (q % 2 == 0) ? q : q + 1;
196}
197
198} // namespace internal
199
260template <typename MatrixType_>
262 public:
263 using MatrixType = MatrixType_;
264 using Scalar = typename MatrixType::Scalar;
265 using RealScalar = typename NumTraits<Scalar>::Real;
266
269
275 template <typename InputTypeA, typename InputTypeB, typename InputTypeQ, typename InputTypeR>
277 const EigenBase<InputTypeQ>& Q, const EigenBase<InputTypeR>& R, bool balance = true) {
278 compute(A, B, Q, R, balance);
279 }
280
287 template <typename InputTypeA, typename InputTypeB, typename InputTypeQ, typename InputTypeR, typename InputTypeE,
288 typename InputTypeS>
291 const EigenBase<InputTypeE>& E, const EigenBase<InputTypeS>& S, bool balance = true) {
292 compute(A, B, Q, R, E, S, balance);
293 }
294
308 template <typename InputTypeA, typename InputTypeB, typename InputTypeQ, typename InputTypeR>
311 bool balance = true) {
312 return computeImpl(A.derived(), B.derived(), Q.derived(), R.derived(), nullptr, nullptr, balance);
313 }
314
331 template <typename InputTypeA, typename InputTypeB, typename InputTypeQ, typename InputTypeR, typename InputTypeE,
332 typename InputTypeS>
336 bool balance = true) {
337 const DynamicMatrix e = E.derived();
338 const DynamicMatrix s = S.derived();
339 return computeImpl(A.derived(), B.derived(), Q.derived(), R.derived(), &e, &s, balance);
340 }
341
346 const MatrixType& solution() const {
347 eigen_assert(m_isInitialized && "DiscreteAlgebraicRiccatiSolver is not initialized.");
348 eigen_assert(m_info == Success && "The Riccati equation could not be solved.");
349 return m_X;
350 }
351
363 eigen_assert(m_isInitialized && "DiscreteAlgebraicRiccatiSolver is not initialized.");
364 return m_info;
365 }
366
367 protected:
368 using DynamicMatrix = Matrix<Scalar, Dynamic, Dynamic>;
369 using RealDynamicMatrix = Matrix<RealScalar, Dynamic, Dynamic>;
370 using RealVector = Matrix<RealScalar, Dynamic, 1>;
371
372 static bool isSelfAdjoint(const DynamicMatrix& M) {
373 return internal::riccati_norm1(M - M.adjoint()) <=
374 RealScalar(100) * NumTraits<RealScalar>::epsilon() * internal::riccati_norm1(M);
375 }
376
377 DiscreteAlgebraicRiccatiSolver& computeImpl(const DynamicMatrix& A, const DynamicMatrix& B, const DynamicMatrix& Q,
378 const DynamicMatrix& R, const DynamicMatrix* E, const DynamicMatrix* S,
379 bool balance);
380
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);
384
385 // Backward error ratios of a solution on the original and on the balanced equation, see backwardErrorRatios().
386 struct BackwardErrorRatios {
387 RealScalar original;
388 RealScalar balanced;
389 };
390
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);
394
395 MatrixType m_X;
397 bool m_isInitialized = false;
398};
399
400template <typename MatrixType_>
401DiscreteAlgebraicRiccatiSolver<MatrixType_>& DiscreteAlgebraicRiccatiSolver<MatrixType_>::computeImpl(
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.");
412
413 m_isInitialized = true;
414 m_info = InvalidInput;
415
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;
419 if (m == 0) {
420 m_X.resize(0, 0);
421 m_info = Success;
422 return *this;
423 }
424 // rcond() bounds 1 / kappa_1(E) from above and kappa_1 <= m kappa_2, so this rejects only kappa_2(E) > 1 / eps.
425 const RealScalar eps = NumTraits<RealScalar>::epsilon();
426 if (E != nullptr && !(RealScalar(m) * PartialPivLU<DynamicMatrix>(*E).rcond() >= eps)) return *this;
427 // U1 is held to the stricter rcond(U1) >= eps: the test above guarantees no false rejection but can accept
428 // kappa_2 > 1 / eps, and for U1 it accepts the non-stabilizable problem of dare_no_stabilizing_solution() with a
429 // meaningless ||X|| ~ 1e16.
430 const RealScalar rcondThreshold = eps;
431
432 DynamicMatrix X;
433 RealVector balancing;
434 m_info = solvePencil(A, B, Q, R, E, S, balance, rcondThreshold, X, balancing);
435 if (balancing.size() > 0) {
436 // Balancing is essential for some badly scaled problems (e.g. DAREX 13 and 14), but costs accuracy for others,
437 // notably when B is small. The unbalanced pencil is solved as well if the balanced solve fails or looks
438 // inaccurate by either ratio of backwardErrorRatios(): above 10 on the original equation (balancing lost
439 // accuracy) or above 100 on the balanced equation (a poor solution that the kappa factor of the first ratio
440 // hides, e.g. 1e3 for the unbalanced solutions of DAREX 14, against 0.2 to 3 for good ones). Of the two
441 // solutions, the one with the smaller ratio on the original equation is kept. Against a long double reference,
442 // this kept the more accurate solution in 40000 random problems like those of dare_random() whenever the
443 // difference exceeded a factor 2, and triggered the second solve only for problems with B scaled by 10^-1 or
444 // less. If balancing left the pencil unchanged, there is nothing to fall back to.
445 const RealScalar inf = NumTraits<RealScalar>::infinity();
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))) {
449 DynamicMatrix Xu;
450 RealVector none;
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)) {
453 X.swap(Xu);
454 m_info = Success;
455 }
456 }
457 }
458 if (m_info == Success) m_X = X;
459 return *this;
460}
461
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) {
485 const RealScalar eps = NumTraits<RealScalar>::epsilon();
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());
491 if (B.cols() > 0) {
492 DynamicMatrix AXBS = XA.adjoint() * B;
493 if (S != nullptr) AXBS += *S;
494 G = AXBS * (R + B.adjoint() * X * B).partialPivLu().solve(AXBS.adjoint());
495 }
496 DynamicMatrix res = AXA - EXE - G + Q;
497 if (numext::is_exactly_zero(res.norm())) return BackwardErrorRatios{RealScalar(0), RealScalar(0)};
498
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();
507 }
508 ratios.balanced = finite(res.norm() / (eps * (AXA.norm() + EXE.norm() + Qb.norm() + G.norm())));
509 return ratios;
510}
511
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();
524
525 // Form the extended symplectic pencil H - lambda J.
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;
532 if (E != nullptr)
533 H.block(m, m, m, m) = E->adjoint();
534 else
535 H.block(m, m, m, m).setIdentity();
536 if (S != nullptr) {
537 H.block(m, 2 * m, m, n) = -*S;
538 H.bottomLeftCorner(n, m) = S->adjoint();
539 }
540 H.bottomRightCorner(n, n) = R;
541
542 if (E != nullptr)
543 J.topLeftCorner(m, m) = *E;
544 else
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();
548
549 // Balance the pencil without destroying its symplectic structure: the scaling is diag(D, D^-1, D_R),
550 // with D derived from the xGEBAL scaling of |H| + |J| with zeroed diagonal, see [3].
551 RealVector scaling;
552 bool balanced = false;
553 balancing.resize(0);
554 if (balance) {
555 RealDynamicMatrix M = H.cwiseAbs() + J.cwiseAbs();
556 M.diagonal().setZero();
557 VectorXi exponents;
558 internal::riccati_balance_exponents(M, exponents);
559 if (!(exponents.array() == 0).all()) {
560 balanced = true;
561 scaling.resize(p);
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);
566 }
567 // As in SciPy, the R block is scaled by D_R = diag(2^e), the inverse of the xGEBAL scaling D^-1 M D. The
568 // xGEBAL-consistent 2^-e loses accuracy: the DAREX 13 and 14 residuals grow from 7.7e-7 and 1.3e-7 to 1.6e-5
569 // and 1.8e-6.
570 for (Index i = 2 * m; i < p; ++i) scaling(i) = numext::ldexp(RealScalar(1), exponents(i));
571 // H <- D H D^-1, J <- D J D^-1 with D = diag(scaling), exact in floating point.
572 H = scaling.asDiagonal() * H * scaling.cwiseInverse().asDiagonal();
573 J = scaling.asDiagonal() * J * scaling.cwiseInverse().asDiagonal();
574 // Also reported when the solve fails, so that computeImpl() falls back to the unbalanced pencil.
575 balancing = scaling.head(m);
576 }
577 }
578
579 // Deflate the pencil by the columns belonging to R, see [1].
580 DynamicMatrix Hd, Jd;
581 if (n > 0) {
582 const HouseholderQR<DynamicMatrix> qr(H.rightCols(n));
583 Hd = (qr.householderQ().adjoint() * H.leftCols(2 * m)).bottomRows(2 * m);
584 Jd = (qr.householderQ().adjoint() * J.leftCols(2 * m)).bottomRows(2 * m);
585 } else {
586 Hd = H;
587 Jd = J;
588 }
589
590 // Generalized Schur form with the eigenvalues inside the unit circle first.
591 typename internal::riccati_qz<DynamicMatrix>::type qz(Hd, Jd, true);
592 if (qz.info() == NoConvergence) return NoConvergence;
593 DynamicMatrix SS = qz.matrixS();
594 DynamicMatrix TT = qz.matrixT();
595 DynamicMatrix ZZ = qz.matrixZ().adjoint();
596 if (qz.info() != Success) {
597 const DynamicMatrix& QQ = qz.matrixQ();
598 // ComplexQZ reports NumericalIssue when its direct reduction of a 2x2 block leaves a subdiagonal entry of S above
599 // eps ||S||, although the reduction is only backward stable to a few eps ||S||. Accept the decomposition if
600 // ||Q (S, T) Z^* - (Hd, Jd)||_F <= 10 (2m) eps ||(Hd, Jd)||_F once that entry is dropped.
601 internal::clean_generalized_schur_form(SS, TT);
602 const RealScalar bound = RealScalar(10 * 2 * m) * NumTraits<RealScalar>::epsilon();
603 if (!((QQ * SS * ZZ.adjoint() - Hd).norm() <= bound * Hd.norm() &&
604 (QQ * TT * ZZ.adjoint() - Jd).norm() <= bound * Jd.norm()))
605 return NumericalIssue;
606 }
607 // The eigenvalues of the symplectic pencil come in pairs (lambda, 1 / conj(lambda)), so exactly m of them lie
608 // inside the unit circle if a stabilizing solution exists. (SciPy does not check this.)
609 if (internal::reorder_generalized_schur_inside_unit_circle<DynamicMatrix>(SS, TT, nullptr, ZZ) != m)
610 return NumericalIssue;
611
612 // The leading m right Schur vectors span the stable deflating subspace.
613 DynamicMatrix U = ZZ.leftCols(m);
614 if (E != nullptr) {
615 // U belongs to the balanced pencil, so it is combined with the balanced E held in J. (SciPy combines it with the
616 // original E, which yields wrong solutions or spurious failures whenever balancing scales the pencil.)
617 U.topRows(m) = (J.topLeftCorner(m, m) * U.topRows(m)).eval();
618 U = HouseholderQR<DynamicMatrix>(U).householderQ() * DynamicMatrix::Identity(2 * m, m);
619 }
620 const auto U1 = U.topRows(m);
621 const auto U2 = U.bottomRows(m);
622
623 // [U1; U2] has orthonormal columns, so ||X||_2 = sqrt(1 - sigma_min(U1)^2) / sigma_min(U1). rcond(U1) is scale
624 // invariant and misses a uniformly small U1, which an unstable mode that B does not reach produces from rounding
625 // errors. ||U1||_1 rcond(U1) estimates 1 / ||U1^-1||_1 <= sqrt(m) sigma_min(U1), so the second test bounds the
626 // solution of the (balanced) pencil by about sqrt(m) / (5 rcondThreshold). Stabilizing solutions beyond that are
627 // numerically indistinguishable from the absence of one. The factor 5 rejected all of 14400 random problems with
628 // unstable modes that B does not reach; with 1, 10% of them returned a meaningless X.
629 const PartialPivLU<DynamicMatrix> lu(U1);
630 const RealScalar rcondU1 = lu.rcond();
631 if (!(rcondU1 >= rcondThreshold && internal::riccati_norm1(U1) * rcondU1 >= RealScalar(5) * rcondThreshold))
632 return NumericalIssue;
633
634 // A large deviation of U1^* U2 from selfadjointness indicates failure, see Theorem 5 in [2].
635 const DynamicMatrix uSym = U1.adjoint() * U2;
636 // SciPy's threshold max(spacing(1000), 0.1 ||U1^* U2||_1), with spacing(1000) = 512 eps.
637 const RealScalar symThreshold =
638 numext::maxi(RealScalar(512) * NumTraits<RealScalar>::epsilon(), RealScalar(0.1) * internal::riccati_norm1(uSym));
639 if (!(internal::riccati_norm1(uSym - uSym.adjoint()) <= symThreshold)) return NumericalIssue;
640
641 // X = U2 U1^-1 is computed as (U1^-* U2^*)^*.
642 X = lu.adjoint().solve(U2.adjoint());
643 X.adjointInPlace();
644 if (balanced) X = balancing.asDiagonal() * X * balancing.asDiagonal();
645 X = (X + X.adjoint()).eval() / RealScalar(2);
646 // The solve, the unscaling and the symmetrization can overflow.
647 if (!X.allFinite()) return NumericalIssue;
648 return Success;
649}
650
651} // namespace Eigen
652
653#endif // EIGEN_DISCRETE_ALGEBRAIC_RICCATI_SOLVER_H
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
RealScalar rcond() const
ComputationInfo
NumericalIssue
Matrix< int, Dynamic, 1 > VectorXi
Namespace containing all symbols from the Eigen library.
constexpr Derived & derived()