    return speed_cost;
}

// Determine cost for lane change and heavily penalise for illegal maneuver
double BehaviourStateMachine::LaneChangeCost(int& lane, int& current_lane)
{
    double lane_cost;

    if (lane < -0.001 || lane > 2.001)
    {
        lane_cost = 1.0;
    }
    else if ((current_lane == 0) && (lane == 1))
    {