Udacity 4.Behavior_Planning 3

udacity4_Behavior_Planning 3

设计第二个损失函数

创建一个成本函数，在给定几个行为选项的情况下，让车辆在可能的最快车道上行驶

输入：

目标速度

预期车道

最终车道

车道限速

目标：当前车道和最终车道都是快速的时候，会降低cost

对每一个cost提供的都是不同的可能性

数值大小在0-1之间

.h函数

#ifndef COST_H
#define COST_H

#include <vector>

double inefficiency_cost(int target_speed, int intended_lane, int final_lane, 
                         const std::vector<int> &lane_speeds);

#endif  // COST_H

定义第二个损失函数

double inefficiency_cost(int target_speed, int intended_lane, int final_lane, 
                         const std::vector<int> &lane_speeds) {
  // Cost becomes higher for trajectories with intended lane and final lane 
  //   that have traffic slower than target_speed.
  double speed_intended = lane_speeds[intended_lane];
  double speed_final = lane_speeds[final_lane];
  double cost = (2.0*target_speed - speed_intended - speed_final)/target_speed;

  return cost;
}