% Usage [Laverage,Lworst,Haverage,Hworst] = optimalcomb_Average(Gy,Gdy,Wn,Wd,Juu,Jud)
%
% This function finds the optimal combination of measurements for locally
% optimal self-optimizing control minimizing average loss. If
% there are nu degrees of freedoms available (dimension of input), the
% method find an nu*ny dimensional matrix, H, such that the controlled
% variables are c = H*y.
%
% Basis - The average loss is 6
%                               Laverage = 0.5*trace(X)                    (1) 
%     where X satisfies
%                lambda_{n_u} (Gy*Juu^{-0.5}*X*Juu^{-0.5}*Gy' - Y*Y') = 0  (2)
%     
%     lambda_{n_u} is the n_uth largest eigenvalue and 
%     Y = [Gy*inv(Juu)*Jud-Gdy)*Wd Wn] (see below for explanation of individual terms)
%     H is given as the transpose of the first n_u eigenvectors of
%     Gy*Juu^{-0.5}*X*Juu^{-0.5}*Gy' - Y*Y'
%
%     This combination matrix H is superoptimal in the sense that it
%     also minimizes the worst-case loss simultaenously, which is 
%                               Lworst =  0.5/msv(X)^2                     (3)                                    
%
% Inputs - 
%      Gy - Gain matrix from u to y (full column rank)
%      Gdy - Gain matrix from d to y    
%      Juu - d^2 J/du^2 , where J is the objective function
%      Jud - d^2 J/du/dd
%      Wd  - Matrix containting expected magnitudes of disturbances (usually diagonal), Default = I
%      Wn  - Diagonal matrix containing  expected magnitudes of implementation erros (usually diagonal), Default = I

% Outputs - 
%      Laverage - Loss as given by (1)
%      Lworst   - Wosrt case loss  given by (3)
%      Haverage - combination matrix designed based on average loss minimization
%      Hwost    - combination matrix designed based on worst case loss minimization
%
% Written by Vinay Kariwala
%      Nanyang Technological University, Singapore
%      June 21, 2006
% Modified by Yi Cao
%      Cranfield University, UK
%      June 21, 2006
%


% The following line was originally in the code, replaced with F for better
% and more reliable results
%function [Laverage,Lworst,Haverage,Hworst] = optimalcomb_Average(Gy,Gdy,Wn,Wd,Juu,Jud)
function [Laverage,Lworst,Haverage,Hworst] = optimalcomb_Average(Gy,Gdy,Wn,Wd,Juu,F)

% Assign defaults, if not supplied by user

[ny,nd] = size(Gdy);
nu = size(Gy,2);
nH = rank(Gy);             % usually nu = nH

if nargin < 5
    Wd = eye(nd);
end

if nargin < 6
    Wn = eye(ny);
end

% Check input

if size(Gy,1) ~= ny
    error('Dimensions of Gy and Gdy do not match')
end

if size(Juu,1) ~= nu || size(Juu,2) ~= nu
    error('Juu must be square a square matrix having dimension nu x nu')
end

% if size(Jud,1) ~= nu || size(Jud,2) ~= nd
%     error('Jud must have dimension nu x nd')
% end

if size(Wd,1) ~= nd || size(Wd,2) ~= nd
    error('Wd must be square a square matrix having dimension nd x nd')
end

if size(Wn,1) ~= ny || size(Wn,2) ~= ny
    error('Wn must be square a square matrix having dimension ny x ny')
end
% The next line was originally in the code, replaced by F equivalent which
% is more reliable
%Y = [(Gy*inv(Juu)*Jud-Gdy)*Wd Wn];
Y = [F*Wd Wn];
Z = Gy*sqrtm(inv(Juu));

% Cholesky Factorization

Y2=Y*Y';
Z0=chol(Y2)'\Z;
Z1=Z0'*Z0;
sigma=1./eig(Z1);
Laverage=0.5*sum(sigma)/nu;
Lworst=0.5*max(sigma);
% [U,S,V] = svd(chol(Y*Y')'\Z);
% X = V*inv(S(1:nu,:).^2)*V';
% Laverage = 0.5*trace(X);
% Lworst = 1/min(diag(S(1:nu,:)));

% Find H
Z3=chol(Z1)'\Z';
Z2=Z3'*Z3;
[E,D]=eig(Z2-Y2);
[dum,idx]=sort(diag(D),'descend');
Haverage=E(:,idx(1:nH))';
% [E,D] = eig(Z*X*Z' - Y*Y');
% 
% % Sort the eigenvalues and eigenvectors
% % This is necessary as Matlab arranges eigenvalues according to absolute magnitude
% 
% [temp,index] = sort(diag(D));
% 
% V1 = [];
% for i = 1:size(E,1)
%     V1 = [V1 E(:,index(i))];
% end
% 
% H = V1(:,end-nH+1:end)';
%    
[E,D]=eig(2*Lworst*Z*Z'-Y2);
[dum,idx]=sort(diag(D),'descend');
Hworst=E(:,idx(1:nH))';

