function [A,DA,B,Q,H] = AugRobustControl(A0,DA0,B0,Q0,H0,delay,delta)
% [A,DA,B,Q,H] = AugRobustControl(A0,DA0,B0,Q0,H0,delay,delta)
%
% Augment the system matrices to take the feedback delay into account.
% [A0, DA0, B0, Q0, H0] are the state space representation matrices
% without delay. The output matrices include the delay.
%
% DELAY: hard time shift in the closed loop system in ms.
% DELTA: discretization step.
%
% Writtent by F. Crevecoeur - Spet. 6, 2019
% Used in: Robust control in human reaching movements: a model free
% strategy to compensate for unpredictable disturbances.
% Crevecoeur F., Scott S. H., Cluff T.
% DOI: https://doi.org/10.1523/JNEUROSCI.0770-19.2019
h = floor(delay/delta); %Feedback delay in number of sample times
n = size(A0,1);
m = size(B0,2);
t = size(Q0,3);
p = size(H0,1);
A = zeros((h+1)*n,(h+1)*n);
DA = zeros((h+1)*n,(h+1)*n);
B = zeros((h+1)*n,m);
Q = zeros((h+1)*n,(h+1)*n,t);
H = zeros(p,(h+1)*n);
A(1:n,1:n) = A0;
A(n+1:end,1:end-n) = eye(h*n);
DA(1:n,1:n) = DA0;
B(1:n,:) = B0;
H(:,end-n+1:end) = H0;
% Adding h times the constraint Q1:
Qaug = zeros(n,n,t+h);
for i = 1:h
Qaug(:,:,i) = Q0(:,:,1);
end
for i = 1:t
Qaug(:,:,i+h) = Q0(:,:,i);
end
%Filling the diagonal Q matrices
for time = 1:t
for ii = 0:h
Q(ii*n+1:(ii+1)*n,ii*n+1:(ii+1)*n,time) = Qaug(:,:,time+h-ii)/(h+1);
end
end