This page describes the details of developing a robust and accurate simulation environment. An accurate simulation combines the equations of motion derived in the System Modeling section, the specific parameter values estimated in the Model Verification section, and the controller designed in the Controller Design section. All of these pieces are necessary to create an accurate simulation environment. An accurate simulation environment enables operators to design and test control designs, filters, observers, and path planning algorithms before they are implemented on the physical system in the real world. The following sections outline the code necessary to implement a quadrotor simulator in MATLAB. Where appropriate, specific code or blocks are highlighted in greater detail.

MATLAB – Script (download here)

The Matlab scripts developed allow a user to quickly and easily make small modifications such as physical parameters or control gains as well as large changes such as the equations of dynamics or controller types. It also allows the user to quickly generate data and manipulate the plotting of the data for analysis purposes. Below is a brief description of the main files and functions required to run and manipulate the simulated quadrotor.

The simulator is run using the quadrotor_sim.m file. This file initializes the simulation environment with the following commands. The global variable Quad is created which will hold all of the quadrotor variables.

 %% Add Paths
addpath utilities

 %% Initialize Workspace
clear all;
close all;
clc;

global Quad;

 %% Initialize the plot
init_plot;
plot_quad_model;

 %% Initialize Variables
quad_variables;
quad_dynamics_nonlinear;

The function init_plot.m draws the three dimensional environment which the quadrotor’s simulated movement will be visualized inside of. Next, plot_quad_model.m is called. This script begins by loading the Quad_plotting_model.mat file which is created by running define_quad_model.m. This .mat file uses the quadrotor’s physical dimensions such as arm length, arm thickness, and propeller radius to define the vertices of each arm and motor in three dimensions for plotting purposes. The plot_quad_model.m script then draws the initial quadrotor model in the three dimensional plotting environment.

In the next section, quad_variables.m is run which defines the primary variables for the simulation including the quadrotor’s physical parameters, initial and desired conditions, simulation parameters, and controller gains. The values of the physical parameters are taken from the Model Verification section. The quad_dynamics_nonlinear.m function uses the initial values to calculate the initial velocities and accelerations of the quadrotor model. These simulation uses the nonlinear equations of motion derived in the System Modeling section. The code representing these equations is below. Note that discrete integration is used to calculate the velocities and positions from the acceleration values.

function quad_dynamics_nonlinear

global Quad;

 %% Update Accelerations
Quad.X_ddot = (-[cos(Quad.phi)*sin(Quad.theta)*cos(Quad.psi)+sin(Quad.phi)*sin(Quad.psi)]*Quad.U1-Quad.Kdx*Quad.X_dot)/Quad.m;
Quad.Y_ddot = (-[cos(Quad.phi)*sin(Quad.psi)*sin(Quad.theta)-cos(Quad.psi)*sin(Quad.phi)]*Quad.U1-Quad.Kdy*Quad.Y_dot)/Quad.m;
Quad.Z_ddot = (-[cos(Quad.phi)*cos(Quad.theta)]*Quad.U1-Quad.Kdz*Quad.Z_dot)/Quad.m+Quad.g;

Quad.p_dot = (Quad.q*Quad.r*(Quad.Jy - Quad.Jz) - Quad.Jp*Quad.p*Quad.Obar + Quad.l*Quad.U2)/Quad.Jx;
Quad.q_dot = (Quad.p*Quad.r*(Quad.Jz - Quad.Jx) + Quad.Jp*Quad.q*Quad.Obar + Quad.l*Quad.U3)/Quad.Jy;
Quad.r_dot = (Quad.p*Quad.q*(Quad.Jx - Quad.Jy) + Quad.U4)/Quad.Jz;

Quad.phi_dot   = Quad.p + sin(Quad.phi)*tan(Quad.theta)*Quad.q + cos(Quad.phi)*tan(Quad.theta)*Quad.r;
Quad.theta_dot = cos(Quad.phi)*Quad.q - sin(Quad.phi)*Quad.r;
Quad.psi_dot   = sin(Quad.phi)/cos(Quad.theta)*Quad.q + cos(Quad.phi)/cos(Quad.theta)*Quad.r;

The next section in quadrotor_sim.m is a loop which runs the actual simulation once all the initialization is complete. The relevant code is shown below.

 %% Run The Simulation Loop
while Quad.t_plot(Quad.counter-1)< max(Quad.t_plot);

    %% Measure Parameters (for simulating sensor errors)
    sensor_meas;

    %% Filter Measurements (future work)

    %% Implement Controller
&nbsp;&nbsp;&nbsp; position_PID;
&nbsp;&nbsp;&nbsp; attitude_PID;
&nbsp;&nbsp;&nbsp; rate_PID;

    %% Calculate Desired Motor Speeds
    quad_motor_speed;

    %% Update Position With The Equations of Motion
    quad_dynamics_nonlinear;

    %% Plot the Quadrotor's Position
    if(mod(Quad.counter,3)==0)
        plot_quad
        Quad.counter;
        drawnow
    end

    Quad.init = 1;  %Ends initialization after first simulation iteration

end

The loop runs for the length of time specified by t_plot. The first two areas are reserved for implementing functions to simulate sensor noise as well as implementing a filter such as an EKF to filter the sensor measurements before sending them to the controller. Currently, only the sensor noise simulation script is implemented. This is done in the sensor_meas.m file. This file updates the global position, linear acceleration, and rotational rate variables using the sensor model derived in the Model Verification section. The specific function is detailed below.

function sensor_meas

global Quad;

 %% GPS Measurements
if(mod(Quad.counter,Quad.GPS_freq) == 0)
    Quad.X = Quad.X + randn(1)*Quad.X_error;
    Quad.Y = Quad.Y + randn(1)*Quad.Y_error;

    %% Barometer Measurements
    Quad.Z = Quad.Z + randn(1)*Quad.Z_error;

end

 %% IMU Measurements
Quad.X_ddot = Quad.X_ddot + Quad.x_acc_bias + Quad.x_acc_sd*randn(1);
Quad.Y_ddot = Quad.Y_ddot + Quad.y_acc_bias + Quad.y_acc_sd*randn(1);
Quad.Z_ddot = Quad.Z_ddot + Quad.z_acc_bias + Quad.z_acc_sd*randn(1);

Quad.p = Quad.p + Quad.x_gyro_bias + Quad.x_gyro_sd*randn(1);
Quad.q = Quad.q + Quad.y_gyro_bias + Quad.y_gyro_sd*randn(1);
Quad.r = Quad.r + Quad.z_gyro_bias + Quad.z_gyro_sd*randn(1);

Next, the position_PID.m function is called which acts as the position controller. The outputs of this function, a desired roll and pitch angle, are inputs to attitude_PID.m which is the attitude/altitude controller. Both are PID implementations and implement the equations derived in the Controller Design section. The translational PID controller code is shown below for reference. Note that the desired positions in the global frame must be converted to the body coordinate frame.

function outer_PID

global Quad

x = Quad.X;
y = Quad.Y;
z = Quad.Z;
phi = Quad.phi;
theta = Quad.theta;
psi = Quad.psi;

 %% Rotate Desired Position from GF to BF (Z axis rotation only)
[Quad.X_des,Quad.Y_des,Quad.Z_des] = rotateGFtoBF(Quad.X_des_GF,Quad.Y_des_GF,Quad.Z_des_GF,0*phi,0*theta,psi);

 %% Rotate Current Position from GF to BF
[Quad.X_BF,Quad.Y_BF,Quad.Z_BF] = rotateGFtoBF(x,y,z,phi,theta,psi);

 %% Rotate Current Velocity from GF to BF
[Quad.X_BF_dot,Quad.Y_BF_dot,Quad.Z_BF_dot] = rotateGFtoBF(Quad.X_dot,Quad.Y_dot,Quad.Z_dot,phi,theta,psi);

 %% X Position PID controller
x_error = Quad.X_des - Quad.X_BF;
if(abs(x_error) < Quad.X_KI_lim)
    x_error_sum = x_error_sum + x_error;
end
cp = Quad.X_KP*x_error;    %Proportional term
ci = Quad.X_KI*Quad.Ts*x_error_sum;
ci = min(Quad.theta_max, max(-Quad.theta_max, ci));    %Saturate ci
cd = Quad.X_KD*Quad.X_BF_dot;                     %Derivative term
Quad.theta_des =  - (cp + ci + cd);   %Theta and X inversely related
Quad.theta_des = min(Quad.theta_max, max(-Quad.theta_max, Quad.theta_des));

 %% Y Position PID controller
y_error = Quad.Y_des - Quad.Y_BF;
if(abs(y_error) < Quad.Y_KI_lim)
    y_error_sum = y_error_sum + y_error;
end
cp = Quad.Y_KP*y_error;    %Proportional term
ci = Quad.Y_KI*Quad.Ts*y_error_sum;
ci = min(Quad.phi_max, max(-Quad.phi_max, ci));    %Saturate ci
cd = Quad.Y_KD*Quad.Y_BF_dot;                      %Derivative term
Quad.phi_des = cp + ci + cd;
Quad.phi_des = min(Quad.phi_max, max(-Quad.phi_max, Quad.phi_des));

end

The attitude/altitude controller code is shown below. The outputs of this controller are the three desired angular rates and the desired thrust.

function attitude_PID

global Quad

phi = Quad.phi;
theta = Quad.theta;
psi = Quad.psi;

 %% Z Position PID Controller/Altitude Controller
z_error = Quad.Z_des_GF-Quad.Z_BF;
if(abs(z_error) < Quad.Z_KI_lim)
    z_error_sum = z_error_sum + z_error;
end
cp = Quad.Z_KP*z_error;         %Proportional term
ci = Quad.Z_KI*Quad.Ts*z_error_sum; %Integral term
ci = min(Quad.U1_max, max(Quad.U1_min, ci));    %Saturate ci
cd = Quad.Z_KD*Quad.Z_dot;                  %Derivative term
Quad.U1 = -(cp + ci + cd)/(cos(theta)*cos(phi)) + (Quad.m * Quad.g)/(cos(theta)*cos(phi));   %Negative since Thurst and Z inversely related
Quad.U1 = min(Quad.U1_max, max(Quad.U1_min, Quad.U1));

 %% Attitude Controller
 %% Roll PID Controller
phi_error = Quad.phi_des - phi;
if(abs(phi_error) < Quad.phi_KI_lim)
    phi_error_sum = phi_error_sum + phi_error;
end
cp = Quad.phi_KP*phi_error;
ci = Quad.phi_KI*Quad.Ts*phi_error_sum;
ci = min(Quad.p_max, max(-Quad.p_max, ci));
cd = Quad.phi_KD*Quad.p;
Quad.p_des = cp + ci + cd;
Quad.p_des = min(Quad.p_max, max(-Quad.p_max, Quad.p_des));

 %% Pitch PID Controller
theta_error = Quad.theta_des - theta;
if(abs(theta_error) < Quad.theta_KI_lim)
    theta_error_sum = theta_error_sum + theta_error;
end
cp = Quad.theta_KP*theta_error;
ci = Quad.theta_KI*Quad.Ts*theta_error_sum;
ci = min(Quad.q_max, max(-Quad.q_max, ci));
cd = Quad.theta_KD*Quad.q;
Quad.q_des = cp + ci + cd;
Quad.q_des = min(Quad.q_max, max(-Quad.q_max, Quad.q_des));

 %% Yaw PID Controller
psi_error = Quad.psi_des - psi;
if(abs(psi_error) < Quad.psi_KI_lim)
    psi_error_sum = psi_error_sum + psi_error;
end
cp = Quad.psi_KP*psi_error;
ci = Quad.psi_KI*Quad.Ts*psi_error_sum;
ci = min(Quad.r_max, max(-Quad.r_max, ci));
cd = Quad.psi_KD*Quad.r;
Quad.r_des = cp + ci + cd;
Quad.r_des = min(Quad.r_max, max(-Quad.r_max, Quad.r_des));

end

Lastly, the rate controller is implemented in the rate_PID.m function. This control loop takes the desired angular rates computed from the attitude controller and calculates the remaining three control inputs. These are then mapped to the motor commands in subsequent code.

function rate_PID

global Quad

p = Quad.p;
q = Quad.q;
r = Quad.r;

 %% Angular Rate Controller
 %% Roll PID Controller
p_error = Quad.p_des - p;
if(abs(p_error) < Quad.p_KI_lim)
    p_error_sum = p_error_sum + p_error;
end
cp = Quad.p_KP*p_error;
ci = Quad.p_KI*Quad.Ts*p_error_sum;
ci = min(Quad.U2_max, max(Quad.U2_min, ci));
cd = Quad.p_KD*Quad.p_dot;
Quad.U2 = cp + ci + cd;
Quad.U2 = min(Quad.U2_max, max(Quad.U2_min, Quad.U2));

 %% Pitch PID Controller
q_error = Quad.q_des - q;
if(abs(q_error) < Quad.q_KI_lim)
    q_error_sum = q_error_sum + q_error;
end
cp = Quad.q_KP*q_error;
ci = Quad.q_KI*Quad.Ts*q_error_sum;
ci = min(Quad.U3_max, max(Quad.U3_min, ci));
cd = Quad.q_KD*Quad.q_dot;
Quad.U3 = cp + ci + cd;
Quad.U3 = min(Quad.U3_max, max(Quad.U3_min, Quad.U3));

 %% Yaw PID Controller
r_error = Quad.r_des - r;
if(abs(r_error) < Quad.r_KI_lim)
    r_error_sum = r_error_sum + r_error;
end
cp = Quad.r_KP*r_error;
ci = Quad.r_KI*Quad.Ts*r_error_sum;
ci = min(Quad.U4_max, max(Quad.U4_min, ci));
cd = Quad.r_KD*Quad.r_dot;
Quad.U4 = cp + ci + cd;
Quad.U4 = min(Quad.U4_max, max(Quad.U4_min, Quad.U4));

end

Next, the function quad_motor_speed.m is used to convert the control inputs into motor speeds, saturate the motor speeds based on the motor characteristics, and then recompute the control inputs for the equations of motion. The relevant code computes the control input to motor speed mapping formulas derived in the Controller Design section.

function quad_motor_speed

global Quad

 %% Calculate motor speeds (rad/s)^2
w1 = Quad.U1/(4*Quad.KT) - Quad.U3/(2*Quad.KT*Quad.l) + Quad.U4/(4*Quad.Kd);
w2 = Quad.U1/(4*Quad.KT) - Quad.U2/(2*Quad.KT*Quad.l) - Quad.U4/(4*Quad.Kd);
w3 = Quad.U1/(4*Quad.KT) + Quad.U3/(2*Quad.KT*Quad.l) + Quad.U4/(4*Quad.Kd);
w4 = Quad.U1/(4*Quad.KT) + Quad.U2/(2*Quad.KT*Quad.l) - Quad.U4/(4*Quad.Kd);

 %% Apply realistic motor speed limits
if w1 > Quad.max_motor_speed^2
    w1 = Quad.max_motor_speed^2;
end
if w1 < Quad.min_motor_speed^2
    w1 = Quad.min_motor_speed^2;
end

if w2 > Quad.max_motor_speed^2
    w2 = Quad.max_motor_speed^2;
end
if w2 < Quad.min_motor_speed^2
    w2 = Quad.min_motor_speed^2;
end

if w3 > Quad.max_motor_speed^2
    w3 = Quad.max_motor_speed^2;
end
if w3 < Quad.min_motor_speed^2
    w3 = Quad.min_motor_speed^2;
end

if w4 > Quad.max_motor_speed^2
    w4 = Quad.max_motor_speed^2;
end
if w4 < Quad.min_motor_speed^2
    w4 = Quad.min_motor_speed^2;
end

Quad.O1 = sqrt(w1);    % Front M
Quad.O2 = sqrt(w2);    % Right M
Quad.O3 = sqrt(w3);    % Rear M
Quad.O4 = sqrt(w4);    % Left M

Quad.O1_plot(Quad.counter) = Quad.O1;
Quad.O2_plot(Quad.counter) = Quad.O2;
Quad.O3_plot(Quad.counter) = Quad.O3;
Quad.O4_plot(Quad.counter) = Quad.O4;

 %% Re-compute traditional control inputs

Quad.U1 = Quad.KT*(Quad.O1^2 + Quad.O2^2 + Quad.O3^2 + Quad.O4^2);
Quad.U1_plot(Quad.counter) = Quad.U1;

Quad.U2 = Quad.KT*Quad.l*(Quad.O4^2 - Quad.O2^2);
Quad.U2_plot(Quad.counter) = Quad.U2;

Quad.U3 = Quad.KT*Quad.l*(Quad.O3^2 - Quad.O1^2);
Quad.U3_plot(Quad.counter) = Quad.U3;

Quad.U4 = Quad.Kd*(Quad.O1^2 + Quad.O3^2 - Quad.O2^2 - Quad.O4^2);
Quad.U4_plot(Quad.counter) = Quad.U4;

Quad.O = (-Quad.O1 + Quad.O2 - Quad.O3 + Quad.O4);
  Quad.O_plot(Quad.counter) = Quad.O;

end

After computing the control inputs using the physical motor limits, the control inputs are input into the quadrotor dynamics function, quad_dynamics_nonlinear.m, to update the quadrotor’s position and attitude.

Last in the simulation loop, the plot_quad.m function is called every three iterations to plot the current position of the quad in the three dimensional environment. This allows the user to visualize the behavior of the quadrotor. This function updates the vertices of the quadrotor arms and motors using the position and attitude of the quadrotor output from the equations of motion. An example of this visualization is shown below.

Once the simulation is completed, the function plot_data.m is called. This function is used to plot various performance metrics of the quadrotor over the time of the simulation. Plots include the actual and desired positions and attitude angels as well as the calculated control inputs. These plots can be used to manually tune the PID controller gains to specific performance characteristics. A sample plot depicting the system’s response to step inputs is shown below validating the control system design and simulation.

MATLAB – Simulink (download here)

An alternative to using the quadrotor simulation script is the use of a Simulink block diagram. The block diagram is stored in the QuadrotorSimulink.mdl file. This file consists of several layers which will be described in detail. There are a couple of benefits with this method. Since the code is stored in blocks, it is easy to switch between different controller or quadrotor dynamics. Simulink also supports blocks such as step inputs and scopes which can be used to analyze and tune the controller parameters. While the variables and parameters can be loaded from a script file, it is also possible to use the block diagram to identify and estimate unknown system parameters using real world flight data. An overview of the simulation is shown below. The step function values and scopes can be used to manipulate the quadrotor initial conditions and analyze the system’s simulated behavior. The simulation can be run from the QuadSimulink.m file which also plots some of the data.

 

At the top level, the block diagram is broken down into 3 primary blocks. These include the outer loop position controller, the inner loop attitude/altitude controller, and the quadrotor dynamics. This series of blocks as well as their inputs and outputs it shown below.

Stepping into the translational position controller reveals the following block diagram. Note that these equations do not depict equations to compute a desired yaw. Typically, the user sends the autopilot yaw commands that translate into a desired yaw rate. This way, the pilot can manipulate the yaw and have the quadrotor maintain a constant heading when the yaw stick is centered. If the yaw commands translated to a specific angle, the quadrotor would rotate back to its original heading when the yaw stick was centered. These equations calculate a desired yaw rate based on a desired yaw angle input. However, the desired yaw rate is typically taken directly from the RC command inputs.

The desired roll and pitch angles computed by the position controller are combined with the desired altitude and heading in the attitude/altitude controller shown below.

 

The altitude controller computes the desired thrust control input from the desired altitude. The attitude controllers take the desired angles and compute desired angular velocities for the final rate controllers.

 

The angular velocity controller computes the final three moment control inputs. Each PID block contains the individual Proportional, Integrator, and Derivative gains shown in the generic block diagram below and detailed further in the Control Systems Design section.

The final inputs are then fed into the quadrotor dynamics block. As shown below, this system is made up of several smaller subsystems.

The motor speed calculator limits the control inputs to the physical motor parameters.

 

The rotational dynamics calculate the angular accelerations.

These values are fed into the angular velocities transformation block which computes the angular velocities.

 

The angles are also fed into the translational dynamics block which computes the translational accelerations.

A sample plot depicting the system’s response to step inputs is shown below validating the control system design and simulation.

[simple-social-share]