Add working version of costFunctionReg.m