keval3.m

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

function [Po,Pi,mv] = ...
  keval3(G3,K3,str,w,FoW,FiW,SoW,SiW,MoW,MiW,ToW,TiW,bMo,bMi,bTo,bTi)

% [Po,Pi,mv]=keval3(G,K,str,w); linear control evaluation.
% Performs analysis on the closed loop obtained by connecting the
% controller K as a negative output to state feedback on G (xd=Ax+Bu-Ky).
% Returns also :
% Po: Closed loop system from control input to plant output.
% Pi: Closed loop system from plant input to plant output.
% If str contains 'd' data and plots will be shown.
% If str contains 'p' Po and Pi will be analysed.
% If str contains 'm' performance robustness will be analysed by mean
% of mu{blk} values of several sensitivity matrices (it may be long).
% Default blk is full size.
% Default w is a 120 log spaced points vector from 1e-6 to 1e6 rad/sec.
% Default str is 'dp', type keval3 for more info.
% See also keval and keval2.

% 
% G.Campa 20/05/98

if nargin<16,bTi=[];end
if nargin<15,bTo=[];end
if nargin<14,bMi=[];end
if nargin<13,bMo=[];end

if nargin<12,TiW=[];end
if nargin<11,ToW=[];end
if nargin<10,MiW=[];end
if nargin<9,MoW=[];end
if nargin<8,SiW=[];end
if nargin<7,SoW=[];end
if nargin<6,FiW=[];end
if nargin<5,FoW=[];end
if nargin<4,w=[];end

if nargin<3,str='dp';end
if nargin<2,error('Please read keval3 help');end

[ty3,no3,ni3,ns3]=minfo(G3);
[tyk,nok,nik,nsk]=minfo(K3);

if no3~=nik | ns3~=nok,
error('G and K have incompatible sizes')
end

[A,B,C,D]=unpck(G3);
no=size(C,1);ni=size(C,1)+size(C,2);
G=sbs(eye(no),pck(A,eye(size(A)),C,zeros(size(C))));
K=abv(zeros(no),K3);

if isempty(bTi),bTi=[ni ni];end
if isempty(bTo),bTo=[no no];end
if isempty(bMi),bMi=[ni no];end
if isempty(bMo),bMo=[no ni];end

if isempty(w),w=logspace(-6,6,120);end

if ~isempty(FoW), sFoW=vunpck(norm3(frsp(FoW,w))); else sFoW=w'*NaN; end
if ~isempty(FiW), sFiW=vunpck(norm3(frsp(FiW,w))); else sFiW=w'*NaN; end
if ~isempty(SoW), sSoW=vunpck(norm3(frsp(SoW,w))); else sSoW=w'*NaN; end
if ~isempty(SiW), sSiW=vunpck(norm3(frsp(SiW,w))); else sSiW=w'*NaN; end
if ~isempty(MoW), sMoW=vunpck(norm3(frsp(MoW,w))); else sMoW=w'*NaN; end
if ~isempty(MiW), sMiW=vunpck(norm3(frsp(MiW,w))); else sMiW=w'*NaN; end
if ~isempty(ToW), sToW=vunpck(norm3(frsp(ToW,w))); else sToW=w'*NaN; end
if ~isempty(TiW), sTiW=vunpck(norm3(frsp(TiW,w))); else sTiW=w'*NaN; end

%------------------------------------------------------------------------%
% Input ant output open loop responses

Fo=mmult(G,K);Fi=mmult(K,G);

% frequency responses
sFo=vunpck(norm3(frsp(Fo,w)));mFo=max(sFo);
sFi=vunpck(norm3(frsp(Fi,w)));mFi=max(sFi);

ww=sort([w 1e-2:.01:10]);
dIFo=vunpck(vdet(frsp(madd(eye(no),Fo),ww)));mdo=min(abs(dIFo));

rWFo=infnan2x(20*log10(min(sFoW./sFo)));
rWFi=infnan2x(20*log10(min(sFiW./sFi)));

if any(str=='d'),
figure;
plot(real(dIFo),imag(dIFo),'r',0,0,'w');add_disk;
axis([-3 3 -3 3]);title('det(I+GK)=det(I+KG)');
figure;
subplot(2,1,1);
semilogx(w,20*log10(sFi),'r',w,20*log10(sFo),'b',...
         w,20*log10(sFiW),'m',w,20*log10(sFoW),'c');
title('blue: F=GK, red: F=KG, (cyan,mag=weight)');
end

%------------------------------------------------------------------------%
% Input ant output sensitivity.

So=starp(mmult(G,K),mmult([eye(no);eye(no)],[-eye(no),eye(no)]),no,no);
Si=starp(mmult(K,G),mmult([eye(ni);eye(ni)],[-eye(ni),eye(ni)]),ni,ni);

% frequency responses
sSo=vunpck(norm3(frsp(So,w)));mSo=max(sSo);
sSi=vunpck(norm3(frsp(Si,w)));mSi=max(sSi);

rWSo=infnan2x(20*log10(min(sSoW./sSo)));
rWSi=infnan2x(20*log10(min(sSiW./sSi)));

if any(str=='d'),
subplot(2,1,2);
semilogx(w,20*log10(sSi),'r',w,20*log10(sSo),'b',...
         w,20*log10(sSiW),'m',w,20*log10(sSoW),'c');
title('blue: S=(I+GK)^-1, red: S=(I+KG)^-1, (cyan,mag=weight)');
end

%------------------------------------------------------------------------%
% Input and output control sensitivity.

% step responses calculation of hloop2.
Mo=starp(G,mmult([eye(ni);eye(ni)],K,[-eye(no) eye(no)]),no,ni);
Mi=starp(K,mmult([eye(no);eye(no)],G,[-eye(ni) eye(ni)]),ni,no);

% frequency responses
sMo=vunpck(norm3(frsp(Mo,w)));mMo=max(sMo);
sMi=vunpck(norm3(frsp(Mi,w)));mMi=max(sMi);

rWMo=infnan2x(20*log10(min(sMoW./sMo)));
rWMi=infnan2x(20*log10(min(sMiW./sMi)));

if any(str=='d'),
figure;
subplot(2,1,1);
semilogx(w,20*log10(sMi),'r',w,20*log10(sMo),'b',...
         w,20*log10(sMiW),'m',w,20*log10(sMoW),'c');
title('blue: M=K(I+GK)^-1, red: M=G(I+KG)^-1, (cyan,mag=weight)');
end

%------------------------------------------------------------------------%
% Input and output complementary sensitivity 

To=starp(mmult([eye(no);eye(no)],G,K),[-eye(no) eye(no)],no,no);
Ti=starp(mmult([eye(ni);eye(ni)],K,G),[-eye(ni) eye(ni)],ni,ni);

% frequency responses
sTo=vunpck(norm3(frsp(To,w)));mTo=max(sTo);
sTi=vunpck(norm3(frsp(Ti,w)));mTi=max(sTi);

rWTo=infnan2x(20*log10(min(sToW./sTo)));
rWTi=infnan2x(20*log10(min(sTiW./sTi)));

if any(str=='d'),
subplot(2,1,2);
semilogx(w,20*log10(sTi),'r',w,20*log10(sTo),'b',...
         w,20*log10(sTiW),'m',w,20*log10(sToW),'c');
title('blue: T=GK(I+GK)^-1, red: T=KG(I+KG)^-1, (cyan,mag=weight)');
end

%------------------------------------------------------------------------%
% gain and phase margins:

gmho=max(1+1/mTo,mSo/(mSo-1));
gmlo=min(1-1/mTo,mSo/(mSo+1));
pho=180/pi*2*max(asin(1/2/mTo),asin(1/2/mSo));

gmhi=max(1+1/mTi,mSi/(mSi-1));
gmli=min(1-1/mTi,mSo/(mSi+1));
phi=180/pi*2*max(asin(1/2/mTi),asin(1/2/mSi));

if any(str=='d'),
disp(' ');
disp([ 'min(abs(det(I+GK)))    :  ' num2str(mdo) ]);
disp([ 'Output phase margin    :  ' num2str(pho) ]);
disp([ 'Output max gain margin :  ' num2str(gmho) ]);
disp([ 'Output min gain margin :  ' num2str(gmlo) ]);
disp([ 'Input phase margin     :  ' num2str(phi) ]);
disp([ 'Input max gain margin  :  ' num2str(gmhi) ]);
disp([ 'Input min gain margin  :  ' num2str(gmli) ]);
end

%------------------------------------------------------------------------%
% PZmaps

if any(str=='d'),

[AFo,BFo,CFo,DFo]=unpck(Fo);
[ASo,BSo,CSo,DSo]=unpck(So);
[AMo,BMo,CMo,DMo]=unpck(Mo);
[ATo,BTo,CTo,DTo]=unpck(To);

[AFi,BFi,CFi,DFi]=unpck(Fi);
[ASi,BSi,CSi,DSi]=unpck(Si);
[AMi,BMi,CMi,DMi]=unpck(Mi);
[ATi,BTi,CTi,DTi]=unpck(Ti);

figure;
subplot(2,2,1);pzmap(AFo,BFo,CFo,DFo);
title('GK');xlabel('');
subplot(2,2,2);pzmap(ASo,BSo,CSo,DSo);
title('(I+GK)^-1');ylabel('');xlabel('');
subplot(2,2,3);pzmap(AMo,BMo,CMo,DMo);
title('K(I+GK)^-1');
subplot(2,2,4);pzmap(ATo,BTo,CTo,DTo);
title('GK(I+GK)^-1');ylabel('');

figure;
subplot(2,2,1);pzmap(AFi,BFi,CFi,DFi);
title('KG');xlabel('');
subplot(2,2,2);pzmap(ASi,BSi,CSi,DSi);
title('(I+KG)^-1');xlabel('');ylabel('');
subplot(2,2,3);pzmap(AMi,BMi,CMi,DMi);
title('G(I+KG)^-1');
subplot(2,2,4);pzmap(ATi,BTi,CTi,DTi);
title('KG(I+KG)^-1');ylabel('');

end

%------------------------------------------------------------------------%
% poles drifting:

if any(str=='d'),

% open loop system
[Aol,Bol,Col,Dol]=unpck(starp(mmult([eye(no);eye(no)],G,K),...
                  [-0*eye(no) eye(no)],no,no));
figure;
pzmap(Aol,Bol,Col,Dol);hold on
title('closed loop poles drifting : from yellow to blue')

avm=[];
for q=0:.02:1,
    [Aq,Bq,Cq,Dq]=unpck(starp(mmult([eye(no);eye(no)],G,K),...
                  [-q*eye(no) eye(no)],no,no));
    avm=[avm eig(Aq)];	
end
l=size(avm,2);

for j=1:size(avm,1),
    plot(real(avm(j,1:l-1)),imag(avm(j,1:l-1)),'g.')
    plot(real(avm(j,l)),imag(avm(j,l)),'bx')
end

end

%------------------------------------------------------------------------%
% Both sensitivities: robust performance condition

if any(str=='m'),

% frequency responses
wm=logspace(-6,6,length(w)/2);

if ~isempty(SoW), sSoW=vunpck(norm3(frsp(SoW,wm))); else sSoW=wm'*NaN; end
if ~isempty(SiW), sSiW=vunpck(norm3(frsp(SiW,wm))); else sSiW=wm'*NaN; end
if ~isempty(MoW), sMoW=vunpck(norm3(frsp(MoW,wm))); else sMoW=wm'*NaN; end