# Discrete Integral Optimal Controller for Quadrotor Attitude Stabilization: Experimental Results
## 2 Control Strategy
### Discrete Model and Integral Control  
As mentioned previously, the mathematical model can be subdivided into four subsystems: subsystem \(z(k)\), \(\psi(k)\), \(x(k)-\theta(k)\) and \(y(k)-\phi(k)\). For subsystem \(z(k)\), we have:  
\[x_{5}(k+1) =Tx_{6}(k)+x_{5}(k),\] \[x_{6}(k+1) =T\bigg{[}\frac{u(k)}{m}\cos(x_{9}(k))\cos(x_{7}(k))-g\bigg{]}+x_ {6}(k)\]  
Now, we use an exact linearization as follows:  
\[u(k)=m(u_{1}(k)+g)(\cos(x_{9}(k))\cos(x_{7}(k)))^{-1},\]  
with these control laws for each subsystem, which are linear and are defined as:  
\[\left[ \begin{array}{c}x_{12}(k+1)\\ x_{2z}(k+1)\end{array} \right]=\underbrace{\left[ \begin{array}{cc}1&T\\ 0&1\end{array} \right]}_{\mathcal{A}_{z}}\left[ \begin{array}{c}x_{12}(k)\\ x_{2z}(k)\end{array} \right]+\underbrace{\left[ \begin{array}{c}0\\ T\end{array} \right]}_{\mathcal{B}_{z}}[u_{1}(k)],\]  
where \(x_{5}(k)\triangleq x_{1z}(k)\) and \(x_{6}(k)\triangleq x_{2z}(k)\). Define \(e_{12}(k)=x_{1z}(k)-x_{1 zR}(k)\), and \(e_{2z}(k)\triangleq x_{2z}(k)-x_{2 zR}(k)\); these are the errors for \(x_{1z}(k)\) and \(x_{2z}(k)\), respectively, and \(x_{1 zR}(k)\) and \(x_{2 zR}(k)\) are the references. The augmented system for the subsystem \(z(k)\) is:  
\[\xi_{z}(k+1)=\underbrace{\left[ \begin{array}{cc}A_{z}&0\\ C_{z}&I_{2}\end{array} \right]}_{\mathcal{A}_{z}}\xi_{z}(k)+\underbrace{\left[ \begin{array}{c}B_{z}\\ 0\end{array} \right]}_{\mathcal{B}_{z}}u_{1}(k)\]  
where \(\xi_{z}(k)=\left[x_{1z}(k)\ x_{2z}(k)\ \sigma_{1z}(k)\ \sigma_{2z}(k)\right]^{T}\) and \(\mathcal{C}_{z}=TI_{2}\). It is not a difficult task to verify that the pair \((\mathcal{A}_{z},\mathcal{B}_{z})\) is controllable in a finite number of steps. Define the following performance index:  
\[\mathcal{J}_{z}=\sum_{k=1}^{\infty}\ (\xi_{z}^{T}(k)\mathcal{Q}_{z}\xi_{z}(k)+u_{1} ^{2}(k)\mathcal{R}_{z}),\]  
where \(\mathcal{Q}_{z}\) is a semidefinite positive matrix of appropriate dimensions and \(\mathcal{R}_{z}\) is a real positive number. As the pair \((\mathcal{A}_{z},\mathcal{B}_{z})\) is controllable, there exists an unique solution to the Riccati equation given by:  
\[\mathcal{P}_{z}=\mathcal{A}_{z}^{T}\mathcal{P}_{z}\mathcal{A}_{z}-\mathcal{A}_ {z}^{T}\mathcal{P}_{z}\mathcal{B}_{z}(\mathcal{R}_{z}+\mathcal{B}_{z}^{T} \mathcal{P}_{z}\mathcal{B}_{z})^{-1}\mathcal{B}_{z}^{T}+\ \mathcal{Q}_{z},\]  
and the solution \(\mathcal{P}_{z}\) describes the optimal control for the subsystem \(z(k)\) given by:  
\[u_{1}^{*}(k)=-(\mathcal{R}_{z}+\mathcal{B}_{z}^{T}\mathcal{P}_{z}\mathcal{B}_ {z})^{-1}\mathcal{B}_{z}^{T}\mathcal{P}_{z}\mathcal{A}_{z}\xi_{z}^{*}(k).\]  
The discrete model for \(\psi(k)\) is given by:  
\[x_{11}(k+1) =Tx_{12}(k)+x_{11}(k),\] \[x_{12}(k+1) =T\tau_{\psi}(k)+x_{12}(k),\]  
Let \(x_{11}(k)\triangleq x_{1\psi}(k)\) and \(x_{12}(k)\triangleq x_{2\psi}(k)\); then, the subsystem \(\psi(k)\) can be rewritten as:  
\[\left[ \begin{array}{c}x_{1\psi}(k+1)\\ x_{2\psi}(k+1)\end{array} \right]=\underbrace{\left[ \begin{array}{cc}1&T\\ 0&1\end{array} \right]}_{\mathcal{A}_{\psi}}\left[ \begin{array}{c}x_{1\psi}(k )\\ x_{2\psi}(k)\end{array} \right]+\underbrace{\left[ \begin{array}{c}0\\ T\end{array} \right]}_{\mathcal{B}_{\psi}}\tau_{\psi}(k)\]  
Define \(e_{1\psi}(k)\triangleq x_{1\psi}(k)-x_{1\psi R}(k)\) and \(e_{2\psi}(k)\triangleq x_{2\psi}(k)-x_{2\psi R}(k)\), where \(x_{1\psi R}(k)\) and \(x_{2\psi R}(k)\) are the given references for the variables \(x_{1\psi}(k)\), \(x_{2\psi}(k)\), respectively. Then, the augmented vector for the subsystem \(\psi(k)\):\[\xi_{\psi(k)}(k)=[x_{1\psi}(k)\ \ x_{2\psi}(k)\ \ \ \sigma_{1\psi}(k)\ \ \ \sigma_{2\psi}(k)]^{T}.\]  
The space state representation of the augmented system is:  
\[\xi_{\psi(k)}(k+1)=\mathcal{A}_{\psi}\xi_{\psi}(k)+\mathcal{B}_{\psi}\tau_{ \psi}(k)\]  
where:  
\[\mathcal{A}_{\psi}=\left[ \begin{array}{cc}A_{\psi}&0\\ C_{\psi}&I_{2}\end{array} \right],\ \mathcal{B}_{\psi}=\left[ \begin{array}{cc}B_{\psi}\\ 0\end{array}