renamed matrix.h to have symmetric matrices
parameter ilqg compiles, but not working. plotting is working for it