# how to implement tracking problem with PID controller

I'm trying to implement the tracking problem for this example using PID controller. The dynamic equation is

$$I \ddot{\theta} + d \dot{\theta} + mgL \sin(\theta) = u$$

where

$\theta$ : joint variable.

$u$ : joint torque

$m$ : mass.

$L$ : distance between centre mass and joint.

$d$ : viscous friction coefficient

$I$ : inertia seen at the rotation axis.

$\textbf{Regulation Problem:}$

In this problem, the desired angle $\theta_{d}$ is constant and $\theta(t)$ $\rightarrow \theta_{d}$ and $\dot{\theta}(t)$ $\rightarrow 0$ as $t$ $\rightarrow \infty$. For PID controller, the input $u$ is determined as follows

$$u = K_{p} (\theta_{d} - \theta(t)) + K_{d}( \underbrace{0}_{\dot{\theta}_{d}} - \dot{\theta}(t) ) + \int^{t}_{0} (\theta_{d} - \theta(\tau)) d\tau$$

The result is

and this is my code main.m

clear all
clc

global error;
error = 0;

t = 0:0.1:5;

x0 = [0; 0];

[t, x] = ode45('ODESolver', t, x0);

e = x(:,1) - (pi/2); % Error theta

plot(t, e, 'r', 'LineWidth', 2);
title('Regulation Problem','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\theta_{d} - \theta(t)$', 'Interpreter','LaTex');
grid on


and ODESolver.m is

function dx = ODESolver(t, x)

global error; % for PID controller

dx = zeros(2,1);

%Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2

% PID tuning
Kp = 5;
Kd = 1.9;
Ki = 0.02;

% u: joint torque
u = Kp*(pi/2 - x(1)) + Kd*(-x(2)) + Ki*error;
error = error + (pi/2 - x(1));

dx(1) = x(2);
dx(2) = 1/I*(u - d*x(2) - m*g*L*sin(x(1)));

end


$\textbf{Tracking Problem:}$

Now I would like to implement the tracking problem in which the desired angle $\theta_{d}$ is not constant (i.e. $\theta_{d}(t)$); therefore, $\theta(t)$ $\rightarrow \theta_{d}(t)$ and $\dot{\theta}(t)$ $\rightarrow \dot{\theta}_{d}(t)$ as $t$ $\rightarrow \infty$. The input is

$$u = K_{p} (\theta_{d} - \theta(t)) + K_{d}( \dot{\theta}_{d}(t) - \dot{\theta}(t) ) + \int^{t}_{0} (\theta_{d}(t) - \theta(\tau)) d\tau$$

Now I have two problems namely to compute $\dot{\theta}_{d}(t)$ sufficiently and how to read from txt file since the step size of ode45 is not fixed. For the first problem, if I use the naive approach which is

$$\dot{f}(x) = \frac{f(x+h)-f(x)}{h}$$

the error is getting bigger if the step size is not small enough. The second problem is that the desired trajectory is stored in txt file which means I have to read the data with fixed step size but I'v read about ode45 which its step size is not fixed. Any suggestions!

Edit:

For tracking problem, this is my code

main.m

clear all
clc

global error theta_d dt;
error = 0;

i    = 1;
t(i) = 0;
dt   = 0.1;
numel(theta_d)
while ( i < numel(theta_d) )
i = i + 1;
t(i) = t(i-1) + dt;
end

x0 = [0; 0];
options= odeset('Reltol',dt,'Stats','on');
[t, x] = ode45(@ODESolver, t, x0, options);

e = x(:,1) - theta_d; % Error theta

plot(t, x(:,2), 'r', 'LineWidth', 2);
title('Tracking Problem','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\dot{\theta}(t)$', 'Interpreter','LaTex');
grid on


ODESolver.m

function dx = ODESolver(t, x)

persistent i theta_dPrev

if isempty(i)
i = 1;
theta_dPrev = 0;
end

global error theta_d dt ;

dx = zeros(2,1);

%Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2

% PID tuning
Kp = 35.5;
Kd = 12.9;
Ki = 1.5;

if ( i == 49 )
i = 48;
end
% theta_d first derivative
theta_dDot  = ( theta_d(i) - theta_dPrev ) / dt;
theta_dPrev = theta_d(i);

% u: joint torque
u = Kp*(theta_d(i) - x(1)) + Kd*( theta_dDot - x(2)) + Ki*error;
error = error + (theta_dDot - x(1));

dx(1) = x(2);
dx(2) = 1/I*(u - d*x(2) - m*g*L*sin(x(1)));

i = i + 1;
end


trajectory's code is

clear all
clc

a = 0:0.1:(3*pi)/2;

file = fopen('trajectory.txt','w');

for i = 1:length(a)
fprintf(file,'%4f \n',a(i));
end

fclose(file);


The result of the velocity is

Is this correct approach to solve the tracking problem?

1. The time step requested by ODE45 is not going to match what you have in your file. In your ODESolver() desired thetas are read one after the other and then the last one is repeated. As a result the desired theta was not a function of $t$. I used interp1() to fix that.

2. The time difference between two iteration of ODE45 is not constant so you can't use dt to calculate the $\Delta{\theta}$. This is fixed by storing the t in a variable for future reference.

3. Same goes for calculating the integral part of PID.

Here is the new ODESolver():

function dx = ODESolver(t, x)
persistent theta_dPrev time_prev

if isempty(theta_dPrev)
theta_dPrev = 0;
time_prev = 0;
end

global error theta_d dt time_array;

dx = zeros(2,1);

%Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2

% PID tuning
Kp = 40;%35.5;
Kd = 10;%12.9;
Ki = 20;%1.5;

% theta_d first derivative
theta_desired_current = interp1(time_array,theta_d,t);
if t==time_prev
theta_dDot = 0;
else
theta_dDot  = ( theta_desired_current - theta_dPrev ) / (t-time_prev);
end
theta_dPrev = theta_desired_current;

% u: joint torque
u = Kp*(theta_desired_current - x(1)) + Kd*( theta_dDot - x(2)) + Ki*error;
error = error + (theta_desired_current - x(1))*(t-time_prev);

dx(1) = x(2);
dx(2) = 1/I*(u - d*x(2) - m*g*L*sin(x(1)));

time_prev = t;
end


I also changed the main.m to the following:

clear all; close all; clc;

global error theta_d dt time_array;
error = 0;

% theta_d = sin([1:.1:20]/3);

time_array=0:dt:dt*(numel(theta_d)-1);

x0 = [0; 0];
options= odeset('Reltol',dt,'Stats','on');
[t_ode, x] = ode45(@ODESolver, [time_array(1),time_array(end)], x0, options);

theta_desired_ode = interp1(time_array,theta_d,t_ode);
e = x(:,1) - theta_desired_ode; % Error theta

figure(1)
plot(t_ode, x(:,2), 'r', 'LineWidth', 2);
title('Velocity','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\dot{\theta}(t)$', 'Interpreter','LaTex');
grid on

figure(2)
plot(t_ode,x(:,1))
hold on
plot(t_ode,theta_desired_ode)
title('Theta','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\dot{\theta}(t)$', 'Interpreter','LaTex');
legend('Actual Theta', 'Desired Theta')
grid on

• wow this is really nice work. Thank you so much. The results are perfect. – CroCo Mar 24 '15 at 19:31
• I'm glad you found it useful. If you plan to implement it on hardware or want to take your control system one step further, try using anti-windup PID instead of the regular one. – BarzinM Mar 24 '15 at 19:36
• Any good reference for anti-windup PID? – CroCo Mar 24 '15 at 19:41
• – BarzinM Mar 24 '15 at 19:44
• I'm not a big fan of global variables but since you are already using them, you can also set u as global. A better way would be using object-oriented programming. I also want to suggest not using ODE45. Make a loop with fixed time step specially since you seem to be interested in hardware in future. A loop would simulate how your controller and sensors work much much better. Also, it is going to be easier to implement your already developed codes on hardware. – BarzinM Mar 25 '15 at 16:29

Since both $\theta_d\left(t\right)$ and $\dot\theta_d\left(t\right)$ are references at your disposal, i.e. you have to provide them in some way, why don't you simply play with $\dot\theta_d\left(t\right)$ and then compute $\theta_d\left(t\right)$ accordingly by means of integration? As you might know, integration is a well posed operation compared with the derivative.

On the other hand, $\dot\theta\left(t\right)$ is a different "beast". In physical systems you barely have access to it (you'd need ad-hoc sensors for measuring it). Valid alternatives are: Kalman estimation, Savitzky-Golay filtering, use of proper D term in the controller equipped with a high-frequency pole.

• position reference is more intuitive than velocity reference even though what you are saying is perfect and a good alternative. Since I'm doing simulation, I am able to access to the velocity, therefore data is available every instant. Having said that, your answer is alternative but not a solution to my problem. Thank you so much though. – CroCo Mar 24 '15 at 19:17
• Are you sure that you have a too generic $\theta_d\left(t\right)$ so that you cannot come up with a analytical derivative of it? How do you produce your position reference profile? I'm insisting because these details are very important in practice. What is the purpose of your simulation then? Not to go to a real implementation? Does it have only a didactic purpose? I understand you can solve the specific problem differently, but this peculiarity is too narrow and there's the risk that it hides other issues, as I said. – Ugo Pattacini Mar 24 '15 at 19:54
• I'm focusing now on simulation to enhance my skills. The problem with hardware is costly first and it needs some space and tools. A lot of academic papers are based on pure simulation. I will stick with it until I got some money and space. :) – CroCo Mar 24 '15 at 20:10
• I see, you've started tackling the problem. "A lot of academic papers are based on pure simulation", that's right why our field (I'm in academic, tough I'll never forget I'm an engineer too) is being populated with questionable works and everything's becoming messy. Remember: exercise always your critics and never trust them :-) Good luck for the future. – Ugo Pattacini Mar 24 '15 at 20:21

You can treat your differential equation as a third order ODE, because of the integral in the PID controller. For this I will denote the integral of $\theta$ as $\Theta$, such that the ODE becomes,

$$I\ddot{\theta}+d\dot{\theta}+mgL\sin(\theta)+K_p\left(\theta-\theta_d(t)\right)+K_i\left(\Theta-\int_0^t\theta_d(\tau)d\tau\right)+K_d\left(\dot{\theta}-\dot{\theta}_d(t)\right)=0$$

One way you can write this into MATLAB code would be:

clear all
clc

% Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2

% PID tuning
Kp = 5000;
Ki = 166667;
Kd = 50;

% Define reference signal
syms t
a = 3 * pi / 10;
temp = a * t;
% temp = pi/2 + 0*t;
r = matlabFunction(temp, 'Vars', t);
R = matlabFunction(int(temp,t), 'Vars', t);
dr = matlabFunction(diff(temp), 'Vars', t);
clear temp t

fun = @(t,x) [x(2); x(3); -m*g*L/I*sin(x(2))-d/I*x(3)] - [0; 0; ...
Kp / I * (x(2) -  r(t)) + ...
Ki / I * (x(1) -  R(t)) + ...
Kd / I * (x(3) - dr(t))];

T = linspace(0, 5, 1e3);
x0 = [0; 0; 0];
[t, Y] = ode45(@(t,x) fun(t,x), T, x0);

y = zeros(size(t));
for i = 1 : length(t)
y(i) = r(t(i));
end

figure
subplot(2,1,1)
plot(t, y, t, Y(:,2))
xlabel('Time [s]')