Basic PID controller demo

pid.m

function control_variable = pid(plant_variable,setpoint,K)
%% control_variable = pid(plant_variable,setpoint,K)
% PID controller

persistent h; % History of the *input*
if nargin == 0 % Clear history
    h = []; control_variable = NaN; return
end

%% Current error term
error = setpoint-plant_variable;

%% Derivative error
if numel(h)>1 % To prevent the error when no history
    derivative_error = plant_variable-h(2);
else
    derivative_error = 0;
end

%% Persistent history
if isempty(h)
    % h(1) - accumulated error
    % h(2) - last 'plant_variable'
    h = [error plant_variable];
else
    h(1) = h(1) + error;
    h(2) = plant_variable;
end

%% Integrated error
integrated_error = h(1);

%% Put P,I and D components together

% Proportional, integral and the derivative term
K = abs(K); % All the gains are necesarilly positive
Kp = K(1); Ki = K(2); Kd = K(3);
control_variable = Kp*error + Ki*integrated_error + Kd*derivative_error;

%% Coersce to +/-100 range
control_variable = max(min(control_variable,5),-5);
end

pid_control.m

close all; clear; clc;

%%
t = 1:2:1e3; % Time, in epochs

%% Tune
% Target set point (sp) for tuning
sp = sin(2*pi*t.^2/7e4)*2;
sp = ((sp>0)-0.5); % Square wave chirp

f = @(K) tuning_optimfun(K,t,sp,true);
K0 = [1 0.6 0]; % Starting point
Kf = random_search(f,K0,1e-3,0.1,300);
%Kf = fminsearch(f,K0); % <- Works only when there is no noise in the plant
tuning_optimfun(Kf,t,sp,true);

%% Controll a signal
% Some other setpoint
P = peaks(10);
sp = interp1(linspace(min(t),max(t),numel(P)),P(:),t);

cv = 0; % Initial controled variable (cv)
pv = []; % Plant varaible

% Pick gain parameters
K = [2 1 1]; % Eeyballed parameters
K = Kf; % Tuned parameters

for ti = 1:numel(t)    
    pv(ti) = plant(cv(ti)); % Get plant varaible
    cv(ti+1) = pid(pv(ti),sp(ti),K); % Figure out control variable for the next epoch
end
cv(end) = []; % Don't need the last one
plant(); pid(); % Clear history

%% Plot
figure(1); clf;
subplot(2,1,1); plot(t,zeros(size(t)),'k--',t,sp,t,pv);
xlabel('time / epochs'); ylabel('Process units')
title('Set Point (SP) and Plant Variable (PV)')
grid on; box on;
ylim([-1 1]*10);

subplot(2,1,2); plot(t,zeros(size(t)),'k--',t,cv);
xlabel('time / epochs'); ylabel('Controlled variable units')
title('Controlled variable ')
grid on; box on;
ylim([-1 1]*5);

plant.m

function plant_variable = plant(input)
%% plant_variable = plant(input)
% Simulate the behaviour of the plant to be controled

persistent h; % History of the *input*
if nargin == 0 % Clear history
    h = []; plant_variable = NaN; return
end

%% Store input history to simulate delayed reaction of the plant
n_hist = 10; % History length
if isempty(h)
    h = input; % Initialise
else
    h(end+1) = input;
    % Keep only so many values
    if numel(h) > n_hist, h = h((end+1-n_hist):end); end
end

%% Exponential envelope into the past
tau = 1.6; % Time constant of the expnential envelope
x = 1:numel(h); % x-axis for the history
envelope = exp((x-numel(h))/tau);

%% Output
plant_variable = 5.7*mean(h.*envelope) + 0.02*randn;

end

random_search.m

function [pf,fval,path] = random_search(f,p0,tol,step,iter_lim,stall_lim)
%% [pf,fval,path] = random_search(f,p0,tol,step_size,iter_limit,stall_limit)
% Take random steps and see whether the funciton value improves

if nargin < 3 || isempty(tol), tol = 1e-4; end % Step tolerance
if nargin < 4 || isempty(step), step = 1; end % Step size
if nargin < 5 || isempty(iter_lim), iter_lim = 1e3; end % Iteration limit
if nargin < 6 || isempty(stall_lim), stall_lim = 15; end % Stall limit

stall = 0; path = p0; % Initialise
pf = p0; fval = f(p0); % Starting point p0
for j = 1:iter_lim % Limit number of iterations
    dp = step.*randn(size(pf));
    pnew = pf + dp; fnew = f(pnew);
    if fnew < fval
        pf = pnew; fval = fnew; stall = 0;
        path = [path; pf]; %#ok<AGROW>
    elseif stall >= stall_lim % Standing in one place for too long
        stall = 0; step = step/2;
    else
        stall = stall + 1;
    end
    if step < tol; break; end
end
end

tuning_optimfun.m

function delta = tuning_optimfun(K,t,sp,plt)
%% delta = tuning_optimfun(K,t,sp,plt)
% Loss funciton for tuning

cv = 0; % Initial controled variable (cv)
pv = []; % Plant varaible
for ti = 1:numel(t)    
    pv(ti) = plant(cv(ti)); % Get plant varaible
    cv(ti+1) = pid(pv(ti),sp(ti),K); % Figure out control variable for the next epoch
end
cv(end) = []; % Don't need the last one
pid(); pid(); % Clear history

% Filter plant variable (to rid of noise and ripple)
%pv = sgolayfilt(pv,1,5);

% Root-mean-square Difference between the setpoint and the plant variable
region = 50:numel(sp);
delta = sqrt(mean((sp(region)-pv(region)).^2));

% Optionally plot
if plt
    figure(1); clf; hold on;
    plot([min(t) max(t)],[0 0],'k--');
    plot(t,sp,t,pv);
    %plot(t,(sp-pv).^2);
    title(sprintf('Kp,i,d = (%.2g,%.2g,%.2g)',K));
    grid on; box on;
    drawnow;
end
end