Added LQR functions, need to find value for mg, Need tilt set in nav_callback