Update costFunction.m