keval2.m

vTools is a toolbox for Matlab 5.3 developed within the Department of Electrical Systems and A

if ~isempty(MiW), sMiW=vunpck(norm3(frsp(MiW,wm))); else sMiW=wm'*NaN; end
if ~isempty(ToW), sToW=vunpck(norm3(frsp(ToW,wm))); else sToW=wm'*NaN; end
if ~isempty(TiW), sTiW=vunpck(norm3(frsp(TiW,wm))); else sTiW=wm'*NaN; end

TSo=starp(mmult([eye(no);eye(no)],G),mmult(daug(K,eye(no)),...
          [eye(no);eye(no)],[-eye(no) eye(no) eye(no)]),no,ni);
TSi=starp(mmult([eye(ni);eye(ni)],K),mmult(daug(G,eye(ni)),...
          [eye(ni);eye(ni)],[-eye(ni) eye(ni) eye(ni)]),ni,no);

MSo=starp(G,mmult(daug([eye(ni);eye(ni)],eye(no)),daug(K,eye(no)),...
    [eye(no);eye(no)],[-eye(no) eye(no) eye(no)]),no,ni);
MSi=starp(K,mmult(daug([eye(no);eye(no)],eye(ni)),daug(G,eye(ni)),...
    [eye(ni);eye(ni)],[-eye(ni) eye(ni) eye(ni)]),ni,no);

sTSo=max(vunpck(mu(frsp(TSo,wm),[bTo;no no],'sU'))')';mTSo=max(sTSo);
sTSi=max(vunpck(mu(frsp(TSi,wm),[bTi;ni ni],'sU'))')';mTSi=max(sTSi);

sMSo=max(vunpck(mu(frsp(MSo,wm),[bMo;no no],'sU'))')';mMSo=max(sMSo);
sMSi=max(vunpck(mu(frsp(MSi,wm),[bMi;ni ni],'sU'))')';mMSi=max(sMSi);

sTSoW=max([sToW sSoW]')';sTSiW=max([sTiW sSiW]')';
rWTSo=infnan2x(20*log10(min(sTSoW./sTSo)));
rWTSi=infnan2x(20*log10(min(sTSiW./sTSi)));
   
sMSoW=max([sMoW sSoW]')';sMSiW=max([sMiW sSiW]')';
rWMSo=infnan2x(20*log10(min(sMSoW./sMSo)));
rWMSi=infnan2x(20*log10(min(sMSiW./sMSi)));

if any(str=='d'),
figure;
subplot(2,1,1);
semilogx(wm,20*log10(sTSi),'r',wm,20*log10(sTSo),'b',...
         wm,20*log10(sTSiW),'m',wm,20*log10(sTSoW),'c');
title('max(mu([T T; S S])); blue: output, red: input, (cyan,mag=weight)');
subplot(2,1,2);
semilogx(wm,20*log10(sMSi),'r',wm,20*log10(sMSo),'b',...
         wm,20*log10(sMSiW),'m',wm,20*log10(sMSoW),'c');
title('max(mu([M M; S S])); blue: output, red: input, (cyan,mag=weight)');
end

%------------------------------------------------------------------------%
% All sensitivities: ultimate robust performance condition

% frequency responses

M=starp(mmult(daug(eye(ni),[eye(no);-eye(no)]),daug(eye(ni),G),...
                      [eye(ni);eye(ni)],[eye(ni) eye(ni) eye(ni)]),...
        mmult(daug(eye(no),[eye(ni);+eye(ni)]),daug(eye(no),K),...
                      [eye(no);eye(no)],[eye(no) eye(no) eye(no)]),no,ni);
T=mmult(daug(eye(ni),...
        [zeros(ni,no) eye(ni); eye(no) zeros(no,ni)],eye(no)),M);

sM=max(vunpck(mu(frsp(M,wm),[ni ni;bMi;bMo;no no],'sU'))')';mM=max(sM);
sT=max(vunpck(mu(frsp(T,wm),[ni ni;bTi;bTo;no no],'sU'))')';mT=max(sT);

sMW=max([sMoW sSoW sMiW sSiW]')';
rWM=infnan2x(20*log10(min(sMW./sM)));

sTW=max([sSoW sToW sTiW sSiW]')';
rWT=infnan2x(20*log10(min(sTW./sT)));

if any(str=='d'),
figure;
subplot(2,1,1);
semilogx(wm,20*log10(sM),'y',wm,20*log10(sMW),'w');
title('max(mu(diag(Si Mi Mo So))) (white=weight)');
subplot(2,1,2);
semilogx(wm,20*log10(sT),'g',wm,20*log10(sTW),'w');
title('max(mu(diag(Si Ti To So))) (white=weight)');
end

else mMSo=0;mMSi=0;mTSo=0;mTSi=0;mM=0;mT=0;...
     rWMSo=0;rWMSi=0;rWTSo=0;rWTSi=0;rWM=0;rWT=0;
end

%------------------------------------------------------------------------%
% Maximum control signal: loop input = control input

[AMo,BMo,CMo,DMo]=unpck(Mo);mxuo=zeros(1,min(no,ni));
for n=1:min(no,ni);
[y,xs,t]=step(AMo,BMo(:,n),CMo(n,:),DMo(n,n));mxuo(n)=max(y);
end
mxuo=max(abs(mxuo));

%------------------------------------------------------------------------%
% Maximum control signal: loop input = plant input

[ATi,BTi,CTi,DTi]=unpck(Ti);mxui=zeros(1,ni);
for n=1:ni;
[y,xs,t]=step(ATi,BTi(:,n),CTi(n,:),DTi(n,n));mxui(n)=max(y);
end
mxui=max(abs(mxui));

%------------------------------------------------------------------------%
% Output Closed loop: from control input to plant output

Po=mmult([D C],To,[zeros(size(B)) eye(size(A))]');

%------------------------------------------------------------------------%
% Input Closed loop: from plant input to plant output

Pi=mmult([D C],Mi);

%------------------------------------------------------------------------%
% Displays main results on Po and Pi for w0=1e-2

if any(str=='p'),

w0=1e-2;
for ois=[ 'o' 'i' ],

eval([ 'sys=P' ois ';' ]);
[Ap,Bp,Cp,Dp]=unpck(sys);
[ty,no,ni,ns]=minfo(sys);
mrp=max(real(spoles(sys)));
mfp=max(abs(spoles(sys)));

if any(str=='d'),
  disp(' ');
  minfo(sys)
  disp(' ');
  disp([ 'Rank(ctrb(A,B)) : ' num2str(rank(ctrb(Ap,Bp),eps^2)) ]);
  disp([ 'Rank(obsv(A,C)) : ' num2str(rank(obsv(Ap,Cp),eps^2)) ]);
  
  Gc=gram3(Ap,Bp);
  Go=gram3(Ap',Cp');
  [Uc,Sc,Vc]=svd(Gc);
  [Uo,So,Vo]=svd(Go);
  cp=1./abs(sqrt(diag(pinv(Uc*Sc*Uc'))));
  op=1./abs(sqrt(diag(pinv(Uo*So*Uo'))));

  [ss,su] = sdecomp(sys,-1e-12);
  [tyu,nou,niu,nsu]=minfo(su);
    
  disp(' ');
  disp([ 'max eig : ' num2str(mrp) ]);
  disp([ 'unstable part : ' num2str(nsu) ' states.' ]);

  gsp=zeros(no,ni);
  for j=1:ni,
  for i=1:no,
    [a,b,c,d]=unpck(sel(ss,i,j));
    if size(a,1)==0, Gs=d;
    else Gs=c*inv(sqrt(-1)*w0*eye(size(a))-a)*b+d;
    end
    gsp(i,j)=abs(Gs);
  end
  end

  figure;

  subplot(2,2,1);
  pzmap(Ap,Bp,Cp,Dp);xlabel('');
  title([ 'P' ois ' poles & zeros' ]);

  subplot(2,2,2);
  sigma(Ap,Bp,Cp,Dp);xlabel('');
  title([ 'P' ois ' singular values' ]);

  subplot(2,2,3);
  semilogy(cp,'r');
  hold on
  semilogy(op,'b');
  xlabel('state');
  ylabel('ctrb (red), obsv (blue)');
  title([ 'P' ois ' ctrb & obsv gram. sv' ]);
  hold off

  subplot(2,2,4);
  if min(size(gsp))>1,contour(1:ni,-1:-1:-no,log10(gsp));end
  xlabel('input');ylabel('output');
  title([ 'max(svd(P' ois '(jw,i,j)))' ]);grid;
end

% settling time and overshoot for each channel
gp=Dp-Cp*inv(Ap)*Bp;

ts=zeros(no,ni);os=zeros(no,ni);ct=0;
if any(str=='d') & no<6 & ni<6, figure; end

for n1=1:no
for n2=1:ni

   ct=ct+1;
   [y,xs,t]=step(Ap,Bp(:,n2),Cp(n1,:),Dp(n1,n2));
   g0=gp(n1,n2);if g0<1e-1, g0=1; end

   ts(n1,n2)=max(t'.*(abs(y-g0)>.05*abs(g0)));
   os(n1,n2)=max(y-g0)/g0;
   
   if any(str=='d') & no<6 & ni<6,
     subplot(no,ni,ct);
     plot(t,y);
     if ct<=ni, title([ 'P' ois ' step' ]); end
   end

end
end

eval([ 'if min(size(ts))==1,ts' ois '=ts(1,1);else ts'...
        ois '=diag(ts); end' ]);
eval([ 'if min(size(os))==1,os' ois '=os(1,1);else os'...
        ois '=diag(os); end' ]);
eval([ 'mp' ois '=mrp;' ]);
eval([ 'mf' ois '=mfp;' ]);

end

if any(str=='d'),
disp(' ');
disp([ 'Maximum Po diagonal settling time : ' num2str(max(tso)) ]);
disp([ 'Maximum Po diagonal overshoot     : ' num2str(max(oso)) ]);
disp([ 'Maximum Pi diagonal settling time : ' num2str(max(tsi)) ]);
disp([ 'Maximum Pi diagonal overshoot     : ' num2str(max(osi)) ]);
end

else tso=0;tsi=0;oso=0;osi=0;mpo=0;mpi=0;mfo=0;mfi=0; end

%------------------------------------------------------------------------%
% M=[Si Si Mo Mo; Mi Mi To To; Ti Ti Mo Mo; Mi Mi So So];
% T=[Si Si Mo Mo; Ti Ti Mo Mo; Mi Mi To To; Mi Mi So So];
% results:

mv=[mFo mFi;		% max value of Fo and Fi over w
    mSo mSi;		% max value of So and Si over w
    mMo mMi;		% max value of Mo and Mi over w
    mTo mTi;		% max value of To and Ti over w

    rWFo rWFi;		% min ratio FoW/Fo and FiW/Fi over w
    rWSo rWSi;		% min ratio SoW/So and SiW/Si over w
    rWMo rWMi;		% min ratio MoW/Mo and MiW/Mi over w
    rWTo rWTi;		% min ratio ToW/To and TiW/Ti over w

    mTSo mTSi;		% max mu{diag(D,D)} of [To To;So So] (i) over w
    mMSo mMSi;		% max mu{diag(D,D)} of [Mo Mo;So So] (i) over w
    mM mT;		% max mu{diag(D,D,D,D)} of M and T over w
    rWTSo rWTSi;	% min ratio diag(ToW,SoW)/mu[To To;So So](i) over w
    rWMSo rWMSi;	% min ratio diag(MoW,SoW)/mu[Mo Mo;So So](i) over w
    rWM rWT;		% min ratio diag(SiW,MiW,MoW,SoW)/mu(M) over w
			% and min ratio diag(SiW,TiW,ToW,SoW)/mu(T) over w

    gmho gmhi;		% max output and input gain margin
    gmlo gmli;		% min output and input gain margin
    pho phi;		% output and input phase margin
    mdo mdo;		% min value of det(I+GK)=det(I+KG) over w

    mpo mpi;		% max realpart pole of Po,Pi
    mfo mfi;		% max magnitude pole of Po,Pi
    mxuo mxui;		% max control signal on a step in Po,Pi
    max(tso) max(tsi);	% max settling time on a step in Po,Pi
    max(oso) max(osi)];	% max overshoot on a step in Po,Pi