%Example 6.3
%

clear all
close all
format short e

% Define zero system.
RHPZ = 2;
n1   = nd2sys( [1 -RHPZ], [0.1 1]);
n2   = nd2sys( [1  RHPZ], [0.1 1]);
Nz = daug( n1, n2 );

% Define pole system, this system is unstable.
RHPP = 3; 
d1   = nd2sys( [1], [1 -RHPP]);
d2   = nd2sys( [1], [1  RHPP]);
Dp = daug( d1, d2 );

% Setup for H-infinity
Wu = eye( 2 );         % Equal weight on both manipulated inputs.
epsv = 1e-4; wb = 0.5; M = 2;
wp = nd2sys( [1/M wb], [1 epsv]);
Wp = daug( wp, wp ); Wp = sysbal( Wp );
nmeas = 2; nu = 2; gam = 0.5; gmx = 20; tol = 0.001;


%**************  ALL ANGLES ********************
angles = [0 30 60  90];
Yz=[];
Phi=[];
%Cc1=[];
%Cc2=[];
Cc=[];
Snorm=[];
Tnorm=[];
Gamma=[];
   systemnames = 'G Wp Wu';
   inputvar    = '[r(2); u(2)]';
   outputvar   = '[Wp; Wu; r-G]';
   input_to_G  = '[u]';
   input_to_Wp = '[r-G]';
   input_to_Wu = '[u]';
   sysoutname  = 'Psys';
for i=1:4
% angle of rotation matrix
angle = angles(i);

% Rotation matrix
za = angle*pi/180;
Tz = [cos(za) -sin(za)
      sin(za)  cos(za) ];

%overall plant
G = mmult( Dp, Tz, Nz );
[A,B,C,D]=unpck(G);

% zero direction
[U,Sv,V]=svd(C*inv(RHPZ*eye(size(A))-A)*B+D);
yz = U(:,2); 
Yz=[Yz yz];

%pole direction
epsp1 = 0.001;  %should try different values and make sure yp is the same
[U,Sv,V]=svd(C*inv((RHPP+epsp1)*eye(size(A))-A)*B+D);
yp =  U(:,1);

% Compute inner product and angle
inp = yz'*yp;
phi = acos(abs(inp))*180/pi;
Phi = [Phi phi];

% Compute c according to (6.27)
c=sqrt((1-inp^2)+(RHPZ+RHPP)^2/(RHPZ-RHPP)^2*inp^2);
Cc=[Cc c];

% H-infinity controller design
if i==4,
   cleanupsysic= 'yes';
else
   cleanupsysic= 'no';
end
   sysic;
   [K, clp, gamfin] = hinfsyn(Psys, nmeas, nu, gam, gmx, tol );
   Gamma=[Gamma gamfin];
% Perform simulation
L = mmult(G,K); S = minv(madd(eye(2),L)); T = msub(eye(2),S);
y = trsp(T, [1 -1]', 10, 0.01);

ms = sel(hinfnorm(S),':',1);
mt = sel(hinfnorm(T),':',1); 
Snorm=[Snorm ms];
Tnorm=[Tnorm mt];

%Plot response
subplot(2,2,i)
vplot('iv,d',sel(y,1,1),sel(y,2,1),'--',1,'w:',-1,'w:')
axis( [0 5 -2 2] ); 
if i>2
 xlabel('Time');
end

% store data:
%AA=[AA  angle]; YZ=[YZ yz]; %YP=[YP yp]; AZ=[AZ phi]; GM=[GM  gamfin];
%dum1 = [ms; c1]; dum2 = [mt; c2];
%MS = [MS dum1]; MT = [MT dum2];
end

%Table on p.225
disp(sprintf('\nAlpha%6d%6d%6d%6d',angles));
disp(sprintf('  Yz %6d%6.2f%6.2f%6d\n     %6d%6.2f%6.2f%6d',Yz'));
disp(sprintf(' Phi %6d%6.1f%6.1f%6.0f',Phi));
disp(sprintf('   c %6.1f%6.2f%6.2f%6.1f',Cc));
disp(sprintf('  Ms %6.2f%6.2f%6.2f%6.2f',Snorm));
disp(sprintf('  Mt %6.2f%6.2f%6.2f%6.2f',Tnorm));
disp(sprintf('Gamma%6.2f%6.2f%6.2f%6.2f',Gamma));

%Figure 6.1
subplot(2,2,1), title('phi = 0 degrees'), 
subplot(2,2,2), title('phi = 71 degrees'), 
subplot(2,2,3), title('phi = 83 degrees'), 
subplot(2,2,4), title('phi = 90 degrees'), 


