Stabilize algorithm according to the implementation of the crazyflie