function [y_stored, u_stored] = main()
% close all figure windows...
close all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% STEP 1.1 SELECT CONTROLLER/OBSERVER PARAMETERS
% p is the order of the ARX model
% q is the prediction horizon
% l is the number of data points used to estimate the parameters of the ARX model
% r is the number of inputs to the system
p = 6; % needs to be at least 6 (original system is 6th order here)
q = 200;
l = 30;
r = 1; % SISO
% number of simulation steps before the controller is switched on
Ninit = 401;
% number of simulation steps after the controller has been switched on
nSimSteps = Ninit;
% setting this flag to '1' adds some measurement noise to the simulated
% data. This will somewhat throw off the system identification. To
% counteract this effect you will have to choose a system order for the ARX
% model which is larger than the minimum (6)
%addNoise = 1;
% check parameters...
if(2*l >= Ninit)
l = floor(Ninit/2);
disp(['Reducing parameter ''l'' to the maximum limit of Ninit/2 = ' num2str(l)]);
end
if(q < p)
q = p; % deadbeat
disp(['Increasing parameter ''q'' to the minimum of p = ' num2str(p) ' (deadbeat controller)']);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% example system (see paper)
m = 1; % m1 = m2 = m3 = 'm' = 1 kg
c = 0.1; % c1 = c2 = c3 = 'c' = 0.1 N*s/m
k = 1000; % k1 = k2 = k3 = 'k' = 1000 N/m
% MM = [m 0 0; 0 m 0; 0 0 m];
% CC = [2*c -c 0; -c 2*c -c; 0 -c 2*c];
% KK = [2*k -k 0; -k 2*k -k; 0 -k 2*k];
%
% A = [zeros(size(MM)), eye(size(MM)); -MM\KK, -MM\CC];
% B = [0; 0; 0; 1; 0; 0]; % input: force applied to first mass
% C = [0 0 1 0 0 0]; % output: displacement (X1) of third mass (-> x3)
% D = 0;
A = [ 0 1 0 0 0 0; ...
-2*k/m -2*c/m k/m c/m 0 0; ...
0 0 0 1 0 0; ...
k/m c/m -2*k/m -2*c/m k/m c/m; ...
0 0 0 0 0 1; ...
0 0 k/m c/m -2*k/m -2*c/m ];
B = [ 0; ...
1; ...
0; ...
0; ...
0; ...
0 ];
C = [0 0 0 0 1 0];
D = 0;
% define continuous-time system equation of the plant
SYSc = ss(A, B, C, D);
% sample rate / sample interval
fs = 100;
Ts = 1/fs;
% compute discrete-time system (zoh)
SYSd = c2d(SYSc, Ts);
% % EXAMPLE 2
% % three lightly damped oscillations at modal frequencies of approx.
% % 14, 40 and 57 rad/s (2.2, 6.3 and 9 Hz)
%
% A = [ 0 1 0 0 0 0; ...
% -2000 -0.2 1000 0.1 0 0; ...
% 0 0 0 1 0 0; ...
% 1000 0.1 -2000 -0.2 1000 0.1; ...
% 0 0 0 0 0 1; ...
% 0 0 1000 0.1 -1000 -0.1 ];
%
% % energy enters as variations of the velocity of mass 1 (interpreting the
% % above matrix as the state update matrix of a coupled mass system); from
% % there (state x2) it trickles through to the coupled states x3, x4, x5
% % and x6
% B = [ 0; ...
% 1; ...
% 0; ...
% 0; ...
% 0; ...
% 0 ]
%
% % output is displacement of mass 3 (state x5)
% C = [ 0 0 0 0 1 0 ];
%
% % no direct feedthrough between input and output
% D = 0;
%
% % define system (continuous-time)
% SYS = SS(A,B,C,D)
% produce frequency response of the system (plant)
plotFrequencyResponse = 1;
if exist('plotFrequencyResponse')
figure
[MAG,PHASE,W] = bode(SYSc);
subplot(2, 1, 1)
loglog(W/2/pi, 20*log10(MAG(:)))
grid
title('Frequency response of the system')
xlabel('f (Hz)')
ylabel('mag [dB]')
subplot(2, 1, 2)
semilogx(W/2/pi, PHASE(:))
grid
xlabel('f (Hz)')
ylabel('phase [°]')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% INITIAL INPUTS u's and OUTPUT's y's %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% use random input signal... 401 values of a signal composed of the
% sinusoids from 1 to 20 Hz (every Hz) with randomized phase; this is
% effectively a band limited white noise signal; signal levels range
% from '-10' to '10'
u = idinput(Ninit, 'sine', [0 20/50], [-5 5], [20 50 1]);
t = Ts*[0:length(u)-1];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plot spectrum of the random signal used to excite the system (stimulus)
plotStimulusSpectrum = 1;
if exist('plotStimulusSpectrum')
figure
spectrum(u, [], [], [], fs); % plot the PSD of the stimulus
grid
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% simulate system to produce system response to the random signal
% continuous time version...
% y = lsim(SYS, u, t);
% discrete time version...
x0 = zeros(size(SYSd.b)); % initial state
[yp, xp] = dlsim(SYSd.a, SYSd.b, SYSd.c, SYSd.d, u, x0);
% add 5% measurement noise (if applicable)
if exist('addNoise')
yp = yp + 0.05*max(abs(yp))*rand(length(yp),1);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plot time-domain evolution of the stimulus signal
%plotStimulusTimeEvolution = 1;
if exist('plotStimulusTimeEvolution')
figure
subplot(2, 1, 1)
stairs(t, u)
grid
xlabel('time (s)')
ylabel('u_{stimulus}')
% plot system response to the stimulus signal
subplot(2, 1, 2)
stairs(t, yp)
grid
xlabel('time (s)')
ylabel('y_p')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% INITIALIZE LOG VARIABLES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% u_stored keeps a log of the entire history of applied control signal
% the most recent element is at the end of this vector
u_stored = u; % last element: most recent
% y_stored keeps a log of the entire history of the system output
% the most recent element is at the end of this vector
y_stored = yp; % last element: most recent
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% perform an off-line sys-ID [function 'sys_id_func' defined below]
% ... i. e. using half the 'sys-ID' data record (u, yp)
%
% -> this could be moved into the simulation loop to be performed on-line
% not required here, as the system remains the same throughout the
% entire duration of the simulation (doesn't need to be adaptive)
mid = ceil(length(u_stored)/2);
u_id = u_stored(1:mid);
y_id = y_stored(1:mid);
[Ap, Bp, Cp] = sys_id_func(q, p, l, r, u_id, y_id);
% validate identified system (just to be sure...)
u_validate = u_stored(mid:end);
y_validate = y_stored(mid:end);
t_id = t(1:mid);
t_validate = t(mid:end);
% determine initial condition of the ARX state-space model
% (note: the states of the ARX system are not the same as those of the
% the [discretized] plant !!)
% find initial condition by simulating the ARX model from 0 onwards...
% note that the last entry in x_arx, i. e. state 'x[k]', corresponds
% to input signal 'u[k]'
[y_arx, x_arx] = dlsim(Ap, Bp, Cp, 0, u_id, zeros(p,1));
x0 = x_arx(end, :);
% now filter the validation input data (using the identified ARX
% state-space model)
y_arx = dlsim(Ap, Bp, Cp, 0, u_validate, x0);
% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % y_arx (using a formulation based on the difference equation)
% % filter with coefficients of the following difference equation:
% % a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ...
% % + b(nb+1)*x(n-nb)- a(2)*y(n-1) - ... - a(na+1)*y(n-na)
% arx.b = [];
% arx.b(1,1) = 0;
% arx.a(1,1) = 1;
% for ii=1:p
% arx.b(1,(ii+1)) = Bp(ii,1);
% arx.a(1,(ii+1)) = -Ap(ii,1);
% end
%
% % find initial condition by simulating the arx model from 0 onwards...
% [y_arx, xf] = filter(arx.b, arx.a, u_id, zeros(p,1));
% x0 = xf; % last final state = new initial state
%
% % now filter the validation input data -- need to ensure that the
% % 'initial state' (x0) matches the input data; as this formulation
% % defines the state vector as "[ x[k - (n-1)] ... x[k] ]" (n delays)
% % the first input should be 'u[mid+1]' (not 'u[mid]'). The
% % corresponding time vector and output vector have to be adjusted
% % accordingly
% u_validate(1) = [];
% t_validate(1) = [];
% y_validate(1) = [];
% y_arx = filter(arx.b, arx.a, u_validate, x0); % arx model output
%
% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% display...
figure
subplot(2, 1, 1)
stairs(t_id, u_id)
hold on
stairs(t_validate, u_validate)
hold off
grid
xlabel('time (s)')
ylabel('u_{stimulus}')
title('System identification')
% plot system response to the stimulus signal
subplot(2, 1, 2)
stairs(t_id, y_id)
hold on
stairs(t_validate, y_validate)
stairs(t_validate, y_arx, 'r.')
hold off
grid
xlabel('time (s)')
ylabel('y_p, y_{validate}')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% INITIALIZE SIMULATION LOOP VARIABLES
% determine initial condition of the ARX state-space model
% (note: the states of the ARX system are not the same as those of the
% the [discretized] plant !!)
% find initial condition by simulating the ARX model from 0 onwards...
% note that the last entry in x_arx, i. e. state 'x[k]', corresponds
% to input signal 'u[k]'
[y_arx, x_arx] = dlsim(Ap, Bp, Cp, 0, u_id, zeros(p,1));
x_k = x_arx(end, :)';
u_k = u_id(end);
tsim = t_id(end);
% initialize prediction horizon with the most recent p elements
% the indexing used in the prediction algorithm expects the first element
% of this vector to be the most recent value
u_p_last = u(end:-1:end-p+1); % first element: most recent
y_p_last = yp(end:-1:end-p+1); % first element: most recent
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% MAIN SIMULATION LOOP
for m = 1:nSimSteps
% compute next control action (u[k])
u_k = predictive_func(q, p, l, r, Ap, Bp, Cp, u_p_last, y_p_last);
% apply new control action to the identified plant (calculate x[k+1], y[k])
%[y_k, x] = dlsim(SYSd.a, SYSd.b, SYSd.c, SYSd.d, [u_k u_k], x_k);
%y_k = y_k(end,:);
x_kp1 = Ap*x_k + Bp*u_k; % next state
y_k = Cp*x_k; % current output
% next time step -> update simulation time and propagate state
tsim = tsim + Ts;
x_k = x_kp1;
% update storage variable for prediction horizon
u_p_last = [u_k; u_p_last(1:end-1)]; % first element: most recent
y_p_last = [y_k; y_p_last(1:end-1)]; % first element: most recent
% update log variables (only used for plotting)
u_stored = [u_stored; u_k]; % last element: most recent
y_stored = [y_stored; y_k]; % last element: most recent
end
% plot results
% simulate actual system to produce open loop system response (controller
% switched off, zero input)
% continuous time version...
% y_actual = lsim(SYS, u_stored, tt);
% discrete time version...
x0 = zeros(size(SYSd.b)); % initial state
u_openloop_A = [u_id; u_validate(2:end)]; % stimulus
u_openloop_B = [u_openloop_A; zeros(length(u_stored)-Ninit, 1)];
[y_actual, x] = dlsim(SYSd.a, SYSd.b, SYSd.c, SYSd.d, u_openloop_B, x0);
tt = Ts*[0:length(y_actual)-1];
% plot controller signal u
figure
subplot(2, 1, 1)
stairs(tt(1:Ninit), u_stored(1:Ninit), 'b-')
hold on
stairs(tt(Ninit:end), u_stored(Ninit:end), 'g-')
stairs(tt(Ninit:end), u_openloop_B(Ninit:end), 'r-')
hold off
grid
xlabel('time (s)')
ylabel('u')
title('Control')
% plot system response to the stimulus signal
subplot(2, 1, 2)
stairs(tt(1:Ninit), y_actual(1:Ninit), 'b-')
hold on
stairs(tt(Ninit:end), y_actual(Ninit:end), 'r-')
stairs(tt(Ninit:end), y_stored(Ninit:end), 'g-')
hold off
grid
xlabel('time (s)')
ylabel('y_{o/l}, y_{c/l}')
end % ends 'main' function
%_________________________FUNCTION DEFINITIONS____________________________
%_________________________________________________________________________
% SYS_ID_FUNC PERFORMS SYSTEM IDENTIFICATION TO PRODUCE MATRICES Ap,Bp,Cp
function [Ap, Bp, Cp] = sys_id_func(q, p, l, r, u, y)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PREDICTIVE CONTROL
% STEP 1.2 FORM DATA MATRIX Y
% EQN 10
% Y is row vector containing values of column vector [y(p),y(p+1)...y(l)]
Y=[];
h=p+1; % Note y(p) is actually the p+1 element of y vector
for j=1:(l-p+1) % Refers to column number in Y
Y(1,j) = y(h,1); % Forms Y using value from input vector y
h=h+1; % Increments to select value from next column of y
end
%Y
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% STEP 1.3 FORM DATA MATRIX V
% EQN 11
count=0; V=[];
for ii = 1:2*p % Refers to row number in V
R = rem(ii,2); % Divides row number by 2 to test whether even or odd row.
if R == 1 % Odd rows contain inputs
for j=1:(l-p+1) % Refers to column number in V
h=p-2+j-count+1; %decrements starting p-1 to zero
V(ii,j)=u(h,1);
end
else % Even rows contain outputs
for j=1:(l-p+1) %column number in V
h=p-2+j-count+1; % note u(p-1) is actually the pth element of
% u. in this case row 2,4,6,8
V(ii,j)=y(h,1);
end
count = count + 1;
end
end
%V
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% STEP 1.4 FIND MATRIX P
% EQN 12
V_plus = pinv(V); % Finds psuedo inverse of V
P = Y*V_plus; % Finds matrix P
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% STEP 2. DEFINE ARX MODEL AS STATE SPACE SYSTEM IN OBSERVABLE CANONICAL FORM
% EQNS 16 and 17
% Determine alpha and input vector Bp
j=2;
for ii = 1:p % Number of rows of alpha
alpha(ii,1) = P(1,j);
Bp(ii,1) = P(1,(j-1));
ii = ii+1;
j = j+2;
end
iden = eye(p,(p-1)); % Identity matrix is part of matrix Ap
Ap = cat(2, alpha, iden); % form state update matrix Ap
% Form output matrix Cp
Cp = eye(1,p);
end % function 'sys_id_func'
%__________________________________________________________________________
% PREDICTIVE_FUNC
function u_new = predictive_func(q, p, l, r, A, B, C, u, y)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% STEP 3. FINDING MATRIX G AND CONTROLLER GAIN MATRICES Gi, Hi
% EQNS. 19 and 22
% alpha vector -> 1st column of 'Ap'
alpha = A(:,1);
G_row_orig=A^(q-1)*B; % First column of matrix used to find G
for ii=2:(q-1) % Subsequent columns
G_row_new = A^(q-ii)*B;
G_row_orig = cat(2, G_row_orig, G_row_new); % columns are placed in
% concatenated matrix
end
G_row = -1*pinv(cat(2, G_row_orig, B)) * (A^q); % -[Ap^q-1*Bp..,ApBp,Bp]+*Ap^q
G = G_row(1,:); % First r-row partition of -[Ap^q-1*Bp..,ApBp,Bp]+*Ap^q
r=1; % r is the number of inputs, in this case a SISO system
G_new = G(r,(1:p))*alpha((1:p),1); % Left-most matrix G1
H_new = G(r,(1:p))*B((1:p),1); % Left-most matrix H1
for ii = 2:p % Calculates G2, G3...Gp
alpha_new = alpha((ii:p),1); % smaller and smaller alpha matrices
G_next = G(r,(1:p-ii+1))*alpha_new; % calculates G2,G3...Gp
G_new = cat(2,G_new,G_next); % stores in concatenated matrix
beta = B((ii:p),1); % beta matrices decreasing in size
H_next = G(r,(1:p-ii+1))*beta; % calculates H2,H3...Hp
H_new = cat(2,H_new,H_next); % stores in concatenated matrix
end
% STEP 4. EVALUATE CONTROL LAW
% EQN 21
u_new = 0;
for ii = 1:p
u_new = u_new + G_new(r,ii)*y(ii,1) + H_new(r,ii)*u(ii,1);
end
end % function 'predictive_func'