Added cost function calculation