Rk4 --- Ready to merge and waiting for review
I added the Linear dynamics and checked the derivatives of the dynamics and costs. The first order derivatives of L and F work.
I implemented the second order derivatives of L as well, however, there is no test yet.
- The current test, by using the residuals doesn't make sense to me for this integration scheme.
- I should implement an FD to unittest Lxx, Lxu etc (which should atlleast work for the case of DAMLQR). That is what I am doing now.
In case you wish to merge it now before the school, you could go ahead.
Edit: I added FD on the cost derivatives as well.
Edited by Rohan Budhiraja