%% SMS Course Aid for Implementing Basic Controller
% Author: Patryk Chaber (patryk.chaber@pw.edu.pl); Date: 27.01.2022; All rights reserved.
%% Controller loop
% This document is prepared as an aid in future implementations and usage of basic control structures
% This document is NOT ALLOWED to be used directly as a solution or a part of a solution. This document is NOT ALLOWED to be distributed outside of the listeners to the SMS course.
% Any violation of the above will be PENALISED.

clc % clear console
clear allvars % |clear all| variables (faster than |clear all|)
options = optimoptions('quadprog'); options.Display = 'none'; % necessary for the clean output in the published file

%
%% Process of control

Tp = 0.1; % sampling period
Gs = tf(1,[1,2,1]); % continuous transfer function of the process
Gz = c2d(Gs,Tp);  % discrete transfer function of the process

%
%% Simulated process from transfer function
% Following equations consider only the single inertia process, althought expanding this to a second-order inertia is pretty easy.
%  G(z) = N(z)/D(z) = Y/U
%  N(z)*U = D(z)*Y
%  N_1Uz^{0} + N_2Uz^{-1} = D_1Yz^0 + D_2Yz^{-1}
%  D_1 = 1 \qquad N_1 = 0
%  Yz^0 = -D_2Yz^{-1} + N_2Uz^{-1}
%  a = D_2 \qquad b = N_2
%  y(k) = -a*y(k-1) + b*u(k-1)

% a = Gz.Den{1}(2); % single inertia
% b = Gz.Num{1}(2); % single inertia

a = Gz.Den{1}(2:end); % use (2:end) if |length(a) > 2|
b = Gz.Num{1}(2:end); % use (2:end) if |length(b) > 2|

%
% If the simulated process of control was more complex (i.e. Gs = tf(1,[1,2,1]), then there would be more parameters)
% Additional noise function is defined here, so that is will not clutter the control loop. A following line defines a lambda function with no arguments.

%measurement_noise = @()((rand()-0.5)*0.1+0.5); % uniform noise in range of [0.45, 0.55]
measurement_noise = @()(0); % zero measurement noise

%
% Simulation parameters

kk = 200; % end of simulation
y  = zeros(1,kk); % measured output (PV)
ym = zeros(1,kk); % true process output
u  = zeros(1,kk); % control signal (MV)
sp = zeros(1,kk); % setpoint (SP)
du = zeros(1,kk); % control increment
t  = (0:kk-1)*Tp; % time

%
% Setpoint trajectory

sp(  1:end) =  1; %   1-> 99:  1
sp(100:end) = -1; % 100->200: -1

figure;
stairs(t, sp);
title('Setpoint trajectory');
ylim([-1.1,1.1]);
ylabel('y^{sp}');
xlabel('time');

%
%% Dynamic Matrix Control (DMC)
% To obtain step response coefficients a step function should be used, and the first element of the result should be discarded.

s = step(Gz);
s = s(2:end);
%E% s = [zeros(3,1);s]; % too high delay in the step response

figure;
stairs(s);
title('Step response suitable for DMC');
ylabel('s_i');
xlabel('i');

%
% In this script three versions of DMC are implemented:
% Fast DMC (FDMC) -- analytic version with efficient control law
% Analytic DMC (ADMC) -- analytic version with unefficient control law
% Numeric DMC (NDMC) -- numeric version but with efficient control law

DMC_Alg = 0; % 0 -- FDMC; 1 -- ADMC; 2 -- NDMC
DMC_Alg_str = 'xDMC';
if(DMC_Alg == 0); DMC_Alg_str = 'FDMC';
elseif(DMC_Alg == 1); DMC_Alg_str = 'ADMC';
elseif(DMC_Alg == 2); DMC_Alg_str = 'NDMC';
end

%
% Dynamics horizon is determined based on the length of obtained step response vector.

D = length(s);
%E% D = 60; % too short value causes periodic "bumps" in signals (works with at least II order inertia)

%
% Other parameters of the DMC are of lower importance -- these are some default values.

N  = D;
Nu = D;
lambda = 1;
psi = 1;

%
% Turning on and off constraints for control signal increment \Delta u^\mathrm{min} = -\Delta u^\mathrm{max} and control signal value u^\mathrm{min} = -u^\mathrm{max}.

dumax = Inf;
% dumax = 0.2;
umax  = Inf;
% umax  = 1.0;

dumin =-dumax;
umin  =-umax;

%
%% DMC preparation
% These are matrices that can be calculated off-line, i.e. are constant during the experminent.
% M = \left[\begin{array}{cccc} s_1 & 0 & \ldots & 0 \\ s_2 & s_1 & \ldots & 0 \\ \vdots  & \vdots & \ddots & \vdots \\ s_N & s_{N-1} & \ldots & s_{N-(D-1)} \end{array}\right]

M = zeros(N,Nu);
for j = 1:Nu
    for i = j:N
        M(i,j) = s(i-j+1);
    end
end

%
% M^\mathrm{p}=\left[\begin{array}{ccc}s_{2}-s_{1}&\ldots&s_{D}-s_{D-1}\\s_{3}-s_{1}&\ldots&s_{D+1}-s_{\mathrm{D}-1}\\\vdots&\ddots&\vdots\\s_{N+1}-s_{1}&\ldots&s_{N+D-1}-s_{D-1}\end{array}\right]

Mp = zeros(N,D-1);
for i = 1:N
    for j = 1:D-1
        Mp(i,j) = s( min(D,j+i) ) - s(j);
    end
end

%
% Diagonal matrices \Psi and \Lambda are of size N\times{}N and N_\mathrm{u}\times{}N_\mathrm{u}, respectively. It is a common practice to define those matrices as identity matrix multiplied by a scallar (\lambda and \psi are used, respectively).
% \Psi = \left[\begin{array}{cccc}           1&0&\ldots&0\\           0&1&\ldots&0\\           \vdots&\vdots&\ddots&\vdots\\           0&0&\ldots&1         \end{array}\right]\cdot\psi
% \Lambda = \left[\begin{array}{cccc}              1&0&\ldots&0\\              0&1&\ldots&0\\              \vdots&\vdots&\ddots&\vdots\\              0&0&\ldots&1            \end{array}\right]\cdot\lambda

Psi = eye(N,N)*psi;
Lambda = eye(Nu,Nu)*lambda;

%
% Gain matrix is defined as
% K = (M^\mathrm{T}\Psi{}M+\Lambda)^{-1}M^\mathrm{T}\Psi

K = (M'*Psi*M+Lambda)^(-1)*M'*Psi;

%
% For the FDMC additional variables are defined as follows:
% K_\mathrm{e} = \sum\limits_{j=1}^N K_{1,j}
% K_\mathrm{u} = \overline{K_{1}}M^\mathrm{p}
% where
% K = \left[\begin{array}{c}\overline{K_1}\\\vdots\\\overline{K_{N_\mathrm{u}}}\end{array}\right] = \left[\begin{array}{ccc}K_{1,1}&\ldots&K_{1,N}\\\vdots&\ddots&\vdots\\K_{N_\mathrm{u},1}&\ldots&K_{N_\mathrm{u},N}\end{array}\right]

Ke = sum(K(1,:));
%E% Ke = 0; % Controller stops working
Ku = K(1,:)*Mp;
%E% Ku(:) = 0; % Works but at a slower speed

%
% Vector \Delta u^\mathrm{p}(k) is defined as follows:
% \Delta u^\mathrm{p}(k) = \left[\begin{array}{c}\Delta u(k-1)\\\vdots\\\Delta u(k-(D-1))\end{array}\right]
% but for k < 0 it can be assumed that \Delta u(k) = 0

dUp = zeros(1,D-1);

%
%% Simulation loop
% In this section, a simulation loop is defined. It will start at k=1, and all inconsistencies related to indices lower than 1, will be resolved in those places, instead of trying to define control loop boundaries to be safe for all lated defined calculations.
% This control loop is finite, thus the number of its iteration is defined.

tic; % Measure simulation time
for k=1:kk

%
% Calculating new output signal of the process based on the values collected from the previous iteration.

    ym(k) = 0;
    for i = 1:length(a)
        if (k-i > 0); ym(k) = ym(k) + -a(i)*ym(k-i); end
    end
    for i = 1:length(b)
        if (k-i > 0); ym(k) = ym(k) +  b(i)*u(k-i); end
    end
    y(k) = ym(k) + measurement_noise();

%
%% On-line calculations
% For all DMC variants the \Delta u^\mathrm{p}(k) vector is necessary. The following set of instructions shifts values in this vector one place down, and sets its first element to the value of the \Delta u(k-1). If k-1 < 1, then it assumes that \Delta u(k-1) = 0.

    %E% for i=2:D-1 % inverted order -> oscillations
    for i=D-1:-1:2
        dUp(i) = dUp(i-1);
    end
    
    if(k-1>0); dUp(1) = du(k-1);
    else;      dUp(1) = 0;
    end

%
%% NDMC

    if(DMC_Alg == 2)

%
% Calculations are based on the M.Ławryńczuk STP script.

        SP = ones(1,N)*sp(k); % setpoint constant over the control horizon (but doesn't have to be)
        Y = ones(1,N)*y(k);
        Y0 = Y+Mp*dUp';
        H = 2*(M'*Psi*M + Lambda);
        H = (H+H')/2; % making sure, that despite to numeric errors, H is still symetrical
        f = -2*M'*Psi*(SP'-Y'-Mp*dUp');

%
% Preparation of left-hand side of the constraints inequalities

        J = tril(ones(Nu,Nu));
        if(k-1>0); U = ones(Nu,1)*u(k-1);
        else; U = zeros(Nu,1);
        end
        Umin = ones(Nu,1)*umin;
        Umax = ones(Nu,1)*umax;
        A_opt = [-J;J];
        b_opt = [-Umin+U;Umax-U];

%
% Calling the quadratic programming solver. It is worth noting, that the constraints for the decision variable (\Delta u(k|k), \ldots , \Delta u(k+N_{\mathrm{u}-1}|k)) is defined as a seperate argument, and are not included in the inequality constraints, although they could be moved there.

        dU = quadprog(H,f,A_opt,b_opt,[],[],ones(Nu,1)*dumin, ones(Nu,1)*dumax, [], options);

%
% Apply first element of the solution to the control process

        du(k) = dU(1);

%
%% ADMC

    elseif(DMC_Alg == 1)

%
% Setpoint is constant over the control horizon, but doesn't have to be. It is just a common practice to assume that.
% Y^\mathrm{sp}(k) = \left[\begin{array}{c}y^\mathrm{sp}(k)\\\vdots\\y^\mathrm{sp}(k) \end{array}\right]

        SP = ones(1,N)*sp(k);

%
% Y(k) = \left[\begin{array}{c}y(k)\\\vdots\\y(k) \end{array}\right]

        Y = ones(1,N)*y(k);

%
% Based on the above it should be clear, why the constant setpoint allows for such a simplification that is exploited in the FDMC.
% Free trajectory:
% Y^0(k) = Y(k) + M^\mathrm{p}\Delta u^\mathrm{p}(k)

        Y0 = Y+Mp*dUp';

%
% Control law:
% \Delta U(k) = K\cdot(Y^\mathrm{sp}(k)-Y^0(k))

        dU = K*(SP-Y0);

%
% Apply first element of the solution to the control process

        du(k) = dU(1);

%
%% FDMC

    elseif(DMC_Alg == 0)

%
% Simple control law, i.e. calculation of the actually applied control signal:
% \Delta u(k|k) = K_\mathrm{e}(y^\mathrm{sp}(k) - y(k)) - K_\mathrm{u}\Delta u^\mathrm{p}(k)

        %E% du(k) = Ke*(sp(k)-y(k)) + Ku*dUp'; % causes oscillations
        du(k) = Ke*(sp(k)-y(k)) - Ku*dUp';
    end

%
%% Constraints
% Projecting control signal increment onto the constraints

    if( du(k) > dumax); du(k) = dumax; end
    if( du(k) < dumin); du(k) = dumin; end

%
% Determine u(k) based on \Delta u(k|k)

    if(k-1>0); u(k) = du(k) + u(k-1);
    else; u(k) = du(k);
    end

%
% Projecting control signal value onto the constraints

    if( u(k) > umax); u(k) = umax; end
    if( u(k) < umin); u(k) = umin; end

%
% Determination of actually used \Delta u(k), because of the possible changed in this increment based on the constraints applied to the control signal value.

    du(k) = u(k);
    if(k-1>0); du(k) = du(k) - u(k-1); end

end
simulation_time = toc; % Stop measuring simulation time

%
%% Results
% Displaying basic information about what was tested

fprintf('Algorithm: %s\n', DMC_Alg_str);
fprintf('Time of simulation: %fs\n',simulation_time);

%
% Plotting results of control

figure;
subplot(2,1,1);
plot(ym,'Color',[.5,.5,.5,.5]); hold on;
plot(y);
stairs(sp,'k--'); hold off;
title(sprintf('%s, t=%fs',DMC_Alg_str,simulation_time));
ylabel('y');
legend('true output','measured output','setpoint');
xlabel('k');

subplot(2,1,2);
stairs(u);
ylabel('u');
xlabel('k');