implemented cost function for gridworld