## 4 Target State Prediction with Uncertainty Estimation
### Stochastic Model Predictive Controller  
The first step of designing the SMPC-based motion planner is defining the plant model [32]. The state-space model of the longitudinal dynamics is defined by considering the characteristics of the longitudinal actuator. The delay of the acceleration and braking can be modeled as a first-order delay model. Therefore, the first-order input delay model has been employed to define a longitudinal model. The state and input vectors are defined as follows:  
\[x_{[k]}=\left[ \begin{array}{cc}p_{[k]}&v_{[k]}&a_{[k]}\end{array} \right]^{T},\quad u_{[k]}=a_{des,[k]} \tag{17}\]  
where \(p_{[k]}\), \(v_{[k]}\), and \(a_{[k]}\) are the position, velocity, and acceleration of the ego vehicle on the curvilinear coordinate along the desired trajectory of the ego vehicle. \(a_{des,[k]}\) is the longitudinal acceleration input, which is determined by solving the SMPC problem. Based on the state and input vectors, the longitudinal dynamics are defined as follows:  
\[\left[ \begin{array}{c}p_{[n+1]}\\ v_{[n+1]}\\ a_{[n+1]}\end{array} \right]=\left[ \begin{array}{ccc}1&dt&0\\ 0&1&dt\\ 0&0&1-dt/\tau_{x}\end{array} \right]\left[ \begin{array}{c}p_{[n]}\\ v_{[n]}\\ a_{[n]}\end{array} \right]+\left[ \begin{array}{c}0\\ 0\\ dt/\tau_{x}\end{array} \right]u_{[k]} \tag{18}\]  
where \(dt\) and \(\tau_{x}\) are the sampling time of the SMPC solver and the acceleration delay constant of the actuator. The next step is defining the constraints for the SMPC problem. The concept of intersection motion planning is described in Figure 10. The example scenario is the primary target-only case where a single conflict point region is predicted inside the intersection as shown in Figure 10a. This conflict point region can be defined based on the prediction time vs. the predicted station plane as shown in Figure 10b. When defining the conflict point region, the prediction results of the ego vehicle under the assumption of the cross mode and the predicted states of the approaching target using the proposed predictor have been used to find the most probable conflict position and time. The constraints have been determined to avoid the conflict point region while considering the prediction uncertainty. Constrains for the SMPC problem are categorized as chance constraints and deterministic constraints. The constraints related to the predicted states of the targets are defined as chance constraints. Meanwhile, the dynamics, actuator limitation, and initial condition correspond to the deterministic constraints. As mentioned before, the predicted states of the target are used to define the conflict point region. Therefore, the position constraint should consider the uncertainty of target state prediction. In other words, the position  
Figure 10: Concept of the proposed motion planner. (**a**) Example scenario; (**b**) motion planning in predicted station vs. prediction time plane. constraint is designed as a chance constraint. The inequalities of the position constraint are defined as follows:  
\[ \begin{array}{l}\textit{Yield Mode}\,:\,0\leq p_{[k]}\,\,\,\textit{and}\,\,\,p_ {[k]}\leq P_{ego,[k]}-c_{[k]}\\ \textit{Cross Mode}\,:\,P_{ego,[k]}+c_{[k]}\leq p_{[k]}\\ \text{where,}\\ c_{[k]}=\Big{(}TTC_{conf,min}-\frac{\tau_{[k],1}}{\tau_{[k],2}}\Big{)}\cdot v_{[ k]}+C_{conf,\min}\end{array}  \tag{19}\]  
where \(TTC_{conf,min}\) and \(C_{conf,min}\) are the minimum value of \(TTC_{conf}\) and \(C_{conf}\) from the human driving data analysis. \(\overline{x}_{[k],1}\) and \(\overline{x}_{[k],2}\) are the predicted position and velocity of the target vehicle, which have uncertainty. \(P_{ego}\) is the base constraint that follows the predicted states of the ego vehicle, which is represented as a green dotted line in Figure 10b. \(c_{[k]}\) is the coupled term between the optimal state of the ego vehicle and the predicted state of the target vehicle. Therefore, (19) has a stochastic variable as a form of an inequality constraint. In other words, (19) is the chance constraint of the SMPC problem.