Enforce symmetry of Q and R matrices in OCP QP
https://bitbucket.org/blaze-lib/blaze/wiki/Symmetric%20Matrices Eigen3 seems not to have a class for symmetric matrix: https://stackoverflow.com/questions/13403982/eigen-efficient-type-for-dense-symmetric-matrix