% Example from "An algorithm for closed-loop data-driven simulation",
% by Ivan Markovsky, submitted for publication to SYSID'09.

function test_cdds()

% --- Generate the data ---
sys = drss(1); % plant
t   = 20; % data length
tr  = 10; % simulation time
rr = [0; ones(tr-1,1)]; % reference trajectory

b    = 1; % controller for the given trajectory 
r    = [zeros(tr,1); ones(t-tr,1)]; rand(t,1); %[zeros(n,1); ones(t-n,1)];
sysc = feedback(-b*sys,1); 
y    = lsim(sysc,r);
u    = -b * (r - y);
w    = [u,y]; % trajectory of sys in feedback with b

a    = -1; % controller for the to-be-found trajectory
sysc = ss(feedback(-a*sys,1)); % sys in feedback with a

% --- Closed-loop data-driven simulation ---

% Construct the block-Hankel matrices needed for Algorithm 1
Hw = blkhank(w,tr); 
Hy = blkhank(y,tr);
Hu = blkhank(u,tr);
Tw = kron(eye(tr),[1 -a]);
Tr = eye(tr) * a;

% Form the matrix A and the vector b in (12)
A  = Tw * Hw;
B  = - Tr * rr;

% Algorithm 1
g0 = pinv(A) * B; % LN solution of (12)
ur = Hu*g0; yr = Hy*g0;
wr = [ur yr];

N  = null(A);
Nu = Hu * N; Ny = Hy * N;

% Find step response as yr + Ny z, for some z
if Ny(1) > 1e-8,  
    Nyz  = - yr(1) / Ny(1); 
    ys = yr + Ny * z;
else
    ys = yr;
end

% Verify that (rr,yr) is correct
ys0 = lsim(sysc,rr); 
err = norm(ys - ys0)

% --- Plot the results ---

figure(1)
stairs((tr-t+1):tr,r,':k','linewidth',2), hold on
stairs((tr-t+1):tr,y,'-b','linewidth',2)
legend('rd','yd','Location','best')

figure(2)
stairs(rr,':k','linewidth',2), hold on
stairs(ys,'-b','linewidth',2), hold on
stairs(ys0,'--r','linewidth',2)
legend('rr','yr','y0','Location','best')


% BLKHANK - Construct block-Hankel matrix H from W with I block-rows.
%
% H = blkhank(w,i)

function H = blkhank(w,i)

[T,dw]  = size(w); w = w';
j = T - i  + 1;
H = zeros(i*dw,j);
for k = 1:i
  H((k-1)*dw+1:k*dw,:) = w(:,k:k+j-1);
end