finish the costFunction.m