## 3 MPC Strategy
Based on the state space model (15), the MPC algorithm is designed to control the system. Here, a cost function is required and optimized through solving a quadratic program (QP) problem to obtain the optimal control sequence in predictive horizon \(N\). The cost function for the MPC can be defined as  
\[J(k)=\sum_{j=1}^{N}\left[\|q(k+j|k)\|_{Q}^{2}+\|u(k+j-1|k)\|_{R}^{2}\right] \tag{16}\]  
where \(Q\in\mathbb{R}^{n\times n}\) and \(R\in\mathbb{R}^{m\times m}\) are appropriate weighting matrices. \(q(k+j|k)\) and \(u(k+j-1|k)\) denote the predicted state and control vector in predictive horizon at time \(k\). Through solving the following finite-horizon optimal control problem online, the optimal control sequence can be obtained. \[u^{*}=\arg\min_{u}\;\left\{J(k)\right\} \tag{17}\]Since the torques generated by motors are limited by the performance of the motors, the \(u(k)\) has an upper and lower bound, and the range of state variable \(q(k)\) is also constrained. Thus, the following constraints should be imposed on the system:  
\[u_{min}\leq u\left(k\right)\leq u_{max} \tag{18}\] \[q_{min}\leq q\left(k\right)\leq q_{max} \tag{19}\]  
In order to solve the optimal control problem, Equation (17) is transformed into a standard quadratic form which a QP solver can solve. The following prediction vector is defined:  
\[\bar{q}(k) =\left[q\left(k+1|k\right),\cdots,q(k+N|k)\right]^{T}\in\mathbb{ R}^{n\times N} \tag{20}\] \[\bar{u}\left(k\right) =\left[u\left(k|k\right),\cdots,u(k+N-1|k)\right]^{T}\in\mathbb{ R}^{m\times N} \tag{21}\]  
Based on Equation (15), the relationship between vectors \(\bar{q}(k)\) and \(\bar{u}(k)\) can be described as  
\[\bar{q}(k)=Gq(k|k)+L(k)\bar{u}(k) \tag{22}\]  
where  
\[q(k+1)=Aq(k)+Bu(k) \tag{15}\]0  
\[L(k) = \begin{bmatrix}\alpha_{11}(k)&0&\ldots&0\\ \alpha_{21}(k)&\alpha_{22}(k)&\cdots&0\\ \vdots&\vdots&\ddots&\vdots\\ \alpha_{N1}(k)&\alpha_{N2}(k)&\ldots&\alpha_{NN}(k)\end{bmatrix} \in\mathbb{R} ^{nN\times mN}\]  
with \(\alpha_{ij}(k)\) defined as  
\[\alpha_{ij}(k)=A^{i-j}B(k+j-1),\;i\geq j\]  
Two block diagonal matrices are defined:  
\[\bar{Q} = \begin{bmatrix}Q&0&\ldots&0\\ 0&Q&\ldots&0\\ \vdots&\vdots&\ddots&\vdots\\ 0&0&\ldots&Q\end{bmatrix} \in\mathbb{R}^{nN\times nN},\;\;\bar{R} = \begin{bmatrix}R&0&\ldots&0\\ 0&R&\ldots&0\\ \vdots&\vdots&\ddots&\vdots\\ 0&0&\ldots&R\end{bmatrix} \in\mathbb{R}^{mN\times mN}\]  
Thus, the optimal control problem (17) can be rewritten as  
\[\arg\min_{u}J(k) =min\left\|\bar{q}\left(k\right)\right\|_{Q}^{2}+\left\|\bar{u} \left(k\right)\right\|_{R}^{2}\] \[=min\left\|Gq(k|k)+L(k)\bar{u}(k)\right\|_{Q}^{2}+\left\|\bar{u} \left(k\right)\right\|_{R}^{2} \tag{23}\]  
subjected to  
\[\bar{u}_{min}  
After some algebraic manipulations, the optimization problem (23) is transformed into a standard quadratic form:  
\[min\frac{1}{2}\bar{u}^{T}(k)W(k)\bar{u}(k)+C^{T}(k)\bar{u}(k) \tag{26}\]  
with  
\[W(k) =2(L^{T}(k)\bar{Q}L(k)+\bar{R})\in\mathbb{R}^{mN\times mN}\] \[C(k) =2L^{T}(k)\bar{Q}Gq(k|k)\in\mathbb{R}^{m\times N}\]  
where matrix \(W(k)\) denotes the quadratic part of the objective function and the vector \(C(k)\) describes the linear part. The QP problem (26) can be solved online to obtain an optimal prediction control sequence. The first control vector of the sequence is applied to the system at time \(k\). In the next sampling instant, this process is repeated to achieve rolling optimization.