costFunction.m updated