# how to implement and code inner and outer PD controllers for quadrotor for position tracking

The quadrotor system is multi-ODEs equations. The linearized model is usually used especially for position tracking, therefore one can determine the desired x-y positions based on the roll and pitch angles. As a result, one nested loop which has inner and outer controllers is needed for controlling the quadrotor. For implementation, do I have to put while-loop inside ode45 for the inner attitude controller? I'm asking this because I've read in a paper that the inner attitude controller must run faster (i.e. 1kHz) than the position controller (i.e. 100-200 Hz). In my code, both loops run at 1kHz, therefore inside ode45 there is no while-loop. Is this correct for position tracking? If not, do I have to insert while-loop inside ode45 for running the inner loop? Could you please suggest me a pseudocode for position tracking?

To be more thorough, the dynamics equations of the nonlinear model of the quadrotor is provided here, if we assume the small angles, the model is reduced to the following equations

\begin{align} \ddot{x} &= \frac{U_{1}}{m} ( \theta \cos\psi + \phi \sin\psi) \\ \ddot{y} &= \frac{U_{1}}{m} ( \theta \sin\psi - \phi \cos\psi) \\ \ddot{z} &= \frac{U_{1}}{m} - g \\ \ddot{\phi} &= \frac{l}{I_{x}} U_{2} \\ \ddot{\theta} &= \frac{l}{I_{y}} U_{3} \\ \ddot{\psi} &= \frac{1}{I_{z}} U_{4} \\ \end{align}

The aforementioned equations are linear. For position tracking, we need to control $x,y,$ and $z$, therefore we choose the desired roll and pitch (i.e. $\phi^{d} \ \text{and} \ \theta^{d}$)

\begin{align} \ddot{x}^{d} &= \frac{U_{1}}{m} ( \theta^{d} \cos\psi + \phi^{d} \sin\psi) \\ \ddot{y}^{d} &= \frac{U_{1}}{m} ( \theta^{d} \sin\psi - \phi^{d} \cos\psi) \\ \end{align}

Therefore, the closed form for the desired angles can be obtained as follows

$$\begin{bmatrix} \phi_{d} \\ \theta_{d} \end{bmatrix} = \begin{bmatrix} \sin\psi & \cos\psi \\ -\cos\psi & \sin\psi \end{bmatrix}^{-1} \left( \frac{m}{U_{1}}\right) \begin{bmatrix} \ddot{x}^{d} \\ \ddot{y}^{d} \end{bmatrix}$$

My desired trajectory is shown below

The results are

And the actual trajectory vs the desired one is

My code for this experiment is

%%
%######################( Position Controller )%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all;
clc;

dt = 0.001;
t = 0;

% initial values of the system
x = 0;
dx = 0;
y = 0;
dy = 0;
z = 0;
dz = 0;

Phi = 0;
dPhi = 0;
Theta = 0;
dTheta = 0;
Psi = pi/3;
dPsi = 0;

%System Parameters:
m  = 0.75;      % mass (Kg)
L  = 0.25;      % arm length (m)
Jx = 0.019688; % inertia seen at the rotation axis. (Kg.m^2)
Jy = 0.019688; % inertia seen at the rotation axis. (Kg.m^2)
Jz = 0.039380; % inertia seen at the rotation axis. (Kg.m^2)
g  = 9.81;      % acceleration due to gravity m/s^2

errorSumX = 0;
errorSumY = 0;
errorSumZ = 0;

errorSumPhi   = 0;
errorSumTheta = 0;

DesiredX = pose(:,1);
DesiredY = pose(:,2);
DesiredZ = pose(:,3);

dDesiredX = 0;
dDesiredY = 0;
dDesiredZ = 0;

DesiredXpre = 0;
DesiredYpre = 0;
DesiredZpre = 0;

dDesiredPhi = 0;
dDesiredTheta = 0;
DesiredPhipre = 0;
DesiredThetapre = 0;

for i = 1:6000

% torque input
%&&&&&&&&&&&&( Ux )&&&&&&&&&&&&&&&&&&
Kpx = 50; Kdx = 8; Kix = 0;
Ux = Kpx*( DesiredX(i) - x  ) + Kdx*( dDesiredX - dx ) + Kix*errorSumX;

errorSumX = errorSumX + ( DesiredX(i) - x );

dDesiredX = ( DesiredX(i) - DesiredXpre ) / dt;
DesiredXpre = DesiredX(i);

%&&&&&&&&&&&&( Uy )&&&&&&&&&&&&&&&&&&
Kpy = 100; Kdy = 10; Kiy = 0;
Uy = Kpy*( DesiredY(i) - y  ) + Kdy*( dDesiredY - dy ) + Kiy*errorSumY;

errorSumY = errorSumY + ( DesiredY(i) - y );

dDesiredY = ( DesiredY(i) - DesiredYpre ) / dt;
DesiredYpre = DesiredY(i);

%&&&&&&&&&&&&( U1 )&&&&&&&&&&&&&&&&&&
Kpz = 100; Kdz = 20; Kiz = 0;
U1 = Kpz*( DesiredZ(i) - z ) + Kdz*( dDesiredZ - dz ) + Kiz*errorSumZ;

errorSumZ = errorSumZ + ( DesiredZ(i) - z );

dDesiredZ = ( DesiredZ(i) - DesiredZpre ) / dt;
DesiredZpre = DesiredZ(i);

%#######################################################################
%#######################################################################
%#######################################################################
% Desired Phi and Theta
R = [  sin(Psi),cos(Psi);
-cos(Psi),sin(Psi)];

DAngles = R\( (m/U1)*[Ux; Uy]);

%Wrap angles
DesiredPhi   = wrapToPi( DAngles(1) ) /2;
DesiredTheta = wrapToPi( DAngles(2) );

%&&&&&&&&&&&&( U2 )&&&&&&&&&&&&&&&&&&
KpP = 100; KdP = 10; KiP = 0;
U2 = KpP*( DesiredPhi - Phi ) + KdP*( dDesiredPhi - dPhi )  + KiP*errorSumPhi;

errorSumPhi = errorSumPhi + ( DesiredPhi - Phi );

dDesiredPhi = ( DesiredPhi - DesiredPhipre ) / dt;
DesiredPhipre = DesiredPhi;

%--------------------------------------
%&&&&&&&&&&&&( U3 )&&&&&&&&&&&&&&&&&&

KpT = 100; KdT = 10; KiT = 0;
U3 = KpT*( DesiredTheta - Theta ) + KdP*( dDesiredTheta - dTheta ) + KiT*errorSumTheta;

errorSumTheta = errorSumTheta + ( DesiredTheta - Theta );

dDesiredTheta = ( DesiredTheta - DesiredThetapre ) / dt;
DesiredThetapre = DesiredTheta;

%--------------------------------------
%&&&&&&&&&&&&( U4 )&&&&&&&&&&&&&&&&&&
KpS = 80; KdS = 20.0; KiS = 0.08;
U4 = KpS*( 0 - Psi ) + KdS*( 0 - dPsi );

%###################( ODE Equations of Quadrotor )###################
%===================( X )=====================
ddx = (U1/m)*( Theta*cos(Psi) + Phi*sin(Psi) );

dx = dx + ddx*dt;
x =  x +  dx*dt;
%===================( Y )=====================
ddy = (U1/m)*( Theta*sin(Psi) - Phi*cos(Psi) );

dy = dy + ddy*dt;
y =  y +  dy*dt;

%===================( Z )=====================
ddz = (U1/m) - g;

dz = dz + ddz*dt;
z =  z +  dz*dt;

%===================( Phi )=====================
ddPhi = ( L/Jx )*U2;

dPhi = dPhi + ddPhi*dt;
Phi =  Phi +  dPhi*dt;

%===================( Theta )=====================
ddTheta =  ( L/Jy )*U3;

dTheta =  dTheta + ddTheta*dt;
Theta =   Theta +  dTheta*dt;

%===================( Psi )=====================
ddPsi =  (1/Jz)*U4;

dPsi = dPsi + ddPsi*dt;
Psi =  Psi +  dPsi*dt;

%store the erro
ErrorX(i)   = ( x - DesiredX(i) );
ErrorY(i)   = ( y - DesiredY(i) );
ErrorZ(i)   = ( z - DesiredZ(i) );
%    ErrorPhi(i)   = ( Phi - pi/4 );
%    ErrorTheta(i) = ( Theta - pi/4 );
ErrorPsi(i)   = ( Psi - 0 );

X(i) = x;
Y(i) = y;
Z(i) = z;

T(i) = t;

%    drawnow
%    plot3(DesiredX, DesiredY, DesiredZ, 'r')
%    hold on
%    plot3(X, Y, Z, 'b')
t = t + dt;

end

Figure1 = figure(1);
set(Figure1,'defaulttextinterpreter','latex');
%set(Figure1,'units','normalized','outerposition',[0 0 1 1]);

subplot(2,2,1)
plot(T, ErrorX, 'LineWidth', 2)
title('Error in $x$-axis Position (m)')
xlabel('time (sec)')
ylabel('$x_{d}(t) - x(t)$', 'LineWidth', 2)

subplot(2,2,2)
plot(T, ErrorY, 'LineWidth', 2)
title('Error in $y$-axis Position (m)')
xlabel('time (sec)')
ylabel('$y_{d}(t) - y(t)$', 'LineWidth', 2)

subplot(2,2,3)
plot(T, ErrorZ, 'LineWidth', 2)
title('Error in $z$-axis Position (m)')
xlabel('time (sec)')
ylabel('$z_{d} - z(t)$', 'LineWidth', 2)

subplot(2,2,4)
plot(T, ErrorPsi, 'LineWidth', 2)
title('Error in $\psi$ (m)')
xlabel('time (sec)')
ylabel('$\psi_{d} - \psi(t)$','FontSize',12);
grid on

Figure2 = figure(2);
set(Figure2,'units','normalized','outerposition',[0 0 1 1]);

figure(2)
plot3(X,Y,Z, 'b')
grid on

hold on
plot3(DesiredX, DesiredY, DesiredZ, 'r')

pos = get(Figure2,'Position');
set(Figure2,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3),pos(4)]);
print(Figure2,'output2','-dpdf','-r0');

legend('actual', 'desired')


The code of the desired trajectory is

clear all; clc;

fileID = fopen('xyTrajectory.txt','w');

angle = -pi; radius = 5; z = 0; t = 0;

for i = 1:6000
if ( z < 2 )
z = z + 0.1;
x = 0;
y = 0;
end
if  ( z >= 2 )
angle = angle + 0.1;
angle = wrapToPi(angle);
z = 2;
end

X(i) = x;
Y(i) = y;
Z(i) = z;

fprintf(fileID,'%f \t %f \t %f\n',x, y, z); end

fclose(fileID); plot3(X,Y,Z) grid on

– acs
May 17, 2015 at 8:22
• @acs, I did. The last code. May 17, 2015 at 8:24
• Ok. I couldnt see the last two equation in the paper.
– acs
May 17, 2015 at 8:44
• I've posted the linearized model of the quadrotor in my post, therefore the code is for the linearized version. May 17, 2015 at 9:03

As extension to this answer, I'd like to share with the community a Simulink model implementing the system described above. It does not represent a solution for the code reported by CroCo but it may give a better insight and, as such, it might be helpful.

The system is depicted below:

In red we have the blocks running at $1\,\text{KHz}$, which deal with the attitude control, whereas in green are shown those blocks that allow tracking the linear displacement and run at $100\,\text{Hz}$.

As side note, since we have to ensure that the "small angles" assumption remains valid throughout the simulation, the take-off phase (i.e. reaching the desired height $z_d$) needs to be almost complete before commencing the displacement tracking of the $x_d\left(t\right)$ and $y_d\left(t\right)$ time-varying linear coordinates (a circle of radius $5\,\text{m}$ in the model). Further, for the same reason, the desired circular trajectory must start from the quadcopter's initial position $\left(x\left(0\right),y\left(0\right)\right)$.

Depending on the controllers' gains, we can get quite nice tracking results:

The first top three diagrams shows the $x$, $y$ and $z$ components (green), respectively, while following the targets (blue). The last plot reports on the roll (blue), pitch (green) and yaw (red). Time is expressed in seconds, displacements are in meters and angles are in degrees.

• thank you so much. It seems that I'm getting somewhere. My ultimate goal is to re-implement the project in C++. In my Matlab code, both controllers are running at 1KHz which means it is hard to carry out aggressive maneuver with linear model. For nonlinear model, I'm using backstepping controller, for very careful and smooth trajectory, it seems the quadrotor follows the trajectory but not very fast. May 26, 2015 at 15:47
• Cont., for using joystick, it is hard to tune the backstepping controller to let the quadrotor follows the trajectory as fast as possible because this mission requires the platform to carry out aggressive maneuver which means the small angles assumption don't hold any more. May 26, 2015 at 15:50
• In the "small angles" assumption (still nonlinear model, yet simplified), the "aggressiveness" of the final maneuvers will depend on the the maximum angles you can reach before invalidating the assumption itself, and not on the rate of the outer loop, I'd say. For the complete nonlinear model as well as the back-stepping controller, I suppose you're referring to the cited paper, I didn't read though. Do you know that with the Embedded Coder toolbox you can obtain production C++ code automatically from Simulink models, ready to be run on your systems? May 26, 2015 at 17:09

Your pseudocode for PD like this:

Kp Kd
erroSum=errorSum+(desired-actual);
ddesired=(desired-predesired)/dt;
desiredpre=desired;
PD= Kp*(desired-actual)+Kd*(ddesired-dactual)


Try to apply this:

previosuerror=0
Kp Kd
error=(desired-actual); //for angles use atan2(sin(desired),cos(actual)) neglect sign
derivative=(previouserror-error)/dt;
previouserror=error;
P=Kp*error;
D=Kd*derivative;
PD=P+D;


Reference: first 3 module of https://class.coursera.org/conrob-001

• What is the problem with my approach? For the link, what is the first 3 module ? I opened the link but it is not clear what you are referring me to. May 17, 2015 at 23:47
• I think your PD codes are wrong. Try to write pseudocode which shown as"try to apply this". that pseudocode wirtten according to reference. Also you have too many PD calculations although you need only three.
– acs
May 18, 2015 at 8:42
• "I think your PD codes are wrong." What is the problem? It is just different approach for implementing the controller. I believe I need six controllers. Three for the outer loop (x,y,z) and three for inner loop (phi, theta, psi). May 18, 2015 at 9:16
• ok you can apply 3 PD but it works with wrong PD code and calculates wrong angle in inner loop.Try my pseudocode to see wrong or right. I'm trying to help you. You must know that no one will write all your code
– acs
May 18, 2015 at 19:00
• I'm totally sure the problem is not from the way PD is coded in my case but I've tried your suggestion with no luck. The problem is more complicated than I thought. The controllers must be tuned carefully and whether the linear model suits aggressive trajectories is the key. I've moved to nonlinear system with backstepping and with careful trajectory and tuning, I'm getting good results but the response is rather slow. May 26, 2015 at 15:58