behavior planning——14.implement a cost function in C++

n most situations, a single cost function will not be sufficient to produce complex vehicle behavior. In this quiz, we'd like you to implement one more cost function in C++. We will use these two C++ cost functions later in the lesson. The goal with this quiz is to create a cost function that would make the vehicle drive in the fastest possible lane, given several behavior options. We will provide the following four inputs to the function:

• Target speed: Currently set as 10 (unitless), the speed at which you would like the vehicle to travel.
• Intended lane: the intended lane for the given behavior. For PLCR, PLCL, LCR, and LCL, this would be the one lane over from the current lane.
• Final lane: the immediate resulting lane of the given behavior. For LCR and LCL, this would be one lane over.
• A vector of lane speeds, based on traffic in that lane: {6, 7, 8, 9}.

Your task in the implementation will be to create a cost function that satisifes:

• The cost decreases as both intended lane and final lane are higher speed lanes.
• The cost function provides different costs for each possible behavior: KL, PLCR/PLCL, LCR/LCL.
• The values produced by the cost function are in the range 0 to 1.

You can implement your solution in cost.cpp below.

cost.cpp

float inefficiency_cost (int target_speed, int intended_lane,int final_lane, vector<int> lane_speeds)
{
   float speed_intended=lane_speeds[intended_lane];
   float speed_final=lane_speeds[final_lane];
   float cost=(2.0*target_speed-speed_intended-speed_final)/target_speed;
   return cost;
}