ECPS 208: Control Systems for Cyber Physical Systems
Homework Assignment Number 4
Higher Dimensional Quadcopter - Trajectory Control
Spring 2023
Consider the case of a 2D quadcopter. We would like to control the quadcopter for a variety of
2D trajectories (in the y-z directions): straight line trajectory, sine wave trajectory, step trajectory.
The files for this section are contained in the folder: “hw4 2d.zip”.
Figure 1: 2D Quadcopter Model.
1 Derivations
Our 2D quadcopter is depicted in Figure 1. The global, or inertia frame, is shown as {a2, a3} and
the body frame of the quadcopter is depicted as {b2, b3}. In the 2D motion, the roll angle ϕ is
shown and blue, and we can model the rotation matrix between the {A} and {B} frames as:
ARB =
cos(ϕ) − sin(ϕ)
sin(ϕ) − cos(ϕ)
(1)
The angular velocity is modeled as:
AωB = ϕ˙ (2)
We will make some simplifying assumptions in our model of the inputs. We assume only two
motors that can each apply a thrust F1, F2, located a distance L away from the quadcopters center
of mass. Our quadcopter has two inputs, u1 (thrust) and u2 (moment):
u1 = F1 + F2 (3)
u2 = L(F1 − F2) (4)
1
The state space of our system is defined by three states: y, z, and ϕ. The equations of motion
for the state variables can be derived as:
my¨ = −u1 sin(ϕ) (5)
mz¨ = −mg + u1 cos(ϕ) (6)
Ixxϕ¨ = u2 (7)
where Ixx is the moment of inertia about the x axis.
1.1 Linearization
Linearize the above state space equations about a hovering equilibrium. Assume as this hover that
ϕ = 0 and the thrust force balances out the gravity.
2 PD Control
We will design a PD controller and derive the equations for our inputs in the controller function.
As we have three states y, z, and ϕ, we will need a set of gains (Kp, Kd) for each state. We form
the following three differential equations that aim to converge the error of each state to zero:
(¨ydes − y¨) + Kpy(ydes − y) + Kdy( ˙ydes − y˙) = 0 (8)
(¨zdes − z¨) + Kpz(zdes − z) + Kdz( ˙zdes − z˙) = 0 (9)
(ϕ¨
des − ϕ¨) + Kpphi(ϕdes − ϕ) + Kdphi(ϕ˙
des − ϕ˙) = 0 (10)
The inputs u1 and u2 are derived as follows:
u1 = m(g + ¨z + Kpz(zdes − z) + Kdz( ˙zdes − z˙)) (11)
u2 = Ixx(ϕ¨ + Kpphi(ϕdes − ϕ) + Kdphi(ϕ˙
des − ϕ˙)) (12)
ϕ = −(¨ydes + Kpy(ydes − y) + Kdy( ˙ydes − y˙))/g (13)
ϕ˙ = (KpyKdy(ydes − y) + (K2
dy − Kpy) ∗ ( ˙ydes − y˙))/g (14)
2.1 PD Controller
Update the file “pd controller 2d.m” following the above equations. Set the controller gains to any
values you feel appropriate as they will be tuned further in the following sections.
function [ u1, u2 ] = pd_controller_2d(~, state, des_state, params)
%CONTROLLER Controller for the planar quadrotor
%
% state: The current state of the robot with the following fields:
% state.pos = [y; z], state.vel = [y_dot; z_dot], state.rot = [phi],
% state.omega = [phi_dot]
%
% des_state: The desired states are:
2
% des_state.pos = [y; z], des_state.vel = [y_dot; z_dot], des_state.acc =
% [y_ddot; z_ddot]
%
% params: robot parameters
% Using these current and desired states, you have to compute the desired
% controls
kdy = ...;
kpy = ...;
kdz = ...;
kpz = ...;
kdphi = ...;
kpphi = ...;
phic = ...;
phic_dot = ...;
u1=...;
u2 = ...;
end
2.2 Trajectory Control
Use the following script “runsim.m” and tune your controller gains to have the quadcopter reach
its goal for each type of trajectory: (a) Straight Line Trajectory, (b) Sine Wave Trajectory, (c) Step
Trajectory. Bonus: feel free to try the Diamond Trajectory as well.
clear;
close all;
addpath(’utils’);
addpath(’trajectories’);
controlhandle = @pd_controller_2d;
% Choose which trajectory you want to test with:
% @traj_line, @traj_sine, @traj_step , @traj_diamond
trajhandle = @traj_line;
%trajhandle = @traj_sine;
%trajhandle = @traj_step;
%trajhandle = @traj_diamond;
[t, state] = simulation_2d(controlhandle, trajhandle);
3
3 Controller Robustness
In this section, we investigate the robustness of controller design with regards to uncertainties in
both modeling and sensing. Recall the general form of a closed loop control system in Figure 2.
Figure 2: General form of a closed loop control system.
3.1 Plant Disturbances
In our system, an example of a plant disturbance is an incorrect or incomplete model of the forces
acting on the quadcopter (thrust, air resistance, friction, etc.) or on the model parameters we have
measured about the quadcopter (e.g., mass, length).
Our noise model will simply add a wind disturbance to the y and z directions following:
y¨ = −u1 sin(ϕ)/m + λddy (15)
z¨ = −g + u1 cos(ϕ)/m + λddz (16)
where dy and dz are the added noise values due to random wind: d ∼ N (µ, σ2
) with µ = 0 and
σ = 1; and λd is our tunable scaling of disturbance noise.
Run the following script “runsim plant disturbances.m” with varying levels of noise (λd =
1, 2, 5, 10, 20) and trajectories (line, sine, step) and comment on the results.
clear;
close all;
addpath(’utils’);
addpath(’trajectories’);
controlhandle = @pd_controller_2d;
% Choose which trajectory you want to test with:
4
% @traj_line, @traj_sine, @traj_step , @traj_diamond
%trajhandle = @traj_line;
%trajhandle = @traj_sine;
trajhandle = @traj_step;
%trajhandle = @traj_diamond;
%Define the level of plant noise: lambda_d
%Our noise will be modeled as wind which affects y’’ and z’’.
noise_level = ...; %1, 2, 5, 10, 20
[t, state] = simulation_2d_plant_disturbance(controlhandle, trajhandle, noise_level);
3.2 Sensor Measurement Noise
Noise in sensor measurements is very common, and one benefit of control is accounting for variabilities in sensing noise. In our closed-loop controller, we assume access to a sensor that measures
the position and the velocity of the quadcopter in the direction. We will now study what happens
when the sensing in the system is not perfect. We introduce noise into the y and z position sensors
as follows:
ymeas = y + λnny (17)
zmeas = z + λnnz (18)
where ymeas and zmeas are the sensor measurements the controller has access to; ny and nz are
the added noise values due to sensing error: n ∼ N (µ, σ2
) with µ = 0 and σ = 1; and λn is our
tunable scaling of the sensor noise level.
Run the following script “runsim sensor noise.m” with varying levels of noise (λn = 0.01, .05, 0.1, 0.5)
and trajectories (line, sine, step) and comment on the results.
clear;
close all;
addpath(’utils’);
addpath(’trajectories’);
controlhandle = @pd_controller_2d;
% Choose which trajectory you want to test with:
% @traj_line, @traj_sine, @traj_step , @traj_diamond
%trajhandle = @traj_line;
%trajhandle = @traj_sine;
trajhandle = @traj_step;
%trajhandle = @traj_diamond;
%Define the level of sensor noise: lambda_n
%Our noise will be modeled as wind which affects y’’ and z’’.
noise_level = ...; % 0.01, .05, 0.1, 0.5
5
[t, state] = simulation_2d_sensor_noise(controlhandle, trajhandle, noise_level);
4 Bonus: 3D Trajectory Control
There are no graded questions within this section. We encourage you to explore the code at your
own interest. The files for this section are contained in the folder: “hw4 3d.zip”. Make sure to
remove the utils folder from your path to avoid errors if you have recently run the code for the 2D
case.
Consider the case of a 3D quadcopter. The code for the dynamics and controller has been
provided for you in the files within these folders. We would like to control the quadcopter for a
variety of 3D trajectories (in the x-y-z directions): Line, Helix, Waypoint. Feel free to experiment
with the code at your own interest. The waypoint trajectory is easily definable to measure your
own custom trajectory path.