Fixed error in optimization alg