Algorithm 1: Trajectory planning

1:initialize: 2:$v_{des}\leftarrow$ desired longitudinal velocity from user 3:procedure GENERATE TRAJECTORY 4:top: 5:      Use Equation (3) to create matrix Q 6:      $\overline{K}\leftarrow$ Ackerman pole placement formula as (4) and calculate $\overline{K}$ 7:      $P\leftarrow$ Obtained $\overline{K}$ in (28)–(37) and generate P 8:      $Q\leftarrow$ Obtained P and substitute (19) in (25)-(27) to calculate Q 9:loop: 10:    ${\overline{x}}_{ref}\leftarrow$generate target states as 11:    $\overline{x}\leftarrow$current state vector 12:    getRoadBoundaryConstraints as (38) (39)–(45) 13:    getCollisionAvoidanceConstraints as (46)–(50) 14:    $u^{*}\leftarrow$ solve MPC with generated Q from top with fixed R 15:    $x^{*}\leftarrow$ Apply optimal input $u\left(k\right)$ as (12)–(15) 16:    if user request to change $v_{des}$ then 17:           goto initialize. 18:           else 19:     goto loop.