Added an estemate cost function

The function calculates an estemate for the cost to travel.