Quadrotor Altitude Control Lab

Robotics & Automation – Fall 2025
Instructor: Dr. Gerardo Flores

This lab is about one simple idea:

Teach a drone to go up and stay where you tell it.

You will:

• Run an altitude controller (Python or Webots).
• Add either noise or a disturbance.
• Try at least two control laws (P, PD, cubic, sign).
• Use what you see to answer the Questionnaire.pdf.

You can complete the lab using Python only or Webots only. You do not have to do both, but you are encouraged to try both if you have time.

---

0. Files in this lab

You should have these files:

• Instructions.pdf – formal lab instructions.
• Questionnaire.pdf – rubric-style questions you will submit.
• 1-control-UAV-altitude.py – Python altitude simulation.
• Webots controller file (for the DJI Mavic 2 Pro):
  • something like mavic2_controller.c (the C file opened by the drone controller in Webots).

If your Webots controller file name is slightly different, that is fine. The structure and code inside will look the same as shown in class.

---

1. Concept in plain language

The drone only moves up and down in this lab.

• z = how high the drone is (altitude).
• vz = how fast it is going up or down (vertical velocity).
• thrust = how hard the propellers push up.
• mg = gravity pulling down.

The core physics is:

• If thrust > mg → drone accelerates up.
• If thrust < mg → drone accelerates down.
• If thrust = mg → drone hovers.

Your controller's job is to choose thrust so that z follows a desired height z_ref.

You will also see:

• Noise: fake sensor jitter.
• Disturbance: fake wind that pushes the drone.

---

2. Path A – Python simulation (1-control-UAV-altitude.py)

This is a 1D simulation that runs quickly and shows plots.

2.1. Run the base code

1. Make sure you have Python 3 and matplotlib installed.
2. In a terminal:

   python 1-control-UAV-altitude.py

3. You should see two plots:

   • Altitude vs time: z(t) and z_ref(t).
   • Thrust vs time: T(t).

Questions to ask yourself:

• Does the altitude reach the reference?
• Is there overshoot?
• Does it settle smoothly or oscillate?

This run corresponds to Q1 and Q2 in the questionnaire: you are confirming that the base experiment runs.

---

2.2. Find the key parts of the Python code

Open 1-control-UAV-altitude.py and locate:

1. Dynamics:

   def quad_altitude_dynamics(state, thrust, t, params):
       z, vz = state
       m = params["m"]
       g = params["g"]
   
       # --- DISTURBANCE / NOISE HERE ---
       #noise = 0.5 * (2*np.random.rand() - 1.0)   # uniform in [-0.5, 0.5]
       # ---------------------------------
   
       # Time-dependent disturbance
       #A = 1.0      # amplitude
       #w = 2.0      # frequency [rad/s]
       #disturbance = A * np.sin(w * t)
   
       dz = vz
       dvz = (thrust - m*g) / m
   
       return np.array([dz, dvz])

2. Controller:

   def altitude_pd_controller(state, ref, params):
       z, vz = state
       z_ref, vz_ref = ref
   
       kp = params["kp"]
       kd = params["kd"]
       m = params["m"]
       g = params["g"]
   
       # tracking errors
       e_z = z - z_ref
       e_vz = vz - vz_ref
   
       # P and PD Control {desired acceleration}
       a_des = - 50*kp * e_z - 50*kd * e_vz
   
       # Controller [required thrust]
       thrust = a_des
       return thrust

You will make your changes only in these two functions.

---

2.3. Step 1 – Add NOISE or DISTURBANCE (choose one)

You only need one of these for the lab.

Option A – Measurement noise

Inside quad_altitude_dynamics, uncomment the noise line and add it to dvz:

# --- DISTURBANCE / NOISE HERE ---
noise = 0.5 * (2*np.random.rand() - 1.0)   # uniform in [-0.5, 0.5]
# ---------------------------------

# Time-dependent disturbance
#A = 1.0      # amplitude
#w = 2.0      # frequency [rad/s]
#disturbance = A * np.sin(w * t)

dz = vz
dvz = (thrust - m*g) / m + noise

What this means:

• The acceleration is now slightly random at every time step.
• The controller will see the result in the altitude response.

Option B – Sinusoidal disturbance

Instead of noise, use a sinusoidal disturbance:

# --- DISTURBANCE / NOISE HERE ---
#noise = 0.5 * (2*np.random.rand() - 1.0)   # uniform in [-0.5, 0.5]
# ---------------------------------

# Time-dependent disturbance
A = 1.0      # amplitude
w = 2.0      # frequency [rad/s]
disturbance = A * np.sin(w * t)

dz = vz
dvz = (thrust - m*g) / m + disturbance

This is like a repeating “wind” that pushes the drone up and down.

After you make the change, run the script again and look at:

• How does z(t) differ from the clean case?
• Does the drone still reach the reference?
• Does it oscillate more?

This connects to Q5 (effect of noise/disturbance) and Q3 (analysis of response).

---

2.4. Step 2 – Try at least two control laws

Now modify altitude_pd_controller.

Option 1 – P control

Replace the a_des line with:

a_des = -kp * e_z

This gives:

• More error → stronger push.
• No derivative term.

Run and observe:

• Does it overshoot?
• Does it oscillate more or less?
• How long until it settles?

Option 2 – PD control

Use both error and derivative:

a_des = -kp * e_z - kd * e_vz

Run and compare with pure P:

• Is the response faster?
• Is there less overshoot?
• Does noise in e_vz create jitter in the thrust plot?

Option 3 – Cubic control

Use error cubed:

a_des = -kp * (e_z**3)

Interpretation:

• Small errors → very small control (soft near the target).
• Large errors → strong correction.

Check:

• Does this reduce chattering near the reference?
• Does it handle large initial error well?

Option 4 – Sign-based control

Use only the sign of the error:

a_des = -kp * np.sign(e_z)

This is like “full up” or “full down” only.

Look for:

• Very fast correction.
• Strong chattering around the setpoint.

Pick at least two of these and compare.

This supports Q4, Q6, and Q7.

---

3. Path B – Webots (DJI Mavic 2 Pro controller)

If you choose the Webots path, you will edit the altitude controller inside the C file that controls the drone in the Mavic2 world.

3.1. Open the controller file

In Webots:

1. Open the DJI Mavic 2 Pro world.
2. Right-click the drone robot → “Open controller”.
3. You should see code like this near the bottom:

const double clamped_difference_altitude =
  CLAMP(target_altitude - altitude + k_vertical_offset, -1.0, 1.0);
const double vertical_input =
  k_vertical_p * pow(clamped_difference_altitude, 3.0);

Here:

• target_altitude is your reference.
• altitude is the measured height.
• clamped_difference_altitude is the error with saturation and offset.
• vertical_input plays the role of "extra thrust" (like desired acceleration or control effort).

This is the cubic altitude controller.

---

3.2. Step 1 – Add noise or disturbance to altitude

You do not have to do both. One is enough.

Right before the controller, you can build modified altitude signals.

Example structure:

// Clean altitude
double altitude_measured = altitude;

// Option A: measurement noise
// double r = (double)rand() / (double)RAND_MAX;
// double noise = 0.2 * (2.0 * r - 1.0);
// altitude_measured = altitude + noise;

// Option B: sinusoidal disturbance
// double disturbance = 0.3 * sin(2.0 * M_PI * 1.0 * time);
// altitude_measured = altitude + disturbance;

• Uncomment only one of the options.
• Replace altitude with altitude_measured in the controller:

const double clamped_difference_altitude =
  CLAMP(target_altitude - altitude_measured + k_vertical_offset, -1.0, 1.0);

Run the simulation and watch how the drone’s altitude changes.

---

3.3. Step 2 – Try different control laws in Webots

In the same place, you can change the control law.

The default is cubic:

const double vertical_input =
  k_vertical_p * pow(clamped_difference_altitude, 3.0);

Try the following options (one at a time):

P control

const double vertical_input =
  k_vertical_p * clamped_difference_altitude;

Cubic control (original)

const double vertical_input =
  k_vertical_p * pow(clamped_difference_altitude, 3.0);

Sign-based control

const double vertical_input =
  k_vertical_p * (double)SIGN(clamped_difference_altitude);

Observe:

• How smooth is the altitude?
• Is there overshoot?
• Does the drone chatter (rapid oscillation)?

You can also adjust k_vertical_p to see the effect of gain.

---

4. How to think like an engineer while you watch plots

As you run different cases, try to label what you see using control vocabulary:

• Overshoot: The altitude goes above the reference before settling.
• Rise time: How quickly it reaches the reference level.
• Settling time: How long until it stays near the reference.
• Damping: Does it smoothly approach the reference or oscillate?
• Steady-state error: Does it end up a little below or above the reference?
• Robustness: How much the controller is affected by noise or disturbance.
• Actuator effort: Is your thrust / vertical_input signal smooth or violent?

You do not need perfect theory. You do need honest observations tied to these ideas.

---

5. Using your work to answer the Questionnaire

Use your experiments to aim for the higher-level options (c) and (d) in each question.

Hints:

• Q1 (experiment setup) Describe clearly:

  • Which platform you used (Python or Webots).
  • What modification you chose (noise or disturbance).
  • Which controllers you compared.

• Q2 (implementation) Mention:

  • What you changed in the code (functions, lines, or blocks).
  • That your code ran and produced sensible plots or behavior.

• Q3 (response analysis) Talk about:

  • Overshoot, oscillations, settling time.
  • How noise or disturbance changed the plots.

• Q4 (best controller) Compare at least two controllers:

  • Which one handled noise better?
  • Which one had smoother thrust?
  • Which one tracked the reference more accurately?

• Q5 (effect of noise/disturbance) Explain:

  • How the response changed when you turned noise/disturbance on.
  • How the control law made it better or worse.

• Q6 (final conclusion) Summarize:

  • Which controller you would prefer in a real drone.
  • Why, using the words: stability, tracking, robustness.

• Q7 (heavier quadrotor) Reason about:

  • Heavier mass needs more thrust to hover.
  • You would need to adjust gains.
  • If you do not, tracking degrades or becomes unstable, and thrust limits can be reached.

---

6. Checklist before you submit

• I ran the base code and confirmed it works.
• I added either noise or a disturbance in Python or Webots.
• I tested at least two different control laws (P, PD, cubic, sign, etc).
• I looked carefully at altitude and control effort and related them to control concepts.
• I answered every question in Questionnaire.pdf with specific references to my experiments.
• I submitted the completed questionnaire (paper copy or PDF).

You are not being graded on writing perfect code from scratch. You are being graded on:

• Making a clear, simple experiment.
• Running it correctly.
• Looking at the results with an engineering brain and describing what you see.