Skip to main content
. 2025 Nov 18;25(22):7032. doi: 10.3390/s25227032
Algorithm 2. Execution Procedure of the PSMC Position Controller
Input: Current UAV state γk = xk, yk,  zkT
            Desired reference yd,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: ek ← yk− yd,k
Linearize nonlinear UAV dynamics at operating point yk:
             Obtain linear system: xk+1= A xk+ B uk
Construct standard MPC cost function:
             J(u) =∑i=0n−1[‖ek+i‖Q2 + ‖uk+i‖R2] 
Formulate MPC constraints:
            State limits:  ymin≤ yk+i≤ ymax
            Input limits: u min≤ uk+i ≤ umax
                Terminal condition: optional
Solve convex MPC optimization problem to obtain  uMPC= [uk…, uK+M−1]
Initialize PSO search around uMPC[0]:
             Define particle positions as perturbations Δu ∈ Rm around uMPC[0]
Define PSO objective function:
             JPSO(Δu) = ‖ypredk +1 − yd,k+1‖Q + λ‖Δu‖2
Run PSO optimizer (see Algorithm 1) to minimize  JPSO(Δu) 
             Obtain best particle Δuopt
Compute final control input:
            uk*←uMPC[0] + Δuopt
Apply uk* to UAV system
Wait for next sampling interval and repeat
Output: Optimized control input uk*