added a streamlined version of the control law which runs faster