path planning project

This is the project of path planning. The current location and the simulation of all cars are given by the input date. There are some criteria, include the speed limit (50), the acceleration (10), and jerk (10). The goal of this project is to predict the location of all cars and give action of the vechile to let the vehicle drive on a freeway.

There are three major parts:

• Predict next location for all cars
  The input of prediction is the sensor fusion data. The output is the location and status of the next timestamp.
• Behavior planner
  The behavior planner includes two steps. The first one is to check the front car. If the front vehicle is too slow and blocks the ego car, then check if the ego car can go left or right lane. After lane change, the ego-car can accelerate and keep cruise speed.
• The trajectory line
  The trajectory line, which is the green line are generated based on the current statues. The x,y points are calculated by the following eqution:
  x~i = xcos(-ref_yaw) - ysin(-ref_yaw)
  y~i = xsin(-ref_yaw) + ycos(-ref_yaw)