void care(MatrixBase<DerivedA> const& A, MatrixBase<DerivedB> const& B, MatrixBase<DerivedQ> const& Q, MatrixBase<DerivedR> const& R, MatrixBase<DerivedX> & X)
{
const size_t n = A.rows();
LLT<MatrixXd> R_cholesky(R);
MatrixXd H(2 * n, 2 * n);
H << A, B * R_cholesky.solve(B.transpose()), Q, -A.transpose();
MatrixXd Z = H;
MatrixXd Z_old;
//these could be options
const double tolerance = 1e-9;
const double max_iterations = 100;
double relative_norm;
size_t iteration = 0;
const double p = static_cast<double>(Z.rows());
do {
Z_old = Z;
//R. Byers. Solving the algebraic Riccati equation with the matrix sign function. Linear Algebra Appl., 85:267–279, 1987
//Added determinant scaling to improve convergence (converges in rough half the iterations with this)
double ck = pow(abs(Z.determinant()), -1.0/p);
Z *= ck;
Z = Z - 0.5 * (Z - Z.inverse());
relative_norm = (Z - Z_old).norm();
iteration ++;
} while(iteration < max_iterations && relative_norm > tolerance);
MatrixXd W11 = Z.block(0, 0, n, n);
MatrixXd W12 = Z.block(0, n, n, n);
MatrixXd W21 = Z.block(n, 0, n, n);
MatrixXd W22 = Z.block(n, n, n, n);
MatrixXd lhs(2 * n, n);
MatrixXd rhs(2 * n, n);
MatrixXd eye = MatrixXd::Identity(n, n);
lhs << W12, W22 + eye;
rhs << W11 + eye, W21;
JacobiSVD<MatrixXd> svd(lhs, ComputeThinU | ComputeThinV);
X = svd.solve(rhs);
}
void lqr(MatrixBase<DerivedA> const& A, MatrixBase<DerivedB> const& B, MatrixBase<DerivedQ> const& Q, MatrixBase<DerivedR> const& R, MatrixBase<DerivedK> & K, MatrixBase<DerivedS> & S)
{
LLT<MatrixXd> R_cholesky(R);
care(A, B, Q, R, S);
K = R_cholesky.solve(B.transpose() * S);
}
