# Quadrotor Formation Strategies Based on Distributed Consensus and Model Predictive Controls
## 3 Dynamic Model of a Quadrotor
### Model Predictive Control
#### 4.2.2 Unconstrained Quadratic Programming Problem
\begin{array}{rcl}J&=&\frac{1}{2}\left(\mathbf{y}_{N_{p}}-\mathbf{r}_{N_{p}}\right)^ {T}P\left(\mathbf{y}_{N_{p}}-\mathbf{r}_{N_{p}}\right)+\frac{1}{2}\sum_{k=0}^{N_{p}-1} \left[\left(\mathbf{y}_{k}-\mathbf{r}_{k}\right)^{T}Q\left(\mathbf{y}_{k}-\mathbf{r}_{k}\right) +\mathbf{u}_{K}^{T}Ru_{k}\right]\\ &=&\frac{1}{2}\left(C\mathbf{x}_{N_{p}}-\mathbf{r}_{N_{p}}\right)^{T}P\left(C\mathbf{x}_{N_ {p}}-\mathbf{r}_{N_{p}}\right)+\frac{1}{2}\sum_{k=0}^{N_{p}-1}\left[\left(C\mathbf{x}_{ k}-\mathbf{r}_{k}\right)^{T}Q\left(C\mathbf{x}_{k}-\mathbf{r}_{k}\right)+\mathbf{u}_{K}^{T}Ru_{k}\right] \\ &=&\frac{1}{2}\left(\mathbf{x}_{N_{p}}^{T}C^{T}PC\mathbf{x}_{N_{p}}-2\mathbf{x}_{N_{p}}^{T }C^{T}\mathbf{p}_{N_{p}}+\mathbf{r}_{N_{p}}^{T}\mathbf{p}_{TN_{p}}\right)+\cdots\\ &&\frac{1}{2}\sum_{k=0}^{N_{p}-1}\left[\mathbf{x}_{K}^{T}C^{T}QC\mathbf{x}_{k}-2\mathbf{x}_ {K}^{T}C^{T}\mathbf{Q}\mathbf{r}_{k}+\mathbf{r}_{K}^{T}Q\mathbf{r}_{k}+\mathbf{u}_{K}^{T}Ru_{k}\right] \\ &=&\frac{1}{2}\widetilde{\mathbf{x}}^{T}\left[\begin{array}{cccc}C^{T}QC&0_{n \times n}&\cdots&0_{n\times n}\\ 0_{n\times n}&\ddots&&\vdots\\ \vdots&&C^{T}QC&0_{n\times n}\\ 0_{n\times n}&\cdots&0_{n\times n}&C^{T}PC\\ \end{array}\right]\widetilde{\mathbf{x}}-\widetilde{\mathbf{x}}^{T}\left[\begin{array}[] {cccc}C^{T}Q&0_{n\times q}&\cdots&0_{n\times q}\\ 0_{n\times q}&\ddots&&\vdots\\ \vdots&&C^{T}Q&0_{n\times q}\\ 0_{n\times q}&\cdots&0_{n\times q}&C^{T}P\\ \end{array}\right]\widetilde{\mathbf{u}}+\cdots\\ &&\widetilde{\mathbf{u}}+\widetilde{\mathbf{r}}^{T}\left[\begin{array}{cccc}C^{T}QC \mathbf{x}_{0}-2\mathbf{x}_{0}C^{T}Q\mathbf{r}_{0}+\mathbf{r}_{0}^{T}Q\mathbf{r}_{0}\\ \end{array}\right]\widetilde{\mathbf{r}}+\cdots\\ &&\\ &=&\frac{1}{2}\widetilde{\mathbf{x}}^{T}\widetilde{Q}_{1}\widetilde{\mathbf{x}}- \widetilde{\mathbf{x}}^{T}\widetilde{Q}_{2}\widetilde{\mathbf{r}}+\frac{1}{2} \widetilde{\mathbf{u}}^{T}\widetilde{R}\widetilde{\mathbf{u}}+\underbrace{\frac{1}{2} \widetilde{\mathbf{r}}^{T}\widetilde{Q}_{3}\widetilde{\mathbf{r}}+\mathbf{x}_{0}C^{T}QC \mathbf{x}_{0}-2\mathbf{x}_{0}C^{T}Q\mathbf{r}_{0}+\mathbf{r}_{0}^{T}Q\mathbf{r}_{0}}_{Constant\text{ } contribution to cost function},\end{array}  \tag{24}\]  
where \(\widetilde{Q}_{1}\in\mathbb{R}^{nN_{p}\times nN_{p}}\), \(\widetilde{Q}_{2}\in\mathbb{R}^{nN_{p}\times qN_{p}}\), \(\widetilde{Q}_{3}\in\mathbb{R}^{qN_{p}\times qN_{p}}\), \(\widetilde{\mathbf{r}}\in\mathbb{R}^{qN_{p}\times 1}\) and \(\widetilde{R}\in\mathbb{R}^{mN_{p}\times mN_{p}}\). In Equation (24), \(\mathbf{x}_{0}\) is the given initial condition and \(\mathbf{r}_{0}\) is the reference of the output state at \(k=0\). \(\widetilde{\mathbf{r}}\) is a column vector consists of the given reference trajectory of the output state at each time step. Thus, the terms depend only on \(\mathbf{x}_{0}\), \(\mathbf{r}_{0}\), or \(\widetilde{\mathbf{r}}\) are constant contribution to cost function \(J\) and can be ignored in the subsequent derivation. The modified cost function now becomes:  
\[\widetilde{J}=\frac{1}{2}\widetilde{\mathbf{x}}^{T}\widetilde{Q}_{1}\widetilde{\mathbf{ x}}-\widetilde{\mathbf{x}}^{T}\widetilde{Q}_{2}\widetilde{\mathbf{r}}+\frac{1}{2} \widetilde{\mathbf{u}}^{T}\widetilde{R}\widetilde{\mathbf{u}}. \tag{25}\]Replacing \(\widetilde{\mathbf{x}}\) in Equation (25) by Equation (23)  
\[\widetilde{J} =\frac{1}{2}\left(\widetilde{A}\mathbf{x}_{0}+\widetilde{B}\widetilde{ \mathbf{u}}\right)^{T}\widetilde{Q}_{1}\left(\widetilde{A}\mathbf{x}_{0}+\widetilde{B} \widetilde{\mathbf{u}}\right)-\left(\widetilde{A}\mathbf{x}_{0}+\widetilde{B}\widetilde{ \mathbf{u}}\right)^{T}\widetilde{Q}_{2}\widetilde{\mathbf{r}}+\frac{1}{2}\widetilde{ \mathbf{u}}^{T}\widetilde{R}\widetilde{\mathbf{u}} \tag{26}\] \[=\frac{1}{2}\widetilde{\mathbf{u}}^{T}\left(\widetilde{B}^{T} \widetilde{Q}_{1}\widetilde{B}+\widetilde{R}\right)\widetilde{\mathbf{u}}+\left[ \begin{array}{cc}\mathbf{x}_{0}^{T}&\widetilde{\mathbf{r}}^{T}\end{array} \right] \left[ \begin{array}{c}\widetilde{A}^{T}\widetilde{Q}_{1}\widetilde{B}\\ -\widetilde{Q}_{2}^{T}\widetilde{B}\end{array}