Implement Continuous algebraic Riccati equation function

[GiulioRomualdi]

This PR implements an algorithm to solve the Continuous algebraic Riccati equation.

The main idea has been taken from drake.

cc @Giulero

[prashanthr05]

This could possibly be an utility function which could be used in other instances for debugging (for example also in estimators library). What do you think?

[prashanthr05]

symmetricTolerance parameter could be a generic defnition across the framework.

[S-Dafarra]

Also, it might be time-consuming. Consider having a way to skip that, like putting it into an assert.

[GiulioRomualdi]

hI, @S-Dafarra I was trying to transform the function into a class but I've several doubts. I was thinking to organize it as

bool initialize(std::weak_ptr<ParametersHandler::IParametersHandler> handler);

bool setMatrices(Eigen::Ref<const Eigen::MatrixXd> A,
                     Eigen::Ref<const Eigen::MatrixXd> B,
                     Eigen::Ref<const Eigen::MatrixXd> Q,
                     Eigen::Ref<const Eigen::MatrixXd> R);

Solution getSolution();

bool getSolution(Eigen::Ref<Eigen::MatrixXd> S);

however, I don't know how to split the code. For instance, in setMatrices I can just copy the matrices A, B Q and R and I can also create the hamiltonian matrix H.
On the other hand in getSolution() I can run the algorithm to compute S. However, given a set of A B Q and R only one solution exists so it's not efficient to compute the solution in getSolution()

For instance this code will run the algorithm 4 times.

CARE care();
care.setMatrices(A,B,Q,R);

CARE::Solution sol = care.getSolution();
sol = care.getSolution();
sol = care.getSolution();
sol = care.getSolution();
sol = care.getSolution();

So the question is: shall we compute the solution while setting A, B Q and R and store it as a parameter of the class? In this case, shall we add the following matrices as a parameter of the class (notice that the size of H is known only after setMatrices is called)?

Eigen::MatrixXd Z = H;
Eigen::MatrixXd Z_old;

Indeed every time we set new matrices H has to be reset

[S-Dafarra]

I think you can add a solve method that does the computation. This may also just output a boolean in case of success/failure. Then, the entire pipeline init -> set -> solve -> get could be handled by a single static function, in case you don't care about memory allocation.

[GiulioRomualdi]

I like it

[GiulioRomualdi]

Hi @S-Dafarra and @prashanthr05 I completely restructure the code. Let me know what you think

[prashanthr05]

I also clarified the previous doubt of the Hamiltonian matrix's subblock sign differences from Wikipedia. Turns out the choice used here is also a Hamiltonian matrix because it respects the identity $J Z = (J Z)^T$ where $J = \begin{bmatrix} 0_n & I_n \\ - I_n & 0_n \end{bmatrix}$.
(the requirement of off-diagonal elements being symmetric and the sum of diagonal elements equal to zero is the property).

For me the changes look neat. However, I will also incorporate @S-Dafarra suggestions.

[prashanthr05]

this note needs to be changed since the parameters are optional, rather, the method itself is optional.

[GiulioRomualdi]

Fixed in 3c6176d

[GiulioRomualdi]

@S-Dafarra let me know if you're fine with the modifications

[S-Dafarra]

@S-Dafarra let me know if you're fine with the modifications

Yes, feel free to merge as soon as the CI passes.