Fixed dimensions of LQR cost matrix, added rotation to nav_callback, added correct weight estimate, ready for testing