# MPC and PSO Based Control Methodology for Path Tracking of 4 WS4 WD Vehicles
## 3 Control Methodology
### MPC-SQP Method
#### 3.1.2 Model Predictive Control  
To develop the MPC controller, the offset model as given in Equations (27) and (28) needs to be discretized and its discrete-time state vector form can be written as  
\[\mathbf{x}_{d}(k+1)= \mathbf{A}_{d}\mathbf{x}_{d}(k)+\mathbf{B}_{d}\mathbf{u}_{d}(k),\] \[\mathbf{y}_{d}(k)= \mathbf{C}_{d}\mathbf{x}_{d}(k), \tag{29}\]  
where \(\mathbf{x}_{d}(k)\), \(\mathbf{y}_{d}(k)\) and \(\mathbf{u}_{d}(k)\) are the state vector, output vector and input vector, respectively. The coefficient matrices \(\mathbf{A}_{d}\), \(\mathbf{B}_{d}\) and \(\mathbf{C}_{d}\) are updated after discretization. To eliminate undesirable oscillations, embedded integrator vectors \(\Delta\mathbf{x}_{d}(k)=\mathbf{x}_{d}(k+1)-\mathbf{x}_{d}(k)\), \(\Delta\mathbf{u}(k)=\mathbf{u}_{d}(k+1)-\mathbf{u}_{d}(k)\) are defined, thereby an augmented state-space model can be expressed as  
\[ \begin{bmatrix}\Delta\mathbf{x}_{d}(k+1)\\ \mathbf{y}_{d}(k+1)\end{bmatrix}  = \begin{bmatrix}\mathbf{A}_{d}&\mathbf{O}^{T}\\ \mathbf{C}_{d}\mathbf{A}_{d}&\mathbf{I}\end{bmatrix}\begin{bmatrix}\Delta\mathbf{x}_{d}(k)\\ \mathbf{y}_{d}(k)\end{bmatrix} + \begin{bmatrix}\mathbf{B}_{d}\\ \mathbf{C}_{d}\mathbf{B}_{d}\end{bmatrix} \Delta\mathbf{u}(k),\] \[\mathbf{y}_{d}(k) = \begin{bmatrix}\mathbf{O}&\mathbf{I}\end{bmatrix}\begin{bmatrix}\Delta \mathbf{x}_{d}(k)\\ \mathbf{y}_{d}(k)\end{bmatrix} , \tag{30}\]  
where \(\mathbf{O}\) is a zero matrix, and \(\mathbf{I}\) represents the identity matrix. Defining the new state vector \(\mathbf{x}(k)=[\Delta\mathbf{x}_{d}(k)^{T}\mathbf{y}_{d}(k)^{T}]^{T}\), the augmented model can be written in the following matrix form:  
\[\mathbf{x}(k+1)= \mathbf{A}\mathbf{x}(k)+\mathbf{B}\Delta\mathbf{u}(k),\] \[\mathbf{y}(k)= \mathbf{C}\mathbf{x}(k), \tag{31}\]  
where  
\[\mathbf{A}= \begin{bmatrix}\mathbf{A}_{d}&\mathbf{O}^{T}\\ \mathbf{C}_{d}\mathbf{A}_{d}&\mathbf{I}\end{bmatrix} ,\quad\mathbf{B}= \begin{bmatrix}\mathbf{B}_{d }\\ \mathbf{C}_{d}\mathbf{B}_{d}\end{bmatrix} ,\quad\mathbf{C}= \begin{bmatrix}\mathbf{O}^{T}&\mathbf{I} \end{bmatrix} .\]  
**Theorem 1**. : _Given a discrete time system following the form of Equation (31), the asymptotic stabilization of the closed-loop system can be realized by substituting the first item of \(\Delta\mathbf{U}^{\ast}\) as the control input \(\Delta\mathbf{u}(k)\), when \(\Delta\mathbf{U}^{\ast}\) is the optimal solution of the following optimization problem:_  
\[\arg\min_{\Delta\mathbf{U}}J=(\mathbf{R}_{s}-\mathbf{Y})^{T}(\mathbf{R}_{s}-\mathbf{ Y})+\Delta\mathbf{U}^{T}\mathbf{R  
where, \(\Delta u_{k},...,\Delta u_{k+N_{p}-1}\) are obtained by the optimal solution of future inputs, and \(y_{k},...,y_{k+N_{p}}\) represent the corresponding error sequence between future outputs and target. \(r_{w}\) is the nonnegative gain matrix.  
According to the definition in Equation (33), the Lyapunov function at the next sample \(k+1\) is written as  
\[V(x_{k+1})=\sum_{i=1}^{N_{p}}y_{k+1+i}^{T}y_{k+1+i}+\sum_{i=0}^{N_{p}-1}\Delta u _{k+1+i}^{T}r_{w}\Delta u_{k+1+i}. \tag{34}\]  
To facilitate the comparison between two neighboring Lyapunov function values, a intermediate function \(\bar{V}\) is defined, which is formed by evaluating \(V(x_{k+1})\) with a defined inputs sequence, which is obtained by shifting the optimal inputs sequence of \(V(x_{ki})\) one step forward, and setting its last input \(\Delta u_{k+N_{p}}\) as zero.