## 3 Control System Design  
### LQI Control Law Design  
The objective of the control system is to drive the robot from an initial position to follow the desired trajectory with a small tracking error, even in the presence of external disturbances and non-minimum phase phenomena. A servo control scheme with state feedback and an integral action is able to fulfill both requirements. The LQI is the servomechanism with optimal gain matrices that minimize the quadratic cost of the closed-loop control system. The cost function consists of the system states, integrator outputs, and control inputs with the corresponding coefficient matrices that can be easily tuned to reflect the weight of each element. Therefore, the LQI control system is designed based on the linearization of the system model in (9)-(11). Select a local equilibrium state at the robot that is unrotated and stabilized about the desired altitude \(\overline{z}_{b}\). It is easily seen that the robot has to inject a constant flowrate \(\widetilde{m}_{0}\) such that the thrust force is equal to the total gravitational force at this position. Meanwhile, the two weights in the weight-shifting mechanism remain in their original position. Additionally, the yaw motion is uncontrollable but stable. Therefore, by denoting the state vector as \(\mathbf{X_{b}}= \begin{bmatrix}x_{b}&y_{b}&z_{b}-\overline{z}_{b}&\varphi_{x}& \varphi_{y}&\dot{x}_{b}&\dot{y}_{b}&\dot{z}_{b}&\omega_{xb}&\omega_{yb}\end{bmatrix}  ^{T}\), with \(\varphi_{x}\) and \(\varphi_{y}\) angular positions about \(\mathbf{x_{b}}\)- and \(\mathbf{y_{b}}\)-axes, and the control input vector as \(\mathbf{U}= \begin{bmatrix}\dot{m}_{0}-\widetilde{m}_{0}&r_{wx}&r_{wy}\end{bmatrix}  ^{T}\), the system can be linearized as follows:  
\[ \begin{split}\dot{\mathbf{X_{b}}}=\mathbf{A}\mathbf{X_{b}}+\mathbf{B}\mathbf{U}+\mathbf{D },\\ \mathbf{Y_{b}}=\mathbf{C}\mathbf{X_{b}}=\begin{bmatrix}x_{b}&y_{b}&z_{b}- \overline{z}_{b}\end{bmatrix}^{T}\end{split}  \tag{12}\]  
The matrices \(\mathbf{A}\), \(\mathbf{B}\), and \(\mathbf{C}\) are given by:  
\[ \begin{split}\mathbf{A}=\begin{bmatrix}0&0&0&0&g&0&0&0& \frac{h\widetilde{m}_{0}}{a_{1}}\\ 0&0&0&-g&0&0&0&0&\frac{-h\widetilde{m}_{0}}{a_{1}}&0\\ 0&0&0&0&0&0&0&0&0\\ 0&0&0&\frac{m_{wx}h_{g}+m_{wy}h_{g}g}{\sigma_{1}}&0&0&\frac{-h \widetilde{m}_{0}\left(h^{2}+l^{2}\right)}{\sigma_{1}}&0&\frac{-\widetilde{m}_{0 }\left(h^{2}+l^{2}\right)}{\sigma_{2}}&0\\ 0&0&0&0&\frac{m_{wx}h_{g}+m_{wy}h_{g}g}{\sigma_{2}}&\frac{h \widetilde{m}_{0}}{\sigma_{2}}&0&0&0&\frac{-\widetilde{m}_{0}\left(h^{2}+l^ {2}\right)}{\sigma_{2}}\\ \mathbf{O}_{7\times 3}&&\end{bmatrix}\\ \mathbf{B}=\begin{bmatrix}2\left(\frac{1}{\rho A}+\frac{1}{4\rho} \right)\widetilde{m}_{0}&0&0\\ \overline{M}+m_{wx}+m_{wy}&0&0&-\frac{m_{wy}g}{I_{wx}+m_{wx}h_{x}^{2}+m_{wy} \left(\overline{z}_{y}^{2}+h_{y}^{2}\right)}\\ 0&0&\frac{m_{wx}g}{I_{wx}+m_{wy}h_{g}^{2}+m_{wx}\left(\overline{h}_{x}^{2}+ \overline{z}_{x}^{2}\right)}&0\\ \mathbf{C}=\begin{bmatrix}\mathbf{I}_{3}&\mathbf{O}_{3\times 7}\end{bmatrix}\end{split}  \tag{13}\]  
\(\mathbf{I}_{n}\) is the identity matrix size \(n\) and \(\mathbf{D}\) is the vector that contains the remaining nonlinear elements and unmodelled disturbances. Moreover, given a desired trajectory \(\mathbf{r}= \begin{bmatrix}X_{d}&Y_{d}&Z_{d}\end{bmatrix} ^{T}\) in the Earth-fixed frame, from the kinematic relationship in (1), the trajectory can be represented in the robot body frame as follows:  
\[\mathbf{r_{b}}= \begin{pmatrix}\mathbf{R_{b}^{c}}\end{pmatrix} ^{T}\mathbf{r}-\int\limits_ {0}^{t} \begin{pmatrix}\dot{\mathbf{R_{b}}}^{E}(\tau)\end{pmatrix} ^{T}\mathbf{r}(\tau)d\tau \tag{14}\]  
An augmented variable of tracking error is defined as follows:  
\[\dot{\mathbf{X_{i}}}=\mathbf{r_{b}}-\mathbf{Y_{b}} \tag{15}\]Then, the system model, in combination with the augmented variable, is written as:  
\[\underbrace{ \begin{bmatrix}\dot{\mathbf{X}}_{\mathbf{b}}\\ \dot{\mathbf{X}}_{i}\end{bmatrix} }_{\dot{\mathbf{X}}}=\underbrace{