%% Data-driven prediction using past data
% Using behavioral approach.

%%
clear all

%% Building a discrete-time LTI model to generate the DATA

ts = 1;                     % sampling period
tfinal = 500;              % final time

ze = 0.5;                   % damping
wn = 0.1;                   % undamped natural frequency
Gct = tf(wn^2,[1 2*ze*wn wn^2]);
G = c2d(Gct,ts);            % discretizing a cont.-time system 
n = 2;                      % order of the system (perhaps autoamatically?)

%% Simulating the model to generate the DATA

t = 0:ts:tfinal;            % time samples
T = length(t);              % final discrete time (but assumes the initial time 1)

Range = [-1,1];             % generating a PRBS sequence
Band = [0 1]; % [0 1/4]
ud = idinput(T,'prbs',Band,Range);

[yd,t] = lsim(G,ud,t);      % simulating the response to the PRBS input

%% Plotting the simulation outcomes

figure(1)

subplot(2,1,1)
stairs(1:length(yd),yd,'.-','LineWidth',1)
xlabel('k')
ylabel('y')
grid on
subplot(2,1,2)
stairs(1:length(ud),ud,'.-','LineWidth',1)
xlabel('k')
ylabel('u')
grid on

%% Checking if the input is sufficiently exciting

Tini = 2;                   % lag of the system (suffices <=n but we pretend we do not know n)
Tf = 50;                    % time-horizon for prediction
T1 = Tini+Tf;               % total prediction horizon including the samples to determine the initial state

L = n + T1;                 % persistence of excitation order

Hu = hankel(ud(1:L),ud(L:(T-2))); % exlude the last Tini simulated inputs

rank(Hu)                    % check the rank
cond(Hu)                    % and conditioning, just in case

%% Building the Hankel matrices

L = T1;                     % prediction horizon (contains both Tini and Tr), number of rows of Hankel matrix
Tm = T-Tini;                % we do not use all the data for building the Hankel matrix
T2 = Tm-T1+1;               % number of cols of the Hankel matrix

U = hankel(ud(1:L),ud(L:Tm));
Y = hankel(yd(1:L),yd(L:Tm));

Up = U(1:Tini,:);
Uf = U(Tini+1:end,:);
Yp = Y(1:Tini,:);
Yf = Y(Tini+1:end,:);

%% Setting up the optimization problem

uini = ud(Tm+1:Tm+Tini);
yini = yd(Tm+1:Tm+Tini);

A = horzcat([Up;Yp;Uf;Yf],[zeros(2*Tini,2*Tf);-eye(2*Tf,2*Tf)]);
b = zeros(2*T1,1);
b(1:2*Tini) = [uini;yini];

nrA = 2*L;                  % number of rows of the Hankel matrix
ncA = T2;                   % number of columns of the Hankel matrix

Q = 10000;                  % weight for the output (assumes scalar, must be changed for vector output)
R = 0.1;                    % weight for the control (assumes scalar, must be changed for vector input)

lambda = 0.1;               % regularization factor

H = diag([repmat(lambda,T2,1);repmat(R,Tf,1);repmat(Q,Tf,1)]); % hessian matrix for quadprog

r = ones(Tf,1);             % reference for the output (assumes scalar, must be changed for vector output)
r(1:10) = 0;                % delayed a bit
r(30:end) = 0;

f = [zeros(T2,1);zeros(Tf,1);-r.*repmat(Q,Tf,1)]; % linear term in the cost function

umin = -10.0;
umax = 10.0;
lb = [repmat(-inf,T2,1);repmat(umin,Tf,1);repmat(-inf,Tf,1)];
ub = [repmat(inf,T2,1);repmat(umax,Tf,1);repmat(inf,Tf,1)];

%% Solving the optimization problem

x = quadprog(H,f,[],[],A,b,lb,ub);

u = x(T2+1:T2+Tf);
y = x(T2+Tf+1:end); 

%% Plotting the simulation outcomes
% Just the controlled time window

figure(2)

subplot(2,1,1)
stairs(1:length(y),y,'.-','LineWidth',1)
hold on
stairs(1:length(r),r,'.-r','LineWidth',1)
hold off
xlabel('k')
ylabel('y')
grid on
subplot(2,1,2)
stairs(1:length(u),u,'.-','LineWidth',1)
xlabel('k')
ylabel('u')
grid on

%%
% Also the past data

Tp = 100;           % how far into the past are we plotting

figure(3)
subplot(2,1,1)
stairs(1:(length(y)+Tp),[yd(end-Tp+1:end);y],'.-','LineWidth',1)
hold on
stairs(Tp+1:Tp+length(r),r,'.-r','LineWidth',1)
hold off
xlabel('k')
ylabel('y')
grid on
subplot(2,1,2)
stairs(1:(length(u)+Tp),[ud(end-Tp+1:end);u],'.-','LineWidth',1)
xlabel('k')
ylabel('u')
grid on