forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
82 lines
2.9 KiB
82 lines
2.9 KiB
#pragma once
|
|
|
|
#include <Eigen/Dense>
|
|
#include <Eigen/QR>
|
|
|
|
#include "drake/common/eigen_types.h"
|
|
|
|
namespace drake {
|
|
namespace math {
|
|
|
|
// TODO(FischerGundlach) Reserve memory and pass it to recursive function
|
|
// calls.
|
|
|
|
/**
|
|
* @param A A user defined real square matrix.
|
|
* @param Q A user defined real symmetric matrix.
|
|
*
|
|
* @pre Q is a symmetric matrix.
|
|
*
|
|
* Computes the unique solution X to the discrete Lyapunov equation: `AᵀXA - X +
|
|
* Q = 0`, where A is real and square, and Q is real, symmetric and of equal
|
|
* size as A.
|
|
* @throws std::exception if A or Q are not square matrices or do not
|
|
* have the same size.
|
|
*
|
|
* Limitations: Given the Eigenvalues of A as λ₁, ..., λₙ, there exists
|
|
* a unique solution if and only if λᵢ * λⱼ ≠ 1 ∀ i,j and λᵢ ≠ ±1, ∀ i [1].
|
|
* @throws std::exception if the solution is not unique.[3]
|
|
*
|
|
* There are no further limitations on the eigenvalues of A.
|
|
* Further, if |λᵢ|<1, ∀ i, and if Q is
|
|
* positive semi-definite, then X is also positive semi-definite [2].
|
|
* Therefore, if one searches for a Lyapunov function V(z) = zᵀXz for the stable
|
|
* linear system zₙ₊₁ = Azₙ, then the solution of the Lyapunov Equation `AᵀXA -
|
|
* X + Q = 0` only returns a valid Lyapunov function if Q is positive
|
|
* semi-definite.
|
|
*
|
|
* The implementation is based on SLICOT routine SB03MD [2]. Note the
|
|
* transformation Q = -C. The complexity of this routine is O(n³).
|
|
* If A is larger than 2-by-2, then a Schur factorization is performed.
|
|
* @throws std::exception if Schur factorization fails.
|
|
*
|
|
* A tolerance of ε is used to check if a double variable is equal to zero,
|
|
* where the default value for ε is 1e-10. It has been used to check (1) if λᵢ =
|
|
* ±1 ∀ i; (2) if λᵢ * λⱼ = 1, i ≠ j.
|
|
*
|
|
* [1] Barraud, A.Y., "A numerical algorithm to solve AᵀXA - X = Q," IEEE®
|
|
* Trans. Auto. Contr., AC-22, pp. 883-885, 1977.
|
|
*
|
|
* [2] http://slicot.org/objects/software/shared/doc/SB03MD.html
|
|
*
|
|
* [3] https://www.mathworks.com/help/control/ref/dlyap.html
|
|
*
|
|
*/
|
|
|
|
Eigen::MatrixXd RealDiscreteLyapunovEquation(
|
|
const Eigen::Ref<const Eigen::MatrixXd>& A,
|
|
const Eigen::Ref<const Eigen::MatrixXd>& Q);
|
|
|
|
namespace internal {
|
|
|
|
// Subroutines which help special cases. These cases are also called within
|
|
// SolveReducedRealContinuousLyapunovFunction.
|
|
|
|
Vector1d Solve1By1RealDiscreteLyapunovEquation(
|
|
const Eigen::Ref<const Vector1d>& A, const Eigen::Ref<const Vector1d>& Q);
|
|
|
|
Eigen::Matrix2d Solve2By2RealDiscreteLyapunovEquation(
|
|
const Eigen::Ref<const Eigen::Matrix2d>& A,
|
|
const Eigen::Ref<const Eigen::Matrix2d>& Q);
|
|
|
|
// If the problem is larger than in size 2-by-2, than it is reduced into a form
|
|
// which can be recursively solved by smaller problems.
|
|
|
|
Eigen::MatrixXd SolveReducedRealDiscreteLyapunovEquation(
|
|
const Eigen::Ref<const Eigen::MatrixXd>& A,
|
|
const Eigen::Ref<const Eigen::MatrixXd>& Q);
|
|
|
|
} // namespace internal
|
|
} // namespace math
|
|
} // namespace drake
|