PID control simulation code improvement

I'm trying to simulate the roll angle dynamics using the PID controller. I just would like to know if this code iw well-written or can be improved?

Any help will be appreciated.

%% Parameters
J = 0.03;                  % moment of inertia (kg*m^2)
Kp = 2;
Ki = 1;
Kd = 0.5;

dt = 0.01;                 % time step
T = 3;                     % total time
t = 0:dt:T;
N = length(t);


phi_ref_deg = 5;
phi_ref = deg2rad(phi_ref_deg);

phi = zeros(1, N);
omega = zeros(1, N);
u = zeros(1, N);
e = zeros(1, N);

I = 0;                    
prev_e = phi_ref - phi(1);

for k = 1:N-1
    
    e(k) = phi_ref - phi(k);
    
    I = I + e(k)*dt;               
    D = (e(k) - prev_e)/dt;        
    
    % PID control
    u(k) = Kp*e(k) + Ki*I + Kd*D;
    
    % Dynamics: J*phi_ddot = u
    phi_ddot = u(k)/J;
    
    omega(k+1) = omega(k) + phi_ddot*dt;
    phi(k+1)   = phi(k)   + omega(k+1)*dt;
    
    prev_e = e(k);
end

e(end) = phi_ref - phi(end);

phi_deg = rad2deg(phi);

peak_val = max(phi_deg);
overshoot = max(0, peak_val - phi_ref_deg);


fprintf('Overshoot: %.3f deg\n', overshoot);

figure;
subplot(2,1,1)
plot(t, phi_deg, 'LineWidth', 1.4); hold on;
yline(phi_ref_deg, '--r', 'Ref');
xlabel('Time (s)');
ylabel('Roll angle (deg)');
title('Roll Angle Response');
grid on;

subplot(2,1,2)
plot(t, u, 'LineWidth', 1.4);
xlabel('Time (s)');
ylabel('u (N*m)');
title('Control Torque');
grid on;

Answer 1

[Mathieu NOE]

hello

minor suggestions - in my view , the code is supposed to reflect a real time implementation so data are only known up to the present sample (k th iteration) - so I prefer to avoid having indexes in the future (k+1).

this is basically the same code with just a one sample shift in the indexes (and in the for loop start and end iteration values)

instead of :

for k = 1:N-1

....

omega(k+1) = omega(k) + phi_ddot*dt;

phi(k+1) = phi(k) + omega(k+1)*dt;

...

end

I personnaly prefer :

for k = 2:N

....

omega(k) = omega(k-1) + phi_ddot*dt;

phi(k) = phi(k-1) + omega(k)*dt;

...

end

then this line becomes unneccessary : e(end) = phi_ref - phi(end);

and you can remove this variable prev_e = e(k); because prev_e = e(k-1)

I also added a saturation on the command (u) so you can see the effect on the convergence time

%% Parameters
J = 0.03;                  % moment of inertia (kg*m^2)
Kp = 2;
Ki = 1;
Kd = 0.5;
dt = 0.01;                 % time step
T = 3;                     % total time
t = 0:dt:T;
N = length(t);
phi_ref_deg = 5;
phi_ref = deg2rad(phi_ref_deg);
phi = zeros(1, N);
omega = zeros(1, N);
u = zeros(1, N);
e = zeros(1, N);
I = 0;    
Umax = 0.1; % max amplitude for the command signal

for k = 2:N
    
    e(k) = phi_ref - phi(k-1);
    
    I = I + e(k)*dt;               
    D = (e(k) - e(k-1))/dt;        
    
    % PID control
    u(k) = Kp*e(k) + Ki*I + Kd*D;
    % saturation
    if abs(u(k))>Umax
        u(k) = Umax*sign(u(k));
    end

    
    % Dynamics: J*phi_ddot = u
    phi_ddot = u(k)/J;
    
    omega(k) = omega(k-1) + phi_ddot*dt;
    phi(k)   = phi(k-1)   + omega(k)*dt;
    
end
phi_deg = rad2deg(phi);
peak_val = max(phi_deg);
overshoot = max(0, peak_val - phi_ref_deg);
fprintf('Overshoot: %.3f deg\n', overshoot);

Overshoot: 0.495 deg

figure;
subplot(2,1,1)
plot(t, phi_deg, 'LineWidth', 1.4); hold on;
yline(phi_ref_deg, '--r', 'Ref');
xlabel('Time (s)');
ylabel('Roll angle (deg)');
title('Roll Angle Response');
grid on;
subplot(2,1,2)
plot(t, u, 'LineWidth', 1.4);
xlabel('Time (s)');
ylabel('u (N*m)');
title('Control Torque');
grid on;

●

[Mathieu NOE]

hello again

some tricks to reduce overshoot - here I use the integral term onky when the error is small, this avoids the large build up when you are still far away from the target value. There are other techniques (plenty of anti windup implementations if you look on the web) to improve the basic PID regulator.

you can also have a look at the non linear PID (gain scheduling)

so here 's just the integral term saturation feature. You can easily comment this section and see the overshoot being significantly larger without the saturation - as it was in my answer above :

%% Parameters
J = 0.03;                  % moment of inertia (kg*m^2)
Kp = 2;
Ki = Kp;
Kd = Kp/5;
dt = 0.01;                 % time step
T = 3;                     % total time
t = 0:dt:T;
N = length(t);
phi_ref_deg = 5;
phi_ref = deg2rad(phi_ref_deg);
phi = zeros(1, N);
omega = zeros(1, N);
u = zeros(1, N);
e = zeros(1, N);
I = 0;    
Umax = 0.1; % max amplitude for the command signal

for k = 2:N
    
    e(k) = phi_ref - phi(k-1);
    
    I = I + e(k)*dt;               
    D = (e(k) - e(k-1))/dt;        

    % integral term saturation
    % here we use integral term only if error is within +/- 0.5 deg (0.0087
    % rad)
    if abs(e(k))>0.0087 % radians
        I = 0;
    end     

    % PID control
    u(k) = Kp*e(k) + Ki*I + Kd*D;

    % command amplitude saturation
    if abs(u(k))>Umax
        u(k) = Umax*sign(u(k));
    end
    
    % Dynamics: J*phi_ddot = u
    phi_ddot = u(k)/J;
    
    omega(k) = omega(k-1) + phi_ddot*dt;
    phi(k)   = phi(k-1)   + omega(k)*dt;
    
end
phi_deg = rad2deg(phi);
peak_val = max(phi_deg);
overshoot = max(0, peak_val - phi_ref_deg);
fprintf('Overshoot: %.3f deg\n', overshoot);

Overshoot: 0.058 deg

figure;
subplot(2,1,1)
plot(t, phi_deg, 'LineWidth', 1.4); hold on;
yline(phi_ref_deg, '--r', 'Ref');
xlabel('Time (s)');
ylabel('Roll angle (deg)');
title('Roll Angle Response');
grid on;
subplot(2,1,2)
plot(t, u, 'LineWidth', 1.4);
xlabel('Time (s)');
ylabel('u (N*m)');
title('Control Torque');
grid on;

Answer 2

[Sam Chak]

Hi @Cesar

My simulation results look almost the same as @Mathieu NOE's, but I use ode45 instead of discrete-time propagation.

% A function that stores the governing equations of the control system 
function [dx, u] = rollDyn(t, x)
    % Parameters
    J       = 0.03;                         % moment of inertia (kg·m²)
    Kp      = 2;                            % proportional gain
    Ki      = 1;                            % integral gain
    Kd      = 0.5;                          % derivative gain
    
    % PID controller
    ref     = deg2rad(5);                   % reference angle in radian
    err     = x(1) - ref;                   % error
    u       = - Kp*err - Ki*x(3) - Kd*x(2); % PID controller (ideal)
    
    % ODEs
    dx(1)   = x(2);
    dx(2)   = u/J;
    dx(3)   = err;                          % integral of error
    
    dx      = [dx(1); dx(2); dx(3)];
end

% call ode45 solver
tspan   = 0:0.01:3;
[t, x]  = ode45(@rollDyn, tspan, [0; 0; 0]);

% for plotting the control signal u
u       = zeros(1, numel(t));
for j = 1:numel(t)
    [~, u(j)] = rollDyn(t(j), x(j,:).');
end

% performance characteristics
xfinal  = 5;
S       = stepinfo(rad2deg(x(:,1)), t, xfinal)

S = struct with fields:
         RiseTime: 0.3429
    TransientTime: NaN
     SettlingTime: NaN
      SettlingMin: 4.5138
      SettlingMax: 5.4847
        Overshoot: 9.6936
       Undershoot: 0
             Peak: 5.4847
         PeakTime: 0.9900

maxErr  = S.Peak - xfinal   % maximum relative error

maxErr = 0.4847

figure;
subplot(2,1,1)
plot(t, rad2deg(x(:,1)), 'LineWidth', 1.4); hold on;
yline(5, '--r', 'Ref');
xlabel('Time, (s)');
ylabel('Roll angle, (deg)');
title('Roll Angle Response');
grid on;

subplot(2,1,2)
plot(t, u, 'LineWidth', 1.4);
xlabel('Time, (s)');
ylabel('u, (Nm)');
title('Control Torque');
grid on;

Answer 3

[Sam Chak]

Hi @Cesar

In addition to @Mathieu NOE's valuable contributions regarding realistic actuator saturation and overshoot reduction, it is important to exercise caution when applying both the ideal time-domain PID controller and the ideal complex-valued frequency-domain PID controller, as they are not exactly equivalent, as demonstrated below using your roll angle control problem.

Ideal time-domain PID controller:

Ideal frequency-domain PID controller:

Many students rely on the powerful auto-tuning capability of pidtune() to obtain PID gains effortlessly. Subsequently, they apply the PID controller using these autotuned gains in systems described in the time domain, only to discover discrepancies between the actual results (poorer performance) and the expected results (better performance). In fact, many undergrad control textbooks do not explicitly specify this observation or issue a caution, including in the documentation for pid() and pidtune().

The ideal frequency-domain PID controller cannot be realized physically and as a result, the control torque cannot be plotted.

% Parameters
J       = 0.03;         % moment of inertia (kg·m²)
Kp      = 2;            % P gain assumed to be computed by pidtune()
Ki      = 1;            % I gain assumed to be computed by pidtune()
Kd      = 0.5;          % D gain assumed to be computed by pidtune()
ref     = deg2rad(5);   % reference angle 

% all transfer functions
Gp      = tf(1, [J, 0, 0])              % Plant (roll dynamics)

Gp =
 
     1
  --------
  0.03 s^2
 
Continuous-time transfer function.

Gc      = pid(Kp, Ki, Kd)               % frequency-domain PID controller

Gc =
 
             1          
  Kp + Ki * --- + Kd * s
             s          

  with Kp = 2, Ki = 1, Kd = 0.5
 
Continuous-time PID controller in parallel form.

Gcl     = minreal(feedback(Gc*Gp, 1))   % Closed-loop system

Gcl =
 
     16.67 s^2 + 66.67 s + 33.33
  ---------------------------------
  s^3 + 16.67 s^2 + 66.67 s + 33.33
 
Continuous-time transfer function.

Gu      = minreal(feedback(Gc, Gp))     % Closed-loop control TF

Gu =
 
        0.5 s^4 + 2 s^3 + s^2
  ---------------------------------
  s^3 + 16.67 s^2 + 66.67 s + 33.33
 
Continuous-time transfer function.

%% plot results
tspan   = 0:0.01:3;

% Plot 1: Roll angle
[y, t]  = step(ref*Gcl, tspan);

subplot(211)
plot(t, rad2deg(y), 'LineWidth', 1.4); hold on;
yline(5, '--r', 'Ref');
xlabel('Time, (s)');
ylabel('Roll angle, (deg)');
title('Roll Angle Response (ideal freq-domain PID controller)');
grid on;

% Plot 2: Control torque
[u, t]  = step(ref*Gu, tspan);

Error using DynamicSystem/step (line 97)
Cannot simulate the time response of improper (non-causal) models.

subplot(2,1,2)
plot(t, u, 'LineWidth', 1.4);
xlabel('Time, (s)');
ylabel('u, (Nm)');
title('Control Torque');
grid on;

[Paul]

Hi Sam,

I think the time and frequency domain controllers yield the same result if we take care to properly (or close to properly) handle the derivative of the unit step input (appoximated below as thin, square pulse). Mathematically, they should be equivalent with the only difference being in the computational approach to approximating the derivative of the step input.

% Parameters
J       = 0.03;                         % moment of inertia (kg·m²)
Kp      = 2;                            % proportional gain
Ki      = 1;                            % integral gain
Kd      = 0.5;                          % derivative gain
ref     = deg2rad(5);                   % reference angle in radian

function [dx, u] = rollDyn(t, x, J, Kp, Ki, Kd,ref)
    
    % PID controller
    err     = x(1) - ref;                   % error
    u       = - Kp*err - Ki*x(3) - Kd*(x(2)-ref*(t<.01)*100); % PID controller (ideal)
    
    % ODEs
    dx(1)   = x(2);
    dx(2)   = u/J;
    dx(3)   = err;                          % integral of error
    
    dx      = [dx(1); dx(2); dx(3)];
end
% call ode45 solver
tspan   = 0:0.01:3;
[t, x]  = ode45(@(t,x) rollDyn(t,x,J,Kp,Ki,Kd,ref),[0,3], [0; 0; 0], ...
    odeset('Events',@EventFcn));

figure
plot(t,x(:,1)*57.3),grid
Gp      = tf(1, [J, 0, 0]);              % Plant (roll dynamics)
Gc      = pid(Kp, Ki, Kd);               % frequency-domain PID controller
Gcl     = feedback(Gc*Gp, 1);
hold on
[y,t]=step(ref*57.3*Gcl,3);
plot(t,y)

function [value,isterminal,direction] = EventFcn(t,x)
value      = t-0.01;
isterminal = 0;
direction  = 1;
end