implement lqr design in c++

[RussTedrake]

Looks like there is some good code available for solving the ARE. a quick search turned up
http://slicot.org/objects/software/shared/libindex.html#S

and for large sparse problems:
http://www.netlib.org/lyapack/README

there may be more. if people have known solutions they would recommend, I'd love to hear them.

[psiorx]

I found some good references on various algorithms for solving ricatti equations.

http://www.engr.iupui.edu/~skoskie/ECE684/Riccati_algorithms.pdf
http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.51.5104&rep=rep1&type=pdf
http://www.scm.org.co/aplicaciones/revista/Articulos/1013.pdf

I got a prototype of the matrix sign function method working in matlab on Friday. The LQR cost-to-go computed matches matlab's LQR function.

If you think it would be worth the cycles to push this to C++ to give us some baseline LQR functionality outside of MATLAB, you can assign this to me.

[psiorx]

State-of-the-art implementations of ARE solvers use a Schur decomposition method. This is what MATLAB is using (SciPy, Octave, and LAPACK also it). The Schur method has superior numerical stability compared to other approaches such as the Matrix Sign function method.

I've looked into what it would take to implement a Schur-based ARE solver using Eigen in c++ and here's what I've found:

Eigen includes a Schur factorization but in order for it to be usable for solving the LQR ARE, the eigenvalues of the quasi-triangular Schur matrix need to be sorted such that all of the eigenvalues of the stable subspace appear in the upper left block.

MATLAB provides a function ordschur() for doing exactly this. SciPy also has a function scipy.linalg.schur(..., sort='lhp') for it. Eigen does not currently have built-in functionality for doing this.

Research on the subject of reordering the eigenvalues of the Schur matrix is available( http://web.cs.ucdavis.edu/~bai/publications/baidemmel93.pdf ). The dominant approach is to solve a Sylvester equation for each pair of eigenvalues that need to be swapped during the sort and then use a chain of concatenated Givens rotations to yield the modified orthogonal matrix of the Schur decomposition.

Eigen has Givens rotations ( http://eigen.tuxfamily.org/dox/classEigen_1_1JacobiRotation.html ) but doesn't appear to have a Sylvester solver yet. It looks like they had it on their radar at one point but the effort may have been abandoned. ( http://eigen.tuxfamily.org/bz/show_bug.cgi?id=840 )