## 3 Model Predictive Control-Based Longitudinal and Lateral Tracking Control
### Online Active Set Algorithm
\begin{split}&\widetilde{x}^{*}:[0,1]\to\mathcal{R}^{n}, \widetilde{x}^{*}(0)=x_{0}^{*},\widetilde{x}^{*}(1)=x_{new}^{*}\\ &\widetilde{\lambda}^{*}:[0,1]\to\mathcal{R}^{m},\widetilde{ \lambda}^{*}(0)=\lambda_{\Lambda}^{*},\widetilde{\lambda}^{*}(1)=x_{\Lambda (new)}^{*}\\ &\widetilde{\mathbf{\Lambda}}\,:[0,1]\to 2^{\mathbf{M}}, \widetilde{\mathbf{\Lambda}}(0)=\mathbf{\Lambda},\widetilde{\mathbf{\Lambda} }(\tau)\subseteq\mathbf{M}\\ &\widetilde{I}\,:[0,1]\to 2^{\mathbf{M}},\widetilde{I}(\tau)\,:= \mathbf{M}\backslash\widetilde{\mathbf{\Lambda}}(\tau)\\ &\mathbf{M}\,:=\{1,\cdots,m\}\end{split}  \tag{18}\]  
where \(\mathbf{M}\) denotes the index of the inequality constraints, \(\widetilde{x}^{*}\) and \(\widetilde{\lambda}^{*}\) define the linear homotopies of the primal and dual optimal solution in \(n\) and \(m\) dimension space, respectively. \(\widetilde{\mathbf{\Lambda}}\) and \(\widetilde{\mathbf{I}}\) define the linear homotopies of the working set and non-working set in the feasible set. For the strict convex QP problem, the KKT conditions should be satisfied at every value of \(\tau\):  
\[ \begin{split}&\left[\begin{array}{cc}\mathbf{H}&G_{\widetilde{ \mathbf{\Lambda}}(\tau)}^{T}\\ G_{\widetilde{\mathbf{\Lambda}}(\tau)}&\mathbf{0}\end{array}\right] \left[\begin{array}{c}\widetilde{x}^{*}(\tau)\\ -\widetilde{\lambda}_{\widetilde{\mathbf{\Lambda}}(\tau)}^{*}(\tau)\end{array} \right]=\left[\begin{array}{c}-\widetilde{\mathbf{g}}(\tau)\\ -\widetilde{\mathbf{b}}_{\widetilde{\mathbf{\Lambda}}(\tau)}(\tau)\end{array} \right]\\ &-\widetilde{\lambda}_{\widetilde{\mathbf{\Lambda}}(\tau)}^{*}(\tau)=0,G_{ \widetilde{\mathbf{I}}(\tau)}^{*}\tau(\tau)\geq\widetilde{\mathbf{b}}_{ \widetilde{\mathbf{I}}(\tau)}(\tau),\widetilde{\lambda}_{\widetilde{\mathbf{ \Lambda}}(\tau)}^{*}(\tau)\geq 0\end{array}\right.\\ \end{split}  \tag{19}\]  
Since \(\widetilde{x}^{*}(\tau)\) and \(\widetilde{\lambda}^{*}(\tau)\) are piecewise linear functions and can be defined as:  
\[\left\{ \begin{array}{c}\widetilde{x}^{*}(\tau)\,:=\widetilde{x}^{*}(0)+\tau \Delta x^{*}\\ \widetilde{\lambda}_{\widetilde{\mathbf{\Lambda}}(\tau)}^{*}(\tau)\,:= \widetilde{\lambda}_{\widetilde{\mathbf{\Lambda}}(0)}^{*}(0)+\tau\Delta x_{ \widetilde{\mathbf{\Lambda}}}^{*}\,\,\,\,;\,\tau\in[0,\tau_{max}]\end{array} \right. \tag{20}\]The primal QP is transformed to the equivalent QP and reuse the KKT condition to get the optimal solution \(\Delta x^{*}\) and \(\lambda^{*}_{\Lambda}\), then the optimal solution of \(QP\big{(}w_{0}^{new}\big{)}\) can be fast calculated. \[ \begin{split}\left[\begin{array}{cc}\mathbf{H}&\mathbf{G}_{ \Lambda}^{T}\\ \mathbf{G}_{G}&\mathbf{0}\end{array}\right]\left[\begin{array}{c}\Delta x^{* }\\ -\Delta\lambda^{*}_{*}\end{array}\right]=\left[\begin{array}{c}-\Delta\mathbf{g }\\ -\Delta\mathbf{b}_{QP\big{(}w_{0}^{new}\big{)}}\end{array}\right]\\ \tau_{max}^{prim}:=\underset{i\in\Lambda}{min}\bigg{\{}\frac{\mathrm{b}_{i}(w_{ 0})-\mathbf{G}_{i}^{\mathsf{T}}\Delta x^{*}}{\mathbf{G}_{i}^{\mathsf{T}}\Delta x ^{*}-\Delta\hbar_{i}}|\mathbf{G}_{i}^{\mathsf{T}}\Delta x^{*}<0\bigg{\}}\in \mathcal{R}^{+}\\ \tau_{max}^{dual}:=\underset{i\in\Lambda}{min}\bigg{\{}-\frac{\lambda^{*}_{ \Lambda}}{\Delta\lambda^{*}_{i}}|\Delta\lambda^{*}_{i}<0\bigg{\}}\in\mathcal{R }^{+}\\ \tau_{max}:=\underset{i\in\Lambda}{min}\bigg{\{}1,\tau_{max}^{prim},\tau_{max} ^{dual}\bigg{\}}\in[0,1]\end{split}  \tag{21}\]  
As long as the \(\tau_{max}\) equals one, the solution of \(QP\big{(}w_{0}^{new}\big{)}\) is found; otherwise the infeasible constraints will be added or removed from the working set (\(\Lambda\)) just like the procedure of the conventional active-set algorithm.