% Computing Identification and validation errors

function [ierror verror] = iderror_acc(YUperp,ymeasi,ymeasv,uv,n,r,Nid,Nv,m,p)
%% Estimate observability matrix - Ghat
[U,S,V] = svds(YUperp,n);
Ghat = U*S;

%% Estimate Chat
Chat = Ghat(1:p,:);

%% Least squares to find Ahat
Ghat1 = Ghat((p+1):(r+1)*p,:);
Ghat2 = Ghat(1:r*p,:);
Ahat = Ghat2\Ghat1;
Ahat = stabilize(Ahat);

%% Least squares to find Bhat, Dhat, x0
% Constructing H matrix
u = uv;
R = zeros(p,n + m*n + m*p); % building block of H
R(:,1:n) = Chat;
R(:,n*m+n+1:end) = kron(u(:,1)',eye(p));

H = R;
for i = 2:Nid
  R = [R(:,1:n)*Ahat R(:,n+1:n*m+n)*kron(eye(size(u,1)),Ahat)+kron(u(:,i-1)',Chat)  kron(u(:,i)',eye(p))];
  H = [H;R];
end

H1 = H;
for i = Nid + 1:Nv
  R = [R(:,1:n)*Ahat R(:,n+1:n*m+n)*kron(eye(size(u,1)),Ahat)+kron(u(:,i-1)',Chat)  kron(u(:,i)',eye(p))];
  H1 = [H1;R];
end

N = Nid;
% Least square to solve for X = [x0;vec(Bhat);vec(Dhat)], as in
% Vandenberghe's code. Use svd and truncate small singular values, for
% stable solutions.
[R1,S1,T1] = svd(H,'econ');
for ii = 1:size(S1,1)
  if S1(ii,ii) < 1e-6
    S1(ii,ii) = 0;
  else
    S1(ii,ii) = 1/S1(ii,ii);
  end
end
X = T1*(S1*(R1'*reshape(ymeasi,N*p,1)));

%% identification error
N = Nid;
ymeas = ymeasi;
ybarI = (ymeas*ones(N,1))/N;
ybarI = repmat(ybarI,N,1);
ierror = norm(H*X - reshape(ymeas,N*p,1),2)/norm(reshape(ymeas,N*p,1) - ybarI,2);

%% Verification error
N = Nv;
ymeas = ymeasv;
ybarI = (ymeas*ones(N,1))/N;
ybarI = repmat(ybarI,N,1);
verror = norm(H1*X - reshape(ymeas,N*p,1),2)/norm(reshape(ymeas,N*p,1)- ybarI,2);
