Phase 2. Controller Design Pt. 1 #

tip

If you wish to skip the theoretical and implementation details, please click here to jump to the summarized findings Ch. 9 - Piloting in a Loop.

1 Continuing from Phase 1… #

Previously, I had addressed problem formulation and initial approaches. The primary tasks included studying the system and subsystems, creating a state-space model for the dynamical system, analyzing aspects like linearity, stability, controllability, and observability, developing Simulink models, proposing classical state feedback control, and validating the simulator for the developed models.

info

Following the delivery of Phase I, issues were identified in the drone controller. Although functional, some minor problems remained unresolved. The movement subsystem had initially been modeled as a 12th order system, established as linearizable, controllable, and observable. In the upcoming work, the decision was made to break down the movement system into four subsystems: vertical, lateral, longitudinal axis movement, and yaw subsystem.

The primary objective is to formulate and address optimal control and estimation problems. It is intended to compare outcomes from the pole placement controller and the Luenberger observer with those achieved using the optimal controller (Linear Quadratic Regulator) and optimal observer (Kalman-Bucy observer).

2 Actuation and Sensors #

Before commencing the formulation and implementation of the required controllers and estimators, there are preliminary details that necessitate consideration.

One of the initial considerations revolves around the actuation, specifically the voltages applied to each motor. It was established during the project’s initial phase that the model was linearized under a hovering condition. From this state, the computation of the required equilibrium motor actuation was derived.

In the context of the linear model, a null actuation maintains the system in a hovering state, implying no resultant motion. This characteristic arises from the actuation being a deviation from the nominal value rather than the actual voltage applied to the motors. To prevent motor saturation, it’s crucial to adhere to the following voltage deviation constraints:

\[\Delta V_{\text{max}} = V_{\text{bat}} - V_0 = 11.1 - 7.9 = 3.2 \, \text{V}\] \[\Delta V_{\text{min}} = V_0 = 7.9 \, \text{V}\]

In the non-linear model, maintaining the hovering condition necessitates applying V0 to all motors. However, the controllers are formulated based on the linear model. Therefore, the voltage offset should only be added after the controllers compute the necessary actuation. As a result, the maximum and minimum voltage deviations align with those of the linear model. Another crucial aspect involves the design of observers. Understanding which state variables lack direct physical measurement is essential, as these variables demand estimation using an observer.

For the drone in question, only the position (Px, Py, and Pz) and orientation ( \(\phi, \theta, \text{and} \psi\) ) are directly measurable. Consequently, the estimation of linear velocity ( \(\dot{P}_x, \dot{P}_y \text{and} \dot{P}_z \) ) and angular velocity ( \(\dot{\phi}, \dot{\theta} \text{and} \dot{\psi} \) ) becomes necessary.

Additionally, it was indicated that sensor noise would impact the measured signals, with the noise variance specified for each sensor.

It was suggested that the noise issue could potentially be addressed by employing the Kalman-Bucy estimator in place of the Luenberger observer.

3 Servo Feedback Control Via Pole Placement #

The objective of stabilizing the system while enabling reference tracking necessitates the implementation of a servo feedback control system. This entails designing the matrices K and ki, as depicted:

Before computing the controller parameters, the selection of desired poles becomes crucial for modifying the system dynamics. As outlined previously, the model can be separated into four distinct subsystems, each with poles located at the origin. Furthermore, it was demonstrated that all these subsystems are controllable.

Specifically, the vertical axis and yaw subsystems are both represented as second-order systems and follow the structure defined in equation 4.

The augmented system is, therefore, given by:

By employing Ackermann’s Formula through the MATLAB function “acker” and selecting all poles at -5, the controller gains for the vertical axis and yaw subsystems are provided by:

The remaining two subsystems, the lateral and longitudinal subsystems, are composed of two second-order systems each (roll and pitch, respectively). Consequently, these subsystems are both fourth-order systems and follow the structure defined:

The resultant augmented system is: \[\begin{bmatrix} \dot{x}_1(t) \\ \dot{x}_2(t) \\ \dot{x}_3(t) \\ \dot{x}_4(t) \\ \dot{x}_i(t) \\ \end{bmatrix} = \begin{bmatrix} 0 & 0 & 0 & 0 & 0 \\ 1 & 0 & 0 & 0 & 0 \\ 0 & Q_{32} & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & -1 & 0 \\ \end{bmatrix} \begin{bmatrix} x_1(t) \\ x_2(t) \\ x_3(t) \\ x_4(t) \\ x_i(t) \\ \end{bmatrix} + \begin{bmatrix} b \\ 0 \\ 0 \\ 0 \\ 0 \\ \end{bmatrix} u(t)\]

Through Ackermann’s Formula and after a trial-and-error process, selecting all poles at -5, the controller gains for the lateral axis and longitudinal axis subsystems are represented: \[\bar{K}_{\text{lateral}} = [1.49, 14.90, 7.59, 18.99, -18.99]\]

4 Luanberger Observer #

As previously mentioned, the onboard sensors can only physically measure position and orientation. Consequently, the estimation of linear and angular velocities within the subsystems’ state vectors becomes necessary.

The fundamental concept behind an estimator involves creating a numerical simulator of the physical system, receiving actuation as input, and producing the state space vector estimation as output. However, this approach alone isn’t sufficient. The estimation error can asymptotically vanish if the state is unstable, and the convergence velocity of the error is limited by the system’s velocity.

To address this, the implementation of a Luenberger observer is essential. As depicted in the following figure, the adjustment of observer performance can be achieved by tuning the parameters within matrix L.

To derive the Luenberger observer, it’s crucial to select the desired poles ensuring that the observer operates faster than the system dynamics without excessively amplifying modeling errors. In this scenario, for this system, a choice was made to position the poles 10 times faster than those of the controller, thus setting all poles at -50. Additionally, it’s pertinent to note that, as previously explained in the initial phase, the system is observable.

Utilizing Ackermann’s formula, specifically via the MATLAB function “acker”, the matrix L for the designed Luenberger observer for the vertical, yaw, lateral, and longitudinal subsystems can be described in the following equations:

5 Linear Quadratic Regulator #

The pole placement method demonstrated in the previous sections yielded satisfactory outcomes. However, it poses challenges in tuning the controller, becoming increasingly complex as the system’s order increases. Additionally, it complicates the task of finding a compromise between conflicting objectives when designing a controller.

In light of this, the focus now shifts to considering an optimal controller that takes into account conflicting goals and strategically places poles in an optimal manner. Specifically, the design will involve a linear quadratic regulator (LQR). In designing an optimal controller, the usual objective is to minimize energy consumption (represented by the control action) while maximizing the distance traveled—objectives that clearly conflict.

The initial step involves selecting a performance index J that reflects these conflicting objectives. In this case, the performance index specified by the following equation is utilized, aiming to be minimized.

Certainly, in a manner akin to PID controller parameters, the matrices Q (where Q ≥ 0) and R (where R > 0) play a pivotal role. They contribute to the behaviour of the system and need to be tuned to achieve the desired system response. Augmenting Q results in faster convergence of states toward the reference, while increasing R penalizes larger control inputs. The balance between performance and energy expended is thus represented by the relative values assigned to both matrices.

In an optimal controller, the controller gains aren’t constant over time. However, it’s more common to design a sub-optimal controller where K is a constant value. Adhering to this approach, the computation of matrix K is outlined:

tip

Matrix P is the solution of the algebraic Riccati equation.

It was mentioned that, in pursuit of tracking a reference rather than merely maintaining states at zero, the MATLAB function “lqi” (linear quadratic integrator) becomes crucial as it computes both K and ki values.

The success in achieving desired performance largely hinges on the choice of relative values within Q and R matrices. Specifically, emphasis was placed on the need to tune diagonal values, allowing for certain values to be set to zero if controlling a particular state isn’t deemed necessary.

Regarding the yaw angle and vertical axis subsystems, considering their augmented third-order nature, the prescribed values for Q and R were specified through equations:

In regard to the longitudinal axis and lateral axis subsystems, the augmented systems are identified as fifth-order. The designated values for Q and R are specifically detailed:

6 Kalman-Bucy Observer #

It was highlighted earlier that the system’s onboard sensors aren’t ideal and will inevitably introduce noise into the measurements. This noise can significantly impact the system’s performance, potentially leading to errors when following a reference and even instability in the drone’s motion, particularly in the more sensitive nonlinear system, susceptible to actuation oscillations.

There are two distinct noise types—process noise (v(t)) and sensor noise (w(t)), both assumed to be zero-mean white noise with known variance.

Due to the presence of noise in the measured values, the accuracy of the implemented Luenberger observer in estimating the state space variables diminishes. To counteract this, the Kalman-Bucy observer will be utilized.

The process of tuning the Kalman filter gains in matrix Lk involves adjusting Q0 and R0 values. The known variance values from the sensor noise were allocated to R0.

Q0 values were determined via a trial-and-error approach until the noise impact was minimized. The resulting values for the yaw, vertical, longitudinal, and lateral subsystems are articulated in equations:

\[Q_0^\text{yaw} = 10^{-6}\] \[Q_0^\text{vertical} = 10^{-6}\] \[Q_0^\text{longitudinal} = 10^{-10}\] \[Q_0^\text{lateral} = 10^{-10}\]

Combining the developed LQR with the Kalman-Bucy observer results the Linear Quadratic Gaussian.

It’s crucial to note that, similar to the LQR, the relative values assigned to Q0 and R0 significantly impact the observer’s performance. Consequently, the small values in Q0 correspond to the small values of sensor noise variances present in R0.

Additionally, it’s important to acknowledge that despite my best effort, successfully eliminating the noise proved challenging. This difficulty persisted even after attempting various values for the Kalman filter parameters. The impact of these results, particularly evident in the nonlinear simulation, is thoroughly discussed in the simulation results.

7 Simulation Results #

I conducted comprehensive testing of the implemented controllers (via pole placement and LQR) and observers (Luenberger and Kalman-Bucy) using both the linear simulator developed in the initial phase and the full nonlinear simulator provided in the subsequent phase.

warning

Two primary tests were conducted to evaluate performance:

• 1. Step Reference Test: This involved applying step changes to the four references (Px, Py, Pz, and ψ). The objective was to assess performance in a complex scenario where the controller needed to combine four actuations to track a four-degree-of-freedom motion.
• 2. Ramp Reference Test: A series of ramps was utilized to create a simpler yet realistic trajectory for the drone to follow.

Both tests were executed in both simulators, comparing results with and without noise injected into the sensors. The Simulink block “Band-Limited White Noise” was employed by the group to implement the noise. The aim was to comprehensively evaluate the controllers’ and observers’ performance under various scenarios and noise conditions using different trajectory types. This allowed for a robust assessment of their effectiveness and robustness in real-world scenarios.

7.1. Linear Model #

The used steps provide the following references to the system:

Using the pole placement controller and the Luenberger observer without noise in the measurements results:

Ensuring that the roll and pitch angles remain within acceptable ranges is crucial, as discussed in the initial phase. Large values of these angles could potentially lead the drone to lose all vertical propulsion forces. In the nonlinear simulator, this scenario might cause the drone to descend rapidly and crash the simulation.

Observing the following figure allowed verification that the roll and pitch angles were maintained at acceptable levels, preventing potential issues that could result in the loss of vertical propulsion forces and subsequent simulation crashes. This detail is particularly critical in maintaining stable flight conditions and avoiding adverse consequences due to extreme pitch and roll angles.

Pitch and Roll via using Pole placement Controller and Luenberger observer - (click to expand)

Verifying that the actuation did not saturate:

Actuation of each motor using the Pole placement Controller and Luenberger observer - (click to expand)

In the subsequent test, similar to the previous one, the LQR controller was employed instead of the pole placement controller. However, the observer used remained the Luenberger observer, as no noise was introduced to the measurements.

Response of the states following the step values with LQR and a Luenberger observer - (click to expand)

Verifying again that the pitch and roll angles stay within reasonable values:

Pitch and Roll using the LQR formula and Luenberger observer - (click to expand)

Actuation for the LQR and Luenberger observer - (click to expand)

info

Observing the actuation, it’s apparent that not only does the actuation avoid reaching saturation values, but it also remains lower compared to the values attained using the pole placement controller. This outcome was anticipated since the LQR operates as an optimal controller, balancing system performance with energy consumption.

Moving forward, the analysis focuses on the system’s behaviour when noise affects the measurements provided by the sensors. As previously discussed, the Luenberger estimator generates estimations with errors and becomes impractical when noise corrupts the measurements. To address this challenge, the developed Kalman-Bucy observer is utilized. T

info

Kalman filter effectively reduces most of the noise from all measured signals, while the Luenberger estimator follows the corrupted signal. This emphasizes the superiority of the Kalman-Bucy observer in mitigating noise-induced errors compared to the Luenberger estimator.

The sensors and noise estimations can be evaluated:

Now, performing the identical test where the four steps serve as references, yet employing the LQR with the Kalman-Bucy observer (Linear Quadratic Gaussian), the results depicted as follows confirm our expectations. The LQG successfully tracks the references even in the presence of noise in the measurements. This reaffirms the robustness and effectiveness of the LQG controller coupled with the Kalman-Bucy observer in handling noisy measurements while accurately following the given references.

Step for LQG - (click to expand)

Pitch and Roll for LQG - (click to expand)

Actuation for LQG - (click to expand)

7.2. Nonlinear Model #

Using the same steps of the previous section and using the pole placement controller with the Luenberger observer:

Actuation of each motor using the pole placemente controller and a Luenberger observer - (click to expand)

Pitch and Roll using the pole placement controller and Luenberger observer - (click to expand)

Roll and pitch angles are acceptable.

Actuation of each motor using the pole placement controller and the Luenberger observer - (click to expand)

Actuation did not saturate.

Using now the LQR with the Luenberger estimator, the reference tracking results:

Position tracking with LQR and Luenberger observer - (click to expand)

Pitch and Roll using the LQR and Luenberger observer - (click to expand)

Roll and pitch angles are acceptable.

Actuation of LQR and Luenberger observer - (click to expand)

Actuation did not saturate.

Having shown in the previous section that when noise is present, the Kalman-Bucy observer is used, the same test using the Linear Quadratic Gaussian will be assessed.

Step with LQG - (click to expand)

Pitch and Roll with LQG - (click to expand)

Again, the roll and pitch angles are reasonable. Notice that the absolute maximum value for both pitch and roll is around 0.03 rad (1.71 degrees).

Actuation with LQR and Kalman-Bucy - (click to expand)

Actuation did not saturate.

7.3. Remarks #

From the comprehensive results obtained from both linear and nonlinear models, it can be concluded that all controllers and observers were correctly implemented and exhibited their intended functionality.

An essential finding across both models was the significance of using the Kalman-Bucy observer when noise significantly impacts sensor readings. This observer proved crucial in handling noisy measurements effectively.

Careful consideration of roll and pitch angles was emphasized due to their potential impact on causing rapid altitude loss in the nonlinear simulation. Numerous instability issues were encountered, especially when step values were too large, resulting in aggressive actuation and subsequent system instability. To mitigate these issues, alternative reference signals were introduced to reduce initial error and consequent motor actuation.

note

Addressing actuation saturation and avoiding extreme pitch and roll angles can be achieved through various methods. One such solution involves implementing a saturation block on motors to ensure voltages stay within permissible limits. However, a straightforward saturation approach on individual motors may hinder the drone from following the desired trajectory. To resolve this, a strategy of limiting all actuations whenever one motor saturates allows the drone to continually attempt trajectory following without losing control or erratic behavior. This approach ensures a more controlled response in scenarios where actuation saturation or extreme angles could destabilize the system.

8 Trajectory Following #

After successfully testing the system with step references, I implemented a trajectory using a series of ramps to guide the drone’s trajectory, aiming to minimize initial error. This trajectory, designed to trace a “8” shape with the drone landing at its initial take-off position, underwent testing in both the linear and nonlinear models.

To determine the maximum slope of the reference ramps, a trial-and-error process was conducted using the nonlinear model. By gradually increasing the slopes of the ramps (for the four references) until the drone became uncontrollable. Moreover, it was noted that the maximum pitch and roll angles remained below 0.6 radians (approximately 35 degrees).

Similar to the steps test, I was interested in evaluating trajectory tracking accuracy, assessing steep roll and pitch angles, and identifying any actuation saturation during the ramp trajectory test. Considering previous conclusions, I opted to use the Linear Quadratic Regulator without noise and the Linear Quadratic Gaussian when noise was introduced to the sensors. This choice allows for a comprehensive evaluation of trajectory tracking performance and system stability under different conditions, ensuring the suitability of the controllers and observers for both noise-free and noisy sensor scenarios.

8.1. Linear Model #

As it can be seen for the reference tracking of the drone, the results are satisfactory and the control system was able to follow the entire trajectory with no issues.

Reference and response along the trajectory - (click to expand)

Actuation of each motor along the trajectory of the linear system - (click to expand)

Actuation did not saturate.

Response of the angles along the trajectory of the Linear system - (click to expand)

The roll and pitch angles are reasonable.

example

I also created a 3D plot of the trajectory flown by the drone for the linear system. Plot is seen in the following figure:

warning

It was previously established that when sensor noise is present, employing the Kalman-Bucy observer becomes essential. The next figure demonstrates that without the Kalman filter, the drone’s motion becomes erratic, showcasing evident actuation saturation issues.

Logically, adding the Kalman filter to tackle the noise, the results improve.

Reference and response along the trajectory - (click to expand)

Actuation of each motor along the trajectory - (click to expand)

Actuation did not saturate.

Pitch and roll along the trajectory - (click to expand)

The roll and pitch angles are reasonable.

example

Aside from the 3D plot of the trajectory, I showcase a zoomed section showcasing the bits of oscillation due to noise.

8.2. Nonlinear Model #

Following a similar strategy as in ch. 8.1. Linear Model:

Reference and response along the trajectory - (click to expand)

Actuation of each motor along the trajectory - (click to expand)

Actuation did not saturate.

Response of the angles along the trajectory - (click to expand)

The roll and pitch angles are reasonable.

example

I also created a 3D plot of the trajectory flown by the drone for the nonlinear system. Plot is seen in the following figure:

Adding noise to the sensor measurements and using the Kalman-Bucy observer, the following results are obtained:

Reference and response along the trajectory - (click to expand)

Actuation of each motor along the trajectory - (click to expand)

Pitch and roll along the trajectory - (click to expand)

example

Aside from the 3D plot of the trajectory, I showcase a zoomed section showcasing the bits of oscillation due to noise.

9 Piloting in a Loop #

This work section aimed to design and test controllers and observers crucial for drone control.

Initially, by designing a controller using the pole placement method, employing Ackermann’s Formula. Simultaneously, a Luenberger observer was tailored to estimate variables unmeasurable by physical means, aligning its dynamics to be faster than the controller.

Subsequently, an optimal controller and observer were developed, specifically the Linear Quadratic Regulator (LQR) and the Kalman-Bucy observer. The LQR demonstrated efficient reference tracking with reduced actuation, minimizing energy consumption. However, with the Kalman filter, despite attempts to tune it, complete noise removal wasn’t achieved, especially in the sensitive nonlinear simulator, affecting responses to step references.

Nonetheless, the Kalman filter proved beneficial in noise-affected sensor scenarios, although further tuning is intended to improve its performance.

In addition, it was also introduced a solution using refined reference signals, enhancing trajectory tracking and stability compared to step references.

tip

Discrete Controller Design