Algorithm 1 Online MPC Control Loop with Optional Disturbances and Re-Entry Strategy
Require: $T_s$, $N_p$, $Q⪰0$, $R\succ 0$, bounds $u_{min},u_{max}$, band $\delta$, reference path $\mathcal{P}$, optional disturbances $\mathcal{D}$
1:	while goal not reached do
2:	Read ${\mathbf{x}}_k={[x_k,y_k,{\theta }_k]}^{\top }$
3:	Compute lateral errors $e_y\left(k\right)$ for all ${\mathbf{p}}_i\in \mathcal{P}$
4:	Find closest point ${\mathbf{p}}_c=(x_c,y_c,{\theta }_c)=arg{min}_{{\mathbf{p}}_i\in \mathcal{P}}\left|e_y\left(k\right)\right|$
5:	${\mathcal{H}}_k$ ← ComputeReferenceHorizon$({\mathbf{x}}_k,{\mathbf{p}}_c,\mathcal{P},N_p,\delta )$	▹ see Algorithm 2
6:	Build ${\mathbf{X}}^{ref}$ by stacking ${\mathbf{x}}_{c+i}^{ref}$ from ${\mathcal{H}}_k$
7:	Linearize model at $({\widehat{x}}_k,{\widehat{y}}_k,{\widehat{\theta }}_k,{\widehat{v}}_k,{\widehat{\omega }}_k)$ to obtain $A_k,B_k$
8:	Construct prediction matrices $(\Phi ,\Gamma )$ from $(A_k,B_k)$ and $N_p$
9:	Form $Q_b=\mathrm{blkdiag}(Q,\dots ,Q)$, $R_b=\mathrm{blkdiag}(R,\dots ,R)$
10:	Compute $H={\Gamma }^{\top }{Q}_b\Gamma +R_b$,   $f={\Gamma }^{\top }{Q}_b(\Phi {\mathbf{x}}_k-{\mathbf{X}}^{ref})$
11:	Solve ${min}_{\mathbf{U}\in {\mathbb{R}}^{2N_p}}\frac{1}{2}{\mathbf{U}}^{\top }H\mathbf{U}+f^{\top }\mathbf{U}$ s.t. $u_{min}\le \mathbf{U}\le u_{max}$
12:	Extract and apply ${\mathbf{u}}_k^{★}={[v_k^{★},{\omega }_k^{★}]}^{\top }$
13:	if a disturbance $(p_j,v_j,{\omega }_j,{\tau }_j)\in \mathcal{D}$ is due then
14:	Apply $(v_j,{\omega }_j)$ for ${\tau }_j$ seconds; mark as applied; continue
15:	Update metrics ($e_y$, band exits, # re-entries)