Cost function seems to be working