made LQR matrices global, fixed rotation vars in nav_callback