Behavior planning--13. Implement a cost function in C + +

In the previous quizzes, you designed a cost function to choose a lane when trying to reach a goal in highway driving:

COST=1?E?? ? ? ∣δd∣ ?? < Span class= "Fontsize-ensurer reset-size5 size5" > ?? /? δs

Here, δwas the lateral distance between the goal lane and the final chosen Lane, and δwas the longitudinal d Istance from the vehicle to the goal.

In this quiz, we ' d like your to implement the cost function in C + +, but with one important change. The finite state machine we use for vehicle behavior also includes states for planning a lane change right or left (PLCR o R PLCL), and the cost function should incorporate this information. We'll provide the following four inputs to the function:

• Intended lane:the intended lane for the given behavior. For PLCR, PLCL, LCR, and LCL, this would is the one lane over from the current lane.
• Final lane:the Immediate resulting lane of the given behavior. For LCR and LCL, this would is one lane over.
• Theδs distance to the goal.
• The goal lane.

Your task in the implementation is to modifyδD in the equation above so it satisifes:

• ΔD is smaller as both intended lane and final lane were closer to the goal lane.
• The cost function provides different costs for each possible behavior:kl, PLCR/PLCL, LCR/LCL.
• The values produced by the cost function is in the range 0 to 1.

You can implement your solution in cost.cpp below.

Cost.cpp

floatGoal_distance_cost (intGoal_lane,intIntended_lane,intFinal_lane,floatdistance_to_goal) {    /*The cost increases with both the distance of intended lane from the goal and the distance of the final lane fro M the goal.    The cost of being out of the goal lane also becomes larger as vehicle approaches the goal. */    intDelta_d =2.0*goal_lane-intended_lane-Final_lane; floatCost =1-Exp (-(ABS (DELTA_D)/distance_to_goal)); returnCost ;}

Behavior planning--13. Implement a cost function in C + +