Quadcopter stabilization algorithm