modified optimized_function.m to speed up calculations