Algorithm 2. Execution Procedure of the PSMC Position Controller

Input: Current UAV state ${\gamma }_k = {\left[x_k,{ y}_k, z_k\right]}^T$             Desired reference $y_{d,k}$,             System matrices $A, B$             MPC horizons: prediction horizon $N$, control horizon $M$             Weighting matrices $Q, R$             Input and state constraints             PSO parameters Compute tracking error: $e_k \leftarrow y_k- y_{d,k}$ Linearize nonlinear UAV dynamics at operating point $y_k$:              Obtain linear system: $x_{k+1}= A x_k+ B u_k$ Construct standard MPC cost function:              $J\left(u\right) ={\sum }_{i=0}^{n-1}[\Vert e_{k+i}\Vert Q^2 + \Vert u_{k+i}\Vert R^2]$ Formulate MPC constraints:             State limits: $y_{min}\le y_{k+i}\le y_{max}$             Input limits: ${u }_{min}\le u_{k+i} \le u_{max}$                 Terminal condition: optional Solve convex MPC optimization problem to obtain $u_{MPC}= [u_k\dots ,{ u}_{K+M-1}]$ Initialize PSO search around $u_{MPC\left[0\right]}$:              Define particle positions as perturbations $\Delta u \in {\mathbb{R}}^m$ around $u_{MPC\left[0\right]}$ Define PSO objective function:             ${ J}_{PSO\left(\Delta u\right) }= \Vert y_{predk +1} - y_{d,k+1}\Vert Q + \lambda {\Vert \Delta u\Vert }^2$ Run PSO optimizer (see Algorithm 1) to minimize ${ J}_{PSO\left(\Delta u\right) }$              Obtain best particle ${\Delta u}_{opt}$ Compute final control input:             $u_k^{*}\leftarrow u_{MPC\left[0\right]} + {\Delta u}_{opt}$ Apply $u_k^{*}$ to UAV system Wait for next sampling interval and repeat Output: Optimized control input $u_k^{*}$