## 3 Motion Prediction of the Surrounding Vehicles
### Prediction of the Vehicle Motion  
The velocity of the controlled vehicle can be measured and assumed to be constant along the prediction horizon. The velocities and the distances measured from the controlled vehicle can be measured. The motion prediction of the surrounding vehicles is based on the NGSIM dataset, with which the density functions are computed. The lateral position of the vehicle is taken into account through the Gamma density function where the parameters of the density function are computed from the measurements Section 3.1. The longitudinal motion prediction is based on the Gaussian density functions, which is presented in Section 3.2. Motion-planning is made using the presented graph-based algorithm, in which weights are assigned to each node. The value of the weight depends on the maximum value of the lateral acceleration (see Section 2). An important step is to guarantee a collision-free trajectory during motion-planning. This can be made using the previously computed graph-based algorithm. Since the positions of the nodes are computed previously, based on the density functions and the measurements of the states of the surrounding vehicles, the probabilities can be computed. These values are added to the weighted graph. In order to guarantee a collision-free trajectory, the probabilities of the collision must be minimized along the reference trajectory.  
Figure 8: Density functions of the longitudinal motion.  
The lateral motion of the vehicle can be predicted with the following equation:  
\[P_{lat}(y_{j})=\int\limits_{a_{min}}^{a_{max}}f_{lat}(x,\alpha,\beta)dx\cdot P_{ w}(\sigma,\mu), \tag{12}\]  
where \(f_{lat}\) is the Gamma density function and \((a_{min},a_{max})\) is the set of the possible lateral accelerations. \(P_{w}\) is responsible for taking into account the width of the vehicle, where \(y_{w}\) is the width of the vehicle and variance is computed as \(\sigma\)=\(\frac{\sqrt{y_{w}}}{4}\). The \(\mu\) gives the position of the center of the gravity of the vehicle. Moreover, the probabilities according to the longitudinal motion are also must be performed:  
\[P(x_{j},a_{d,i})=\int\limits_{0}^{t_{i}v_{x}}f_{j,i}(x)dx, \tag{13}\]  
where \(v_{x}\) gives the longitudinal velocity of the controlled vehicle. \(x_{j}\) gives the longitudinal position of the graph node in the \(j^{th}\) time step, and \(a_{d,i}\) is the current acceleration of the vehicle. Using Equation (12) and (13) the probability of collision can be computed to one node as:  
\[P(x_{j},y_{j},a_{d,i})=P(x_{j},a_{d,i})\cdot P_{lat}(y_{j}). \tag{14}\]  
During the reduction of the graph, a bound (\(y_{b,p}\)) has been defined in Equation (7). The maximum value of the probability is assigned to the given node:  
\[P(x_{m},y_{m})=\max(P(x_{j},y,a_{d,i}))\quad y\in[y_{m}-\frac{y_{b,p}}{2},\,y_ {m}+\frac{y_{b,p}}{2}], \tag{15}\]  
where \(y_{b,p}\) is the bound, which is used during the k-medoids algorithm, see Equation (7). Finally, to every node, the probabilities can be computed. The weights between to edges can be assigned as \(w:\bar{E}\rightarrow\mathbb{R}\)  
\[w_{n}=\beta(P(x_{m},y_{m}))+\theta(c\cdot v_{x}^{2}), \tag{16}\]  
where \((x_{m},y_{m})\) represents the coordinates of \(m^{th}\) node, which is connected with the \(n^{th}\) node and \(m>n\). The curvature (\(c\)) can be easily computed from (4). Note that the collision free trajectory must be guaranteed, in order to achieve this, \(\beta\) and \(\theta\) weighting functions can be used. This means the maximum value of the collision probability is assigned to the nodes.