fixed a control-loop algo

Signed-off-by: Nicholas Tan Jerome <nicholas.jerome@kit.edu>