**Phase 1. Drone System Dynamics**
#

tip

If you wish to skip the theoretical and implementation details, please click here to jump to the summarized findings Ch. 4 - The Moving UAV.

## 1 System Dynamics Modeling #

Breaking down the drone into subsystems helps manage its complexity. The quadrotor has three main parts:

Actuation System: This handles the rotors’ model, the forces they generate ( \(F_p\) ), and the moments they create ( \(M_p\) ). Each rotor gets an input voltage ( \(V_{m_i}\) ) and produces an angular velocity ( \(\Omega_i\) ).

Movement System: This part deals with forces (not just from rotors but also external factors like wind) and moments that drive the drone’s movement. It uses physics laws, dynamics, and kinematics to determine the drone’s position and velocity.

Instrumentation System: This part involves the sensor model. In this project phase, the sensors are simplified as ideal sensors, meaning they have a gain of one.

Breaking it down this way helps in focusing on and understanding each aspect separately, making the overall understanding and development of the drone more manageable.

### 1.1. Actuation Subsystem #

The actuation subsystem involves individual DC motors powering propellers to generate lift forces in the drone’s four independently controlled motors (i=1,2,3,4). The values of I and \(\Omega\) change over time based on the applied motor voltage, Vm, while constants like Lm, Rm, Ke, Kt, and Jm characterize the motor. This results in a second-order system for each drone motor, expressed as:

\[\dot{\tilde{x}} = A\tilde{x} + B\tilde{u} = \begin{bmatrix} \dot{\tilde{I}}_i \\ \dot{\tilde{\Omega}} \end{bmatrix} = \begin{bmatrix} a_{11} & a_{12} \\ a_{21} & a_{22} \end{bmatrix} \begin{bmatrix} \tilde{I}_i \\ \Omega \end{bmatrix} + \begin{bmatrix} b_2 \\ 0 \end{bmatrix} \tilde{V}_m\] \[\tilde{y} = C\tilde{x} + D\tilde{u} = \Omega, \quad i = [0 \, 1]\]The parameters in matrices A and B are determined by the motor specifications and the chosen flight conditions for linearization.

#### 1.1.1. Model and Linearization #

The actuation subsystem dynamics are defined by the following nonlinear equations:

\[\dot{I}_i = \frac{1}{L_m} (V_{mi} - R_{m}I_{i} - K_{e}\Omega_{i})\] \[\dot{\Omega}_i = \frac{1}{J_m} (K_t I_i - Q_i - B_m\Omega_{i})\] \[\text{Where: } Q_i = K_Q \Omega_i^2\]To linearize this model, a nominal point around which small deviations are considered for the variables I, Vm and Ω is selected, representing the hovering condition:

\[I_i = I_o + \tilde{I}_i\] \[V_{mi} = V_{mo} + \tilde{V}_{mi}\] \[\Omega_i = \Omega_{i_o} + \tilde{\Omega}_{i}\]Using these relationships, the linearized dynamics are obtained as:

\[\dot{\tilde{I}}_i = \frac{1}{L_m} (\tilde{V}_{m_i} - R_m \tilde{I}_i - K_e \tilde{\Omega}_i)\] \[\dot{\tilde{\Omega}}_i = \frac{1}{J_m} (K_t \tilde{I}_{i} - 2 K_Q \Omega_{i_o} \tilde{\Omega}_i - B_m \tilde{\Omega}_i)\]The linearized state space model for each actuation subsystem, where the output is the motor angular speed, is given by:

\[\begin{bmatrix} \dot{I}_i \\ \dot{\Omega}_i \end{bmatrix} = \begin{bmatrix} -\frac{R_m}{L_m} & -\frac{K_e}{L_m} \\ \frac{K_t}{J_m} & -\frac{K_Q \Omega_{i_o}+B_m}{J_m} \end{bmatrix} \begin{bmatrix} \tilde{I_i} \\ \tilde{\Omega}_i \end{bmatrix} + \begin{bmatrix} \frac{1}{L_m} \\ 0 \\ \end{bmatrix} \tilde{V}_{mi}\]The state space variables are, therefore, \(\tilde{I}_i\) and \( \tilde{\Omega}_{i}\) . The only input provided to each motor is the applied voltage \( \tilde{V}_{m_{i}} \) . The state space model for the complete actuation subsystem can be obtained by combining the state space models for each motor into a single state space model:

\[ \begin{bmatrix} \dot{\tilde{I}}_1 \\ \dot{\tilde{\Omega}}_1 \\ \dot{\tilde{I}}_2 \\ \dot{\tilde{\Omega}}_2 \\ \dot{\tilde{I}}_3 \\ \dot{\tilde{\Omega}}_3 \\ \dot{\tilde{I}}_4 \\ \dot{\tilde{\Omega}}_4 \\ \end{bmatrix} = \begin{bmatrix} A_1 & 0 & 0 & 0 \\ 0 & A_2 & 0 & 0 \\ 0 & 0 & A_3 & 0 \\ 0 & 0 & 0 & A_4 \\ \end{bmatrix} \begin{bmatrix} \tilde{I}_1 \\ \tilde{\Omega}_1 \\ \tilde{I}_2 \\ \tilde{\Omega}_2 \\ \tilde{I}_3 \\ \tilde{\Omega}_3 \\ \tilde{I}_4 \\ \tilde{\Omega}_4 \\ \end{bmatrix} + \begin{bmatrix} B_1 & 0 & 0 & 0 \\ 0 & B_2 & 0 & 0 \\ 0 & 0 & B_3 & 0 \\ 0 & 0 & 0 & B_4 \\ \end{bmatrix} \begin{bmatrix} \dot{\tilde{V}}_{m1} \\ 0 \\ \dot{\tilde{V}}_{m2} \\ 0 \\ \dot{\tilde{V}}_{m3} \\ 0 \\ \dot{\tilde{V}}_{m4} \\ 0 \\ \end{bmatrix} \\\] \[\begin{bmatrix} \tilde{\Omega}_1 \\ \tilde{\Omega}_2 \\ \tilde{\Omega}_3 \\ \tilde{\Omega}_4 \\ \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} \tilde{I}_1 \\ \tilde{\Omega}_1 \\ \tilde{I}_2 \\ \tilde{\Omega}_2 \\ \tilde{I}_3 \\ \tilde{\Omega}_3 \\ \tilde{I}_4 \\ \tilde{\Omega}_4 \\ \end{bmatrix}\]Each matrix Ai and Bi represent a 2 × 2 matrix. As all four motors share identical parameters, the matrices Ai and Bi exhibit identical properties. However, due to the independent dynamics of each motor, the subsystem analysis simplifies. Subsequent sections will confirm the motors’ rapid response times, allowing an approximation using a static gain, as demonstrated:

\[\delta_i = K_{\Omega} \Omega_i\]With this outcome in consideration, the angular velocities \(\Omega_i\) can be presented solely as a function of the voltage constant \(K_{\Omega_{i}}\) and the actuations \(\delta_i\)

\[\begin{bmatrix} \Omega_1 \\ \Omega_2 \\ \Omega_3 \\ \Omega_4 \\ \end{bmatrix} = \begin{bmatrix} 1 & K_{\Omega} & 0 & 0 \\ 0 & 1 & K_{\Omega} & 0 \\ 0 & 0 & 1 & K_{\Omega} \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} \delta_1 \\ \delta_2 \\ \delta_3 \\ \delta_4 \\ \end{bmatrix} \\\]This process essentially proportionally adjusts the input variables \(\delta_i\) . Labeling the diagonal matrix as \(T_M\) , this relationship can be expressed by considering the linear deviations around the nominal operating point.

\[\tilde{\Omega} = T_M \tilde{\delta}\]#### 1.1.2. Transfer Function Model #

With the state space model acquired for the actuation subsystem, it is possible to derive the transfer function.

\[G(s) = C(sI - A)^{-1}B + D\]The relationship between the output (angular speed \(\tilde{\Omega}_i\) ) and the input (applied voltage \(\tilde{V}_{mi}\) ) is expressed as the transfer function:

\[G(s) = \frac{\tilde{\Omega}_i(s)}{\tilde{V}_{mi}(s)} = \begin{bmatrix} 0 & 1 \end{bmatrix} \begin{bmatrix} s + \frac{R_m}{L_m} & \frac{K_e}{L_m} \\ -\frac{K_t}{J_m} & s + \frac{2K_Q \Omega_{i_o} + B_m}{J_m} \end{bmatrix}^{-1} \begin{bmatrix} 1 \\ \frac{1}{L_m} \\ 0 \\ \end{bmatrix}\]When the aforementioned parameters are replaced with typical values for a DC motor, the resulting transfer function model can be represented.

\[G(s) = \frac{4.901 \times 10^6}{s^2 + 576.3s + 1.687 \times 10^5}\]#### 1.1.3. Stability #

To assess stability, the system poles need computation, achievable through two approaches. First, by determining the roots of the denominator the defined transfer function G(s), or second, by computing the eigenvalues of matrix A as depicted as follows:

\[\text{det}(sI - A) = s^2 + 576.3s + 1.687 \times 10^5 = 0 \quad \Leftrightarrow \quad s = -288.13 \pm 292.67i \quad (21)\]The poles, located in the left half-plane and forming complex conjugates, signify system stability. Expect an overshoot with a step input. The unit step response demonstrates a small overshoot ( \(M_p\) = 4.54%) and rapid system response ( \(T_s\) = 0.15s). Consequently, approximating the actuation subsystem as a static gain seems plausible. The root locus plot confirms system stability across various proportional controllers.

#### 1.1.4. Controllability #

The controllability of a state space model implies the capability to transition any initial state to a final state through a finite control action within a finite duration. This assurance exists when the rank of matrix C matches the dimension of the previously defined state vector, represented as n. For the actuation subsystem, n = 2, and matrix \(\mathcal{C}\) is thus expressed as follows. Upon observation, it’s evident that C holds a rank of 2, affirming the system’s controllability.

\[\mathcal{C} = [B \quad AB] = \begin{bmatrix} \frac{1}{Lm} & -\frac{Rm}{L^2m} \\ 0 & \frac{Kt}{Jm Lm} \\ \end{bmatrix}\]#### 1.1.5. Observability #

Observability of a state space model implies the ability to deduce any initial state from the system output within a given time interval. This certainty prevails when the rank of the observability matrix \(\mathcal{O}\) matches the dimension of the state vector.

Given the state vector’s dimension as n = 2, the observability matrix \(\mathcal{O}\) is defined according to equation 23. Upon inspection, it’s evident that the resulting matrix has a rank of 2, affirming the system’s observability.

\[\mathcal{O} = \begin{bmatrix} C \\ CA \end{bmatrix} = \begin{bmatrix} 0 & 1 \\ \frac{K_t}{J_m} & -\frac{B_m + 2K_q \Omega_o}{J_m} \\ \end{bmatrix} \]### 1.2. Movement Subsytem #

To understand the movement subsystem, it’s crucial to pinpoint the forces and moments within the system. The forces and moments are generated by the combined angular speeds of the rotors, as shown below:

\[T_i = K_T \Omega_i^2 \] \[Q_i = K_Q \Omega_i^2 \] \[F_p = \begin{bmatrix} 0 \\ 0 \\ - \sum_{i=1}^{4} T_i \\ \end{bmatrix}\] \[M_p = \begin{bmatrix} b \cdot \cos(\pi/4) \cdot (T_1 - T_2 - T_3 + T_4) \\ b \cdot \cos(\pi/4) \cdot (T_1 + T_2 - T_3 - T_4) \\ -Q_1 + Q_2 - Q_3 + Q_4 \\ \end{bmatrix}\]The equations for the dynamics and kinematics are the following:

\[m\dot{v} = -\omega \times mv + F_p + F_a + F_g \\ J\dot{\omega} = -\omega \times J\omega + M_p \\ \dot{p} = Rv\\ \dot{\Phi} = S\omega\]Considering the four angular velocities from the motors as inputs \(\tilde{u} = \tilde{\Omega} = [\tilde{\Omega}_1, \tilde{\Omega}_2, \tilde{\Omega}_3, \tilde{\Omega}_4]\) and the twelve variables denoting both the linear and angular positions and velocities as the state \(X = [\tilde{v}^T, \tilde{\omega}^T, \tilde{p}^T, \tilde{\Phi}^T]^T\) , the model is then represented:

\[\begin{bmatrix} \dot{\tilde{v}} \\ \dot{\tilde{\omega}} \\ \dot{\tilde{p}} \\ \dot{\tilde{\Phi}} \\ \end{bmatrix} = \begin{bmatrix} 0 & 0 & 0 & A_V \\ 0 & 0 & 0 & 0 \\ I_3 & 0 & 0 & 0 \\ 0 & I_3 & 0 & 0 \\ \end{bmatrix} \begin{bmatrix} \tilde{v} \\ \tilde{\omega} \\ \tilde{p} \\ \tilde{\Phi} \\ \end{bmatrix} + \begin{bmatrix} B_V \\ B_{\omega} \\ 0 \\ 0 \\ \end{bmatrix} \tilde{\Omega}\]Where Av, Bv, Bω are given by:

\[A_V = \begin{bmatrix} 0 & -g & 0 \\ g & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix}, \quad B_V = \begin{bmatrix} 0 & 0 & 0 & 0 \\ g & 0 & 0 & 0 \\ -b_z & -b_z & -b_z & -b_z \\ \end{bmatrix}, \quad B_{\omega} = \begin{bmatrix} 0 & -b_p & 0 & b_p \\ b_q & 0 & -b_q & 0 \\ -b_q & b_p & -b_p & b_p \\ \end{bmatrix}\]The coefficients for the matrix \(B_V\) and \(B_{\omega}\) depend on the parameters of the quad rotor and flight conditions. But combining previous equations, it is obtained:

\[\begin{bmatrix} \dot{\tilde{v}}_x \\ \dot{\tilde{v}}_y \\ \dot{\tilde{v}}_z \\ \dot{\tilde{\omega}}_x \\ \dot{\tilde{\omega}}_y \\ \dot{\tilde{\omega}}_z \\ \dot{\tilde{p}}_x \\ \dot{\tilde{p}}_y \\ \dot{\tilde{p}}_z \\ \dot{\tilde{\phi}} \\ \dot{\tilde{\theta}} \\ \dot{\tilde{\psi}} \\ \end{bmatrix} = \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & -g & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & g & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ \end{bmatrix} \begin{bmatrix} \tilde{v}_x \\ \tilde{v}_y \\ \tilde{v}_z \\ \tilde{\omega}_x \\ \tilde{\omega}_y \\ \tilde{\omega}_z \\ \tilde{p}_x \\ \tilde{p}_y \\ \tilde{p}_z \\ \tilde{\phi} \\ \tilde{\theta} \\ \tilde{\psi} \\ \end{bmatrix} +\\ \begin{bmatrix} 0 & 0 & 0 & 0 \\ g & 0 & 0 & 0 \\ -bz & -bz & -bz & -bz \\ 0 & -bp & 0 & bp \\ bq & 0 & -bq & 0 \\ -bq & bp & -bp & bp \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \end{bmatrix} \begin{bmatrix} \tilde{\Omega}_1 \\ \tilde{\Omega}_2 \\ \tilde{\Omega}_3 \\ \tilde{\Omega}_4 \\ \end{bmatrix}\]It is revelant to note that another possibility would be to divide the quad rotor dynamics into six subsystems, instead of a big matrix. Following up the state equation, the output would translate to:

\[y = \begin{bmatrix} 0 & 0 & I_3 & 0 \\ 0 & 0 & 0 & I_3 \\ \end{bmatrix} \begin{bmatrix} \tilde{v} \\ \tilde{\omega} \\ \tilde{p} \\ \tilde{\Phi} \\ \end{bmatrix} = \begin{bmatrix} \tilde{p} \\ \tilde{\Phi} \\ \end{bmatrix}\]Which would expand to:

\[\begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} \tilde{v}_x \\ \tilde{v}_y \\ \tilde{v}_z \\ \tilde{\omega}_x \\ \tilde{\omega}_y \\ \tilde{\omega}_z \\ \tilde{p}_x \\ \tilde{p}_y \\ \tilde{p}_z \\ \tilde{\phi} \\ \tilde{\theta} \\ \tilde{\psi} \\ \end{bmatrix} =\begin{bmatrix} \tilde{p}_x \\ \tilde{p}_y \\ \tilde{p}_z \\ \tilde{\phi} \\ \tilde{\theta} \\ \tilde{\psi} \\ \end{bmatrix}\]#### 1.2.1. Simplification and Analysis #

The analysis of the movement subsystem can be simplified by approximating the motor dynamics by a static gain. This means it is possible to change the matrix B such that the actuation variables \(\delta_i\) becomes the inputs instead of the motor angular speeds \(\Omega_i\) . This transformation is expressed:

\[\dot{X} = AX + B\tilde{\Omega} = AX + BT_M \tilde{\delta}\]The matrix \(T_M\) represents the voltage constant ( \(K\Omega\) ) as depicted previously. This adjustment does not alter the subsequent analysis; it merely scales the inputs. However, this transformation proves valuable for future use when implementing a controller.

#### 1.2.2. Stability #

The stability assessment involves determining the system’s poles, achieved by computing the eigenvalues of matrix A derived previously, encompassing the entire movement subsystem. Upon computation in Matlab, it reveals that all 12 eigenvalues, corresponding to the poles, are zero, indicating system instability.

#### 1.2.3. Controllability #

In order to assess the controllability of a state space model, it is necessary to compute its controllability matrix. When considering the entire movement subsystem, the controllability matrix \(\mathcal{C}\) is defined:

\[\mathcal{C} = \begin{bmatrix} B & AB & A^2B & A^3B & A^4B & A^5B & A^6B & A^7B & A^8B & A^9B & A^{10}B & A^{11}B \end{bmatrix}\]Upon conducting a rank computation of this matrix in Matlab, it is established that it holds a rank of 12. Consequently, indicating the controllability of the system.

#### 1.2.3. Observability #

When assessing observability, the rank of the observability matrix \(\mathcal{O}\) is computed. Considering the entirety of the movement subsystem, the matrix \(\mathcal{O}\) is represented as:

\[\mathcal{O} = \begin{bmatrix} C \\ CA \\ \vdots \\ CA^{11} \\ \end{bmatrix}\]This matrix has also rank 12 and, therefore, the system is observable.

## 2 State Feedback Control #

Previously, it was derived a model for the UAV, enabling to create linear and nonlinear simulators in Simulink. The next step is to implement a state feedback control system that allows the UAV to both follow and maintain a specified reference trajectory.

To effectively control the UAV, I aim to manage its lateral movement ( \(P_N\) and \(P_E\) ), vertical movement ( \(P_D\) ), and yaw angle ( \(\psi\) ). Thus, it is needed an adjustment of the matrix C to exclusively capture these controlled states in the output. Consequently, the updated matrix C will be tailored to reflect only these specific states, ensuring the control system targets the intended dynamics.

\[C =\begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix}\]Upon computing the resulting observability matrix, it was verified that its rank remained at 12, indicating the system’s sustained observability. Given the interest in tracking a specific reference while ensuring resilience against disturbances, a servo feedback system emerges as a promising choice. To achieve this, it necessitates employing the entire state vector in feedback, thereby requiring the ability to measure all 12 variables within the state vector. This measurement can be achieved through physical sensors or, alternatively, using observer techniques. For the current phase, it is assumed that reliable measurements of all system states are available.

A servo feedback control system exhibits the structure illustrated onwards. To actualize this closed-loop configuration, the values of K and \(K_i\) need computation. This process commences by deriving the augmented open-loop system.

\[\begin{bmatrix} \dot{x}(t) \\ \dot{x}_i(t) \\ \end{bmatrix} = \begin{bmatrix} A & 0 \\ -C & 0 \\ \end{bmatrix} \begin{bmatrix} x(t) \\ x_i(t) \\ \end{bmatrix} + \begin{bmatrix} B \\ 0 \\ \end{bmatrix} u(t) + \begin{bmatrix} 0 \\ 1 \\ \end{bmatrix} r\]\[y(t) = \begin{bmatrix} C & 0 \\ \end{bmatrix} \begin{bmatrix} x(t) \\ x_i(t) \\ \end{bmatrix} \bar{A} = \begin{bmatrix} A & 0 \\ -C & 0 \\ \end{bmatrix} , \bar{B} = \begin{bmatrix} B \\ 0 \\ \end{bmatrix} , \bar{C} = [C 0] \]Based on the previously obtained model, \(\bar{A}\) is a 16x16 matrix, \(\bar{B}\) is a 16x4 matrix and \(\bar{C}\) is a 4x16 matrix. The next step involves determining the gains K and \(k_i\) to design a controller ensuring the asymptotic stability of the system.

\[u(t) = - \begin{bmatrix} K & k_i \\ \end{bmatrix} \begin{bmatrix} x(t) \\ x_i(t) \\ \end{bmatrix} \begin{bmatrix} \dot{x}(t) \\ \dot{x}_i(t) \\ \end{bmatrix} =\begin{bmatrix} A - BK & Bk_i \\ -C & 0 \\ \end{bmatrix} \begin{bmatrix} x(t) \\ x_i(t) \\ \end{bmatrix} + \begin{bmatrix} 0 \\ 1 \\ \end{bmatrix} r\]To determine the entries of K and \(K_i\) , selecting the desired new pole locations was conducted through a trial-and-error approach. The aim was to achieve a rapid response while minimizing oscillations and conserving actuator energy. After identifying the new pole locations, the computation of K and \(K_i\) was performed using the MATLAB function ‘place’, which generates a 4x16 matrix \(\bar{K}\) enmpassing all the entries, from which a 4x12 matrix \(\bar{K}\) and a 4x4 matrix \(\bar{K}_i\) were subsequently derived.

\[\bar{K} = [K | K_i]\]The upcoming section will involve testing the behaviour of both the linear and nonlinear models when employing the acquired controller.

## 3 Simulation Results #

To assess system performance, two Simulink models were developed. The linear model represents the state-space model derived from a linearization that assumes a hovering condition where the drone’s weight is offset by the four propulsion forces. On the other hand, the nonlinear model, implements dynamic and kinematic equations alltogether.

Following an iterative process, desired poles were obtained through trial and error. These poles were crucial for computing the K and \(K_i\) matrices, enabling the construction of a controller for both models. This controller transformed them into servo systems with state feedback control. This section focuses on analyzing the performance of the servo feedback control in both simulators, along with evaluating the controller’s robustness against disturbances.

**MATLAB Simulink:** Linear Model - (click to expand)

**MATLAB Simulink:** Non-linear Model - (click to expand)

### 3.1. Linear Model Results #

As previously stated, the linear model was linearized under the assumption of a hovering state. In the forthcoming simulations, the model will undergo two combined movements: a vertical and yaw movement, as well as a vertical and lateral movement. Each simulation will present data on position, yaw angle, and actuation for the UAV.

#### 3.1.1. Vertical and Yaw Movement #

Combining the vertical climb with a yaw movement is equivalent to introducing a negative P \(D_{ref}\) alongside a specified final yaw angle value \(\Psi_{ref}\) .

**Vertical and Yaw Movement:** Movement of Z and Yaw - (click to expand)

It illustrates the drone adeptly tracking the given references without encountering any noticeable issues. However, the next figure indicates a minimal, yet inconsequential impact on the lateral position ( \(P_N\) and \(P_E\) ), noted as exceedingly small ( \(10^{-4}\) ).

**Vertical and Yaw Movement:** Movement of X and Y - (click to expand)

As anticipated, the activation of all four motors caused the UAV to ascend vertically. Notably, motors 1 and 3, as well as motors 2 and 4, exhibited varying actuations, leading to a variation in the yaw angle.

**Vertical and Yaw Movement:** Actuations - (click to expand)

#### 3.1.1. Vertical and Lateral Movement #

For the second movement, a decision was made to combine forward and vertical positioning for the drone. The following figure illustrates the UAV’s stabilization on the specified reference, although initial oscillations in the vertical position are evident. As predicted, the system eventually reaches the final position dictated by the reference, affirming the seamless functionality of the servo controller.

**Vertical and Lateral Movement:** Movement of X and Z - (click to expand)

Additionally, the next figure indicates a minimal PE movement ( \(10^{-3}\) ), aligning with expectations.

**Vertical and Lateral Movement:** Movement of Y - (click to expand)

The resulting actuation of the four motors can also be seen:

**Vertical and Lateral Movement:** Actuations - (click to expand)

### 3.2. Non-linear Model Results #

The nonlinear model exhibited increased instability during testing compared to the linear model. Some simulations encountered errors due to numerical overflow, caused by large pitch and roll angles attempting to track references.

This instability arose from significant pitch or roll angles inducing high speeds, leading to the loss of lift forces from the propellers and the drone’s descent. To focus on testing the servo controller, smaller references were used. Alternatively, employing ramp-like references or smaller steps instead of large singular steps may mitigate extreme responses in the nonlinear model.

#### 3.2.1. Vertical and Yaw Movement #

In this simulation, the given reference involves ascending while simultaneously rotating on the yaw angle. The upcoming figure illustrates the successful tracking of the reference without any issues.

**Vertical and Yaw Movement:** Movement of Z and yaw - (click to expand)

Similar to observations in the linear model, there was a minor impact on the lateral position, as depicted:

**Lateral Movement:** Movement of X and Y - (click to expand)

As anticipated, all motors were activated to elevate the altitude, with motors 2 and 4 exhibiting substantial actuation to align the yaw angle with the provided reference.

**Vertical and Lateral Movement:** Motor Actuations - (click to expand)

#### 3.2.2. Vertical and Lateral Movement #

In this scenario, the reference entails ascending while moving forward. It is shown the drone adeptly tracking the lateral reference, albeit with slight oscillations. However, despite stabilizing approximately 7 seconds later, the oscillations around the final altitude exceed acceptable limits.

**Vertical and Lateral Movement:** Movement of Z and X - (click to expand)

**Lateral Movement:** Movement of Y - (click to expand)

**Vertical and Lateral Movement:** Motor Actuations - (click to expand)

Upon reviewing the actuation of the four motors, it’s evident that the actuation exhibited chaotic behaviour. This suggests that the controller values require correction and fine-tuning to enable smoother tracking of the provided reference, eliminating substantial oscillations.

### 3.3. Robustness to Disturbances #

In this section, the focus lies on evaluating the system’s response to disturbances. Specifically, the goal is to test its resilience against input disturbances. These disturbances entail unexpected alterations in the control action for select motors. This situation compels the servo control system to adapt and uphold the designated reference position and yaw angle.

- Yaw Compensation - Motors 2 and 4
- Altitude Compensation - All Motors

## 4 The Moving UAV #

This section aimed to derive and analyze a realistic quadcopter model. Initially, it involved establishing the dynamics of two subsystems: actuation (comprising four motors and propellers) and movement (relating actuation forces to drone dynamics and kinematics).

Once the complete model was obtained, it was initially simplified with a static gain assumption in the actuation subsystem. Subsequent analysis involved studying stability, observability, and controllability. The model, despite lacking stability, proved to be both observable and controllable, indicating feasibility for implementing a feedback control system.

The model’s characteristics indicated the need to control three coordinates and the yaw angle for movement regulation. Consequently, a servo feedback control system was developed and tested using both linear and nonlinear models. While the controller allowed the model to track given references, outcomes varied—excessive oscillations and slower response times were evident, notably in the nonlinear model.

The nonlinear simulation revealed challenges where large references risked crashing the simulator, emphasizing the criticality of refining reference signal design. Solutions might involve providing smaller sequential references or constraining maximum pitch and roll angles.

These observations underscored the need for controller fine-tuning to enhance reference tracking performance. Designing a controller capable of concurrently tracking all four references without excessive oscillations and with rapid response times emerged as a significant challenge.

In conclusion, the developed models exhibited accuracy and reliability in predicting real drone behaviour. Furthermore, they serve as a foundation for designing more effective controllers to optimize drone performance.

tip

Continuous Controller Design