added updatecosts and updated dstar and cost_function