Add working version of costFunction.m