% 
% Cálculo das incertezas - Veja arquivo Aula7_Incerteza.doc
%
% SEM 5928 - Sistemas de Controle 
% Escola de Engenharia de São Carlos - USP
%
% Adriano Siqueira - 2016
%
 
clear;
close all
A=[ 0 0 0 0 0 0;
    0 0 0 0 0 0;
    0.14 -0.12 -0.02 0.005 2.4 -32;
    0.36 -8.6 -0.014 0.44 -1.3 -30;
    0.35 0.009 0 0.018 -6 1.2;
    0 0 0 0 1 0];
B=[1 0;
    0 1;
    0 0;
    0 0;
    0 0;
    0 0];
C=[0 0 0 1 0 0;
    0 0 0 0 0 57.3];
D=[0 0; 0 0];

sysG=ss(A,B,C,D);
w=logspace(-2,3,100);

aux=[0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0];
b=0;
w_=i*w;
for j=1:100,
    inverro(j)= 100000;
    for i=1:10
        gr=tf(625, [1 50*aux(i) 625]);
        sysGr=series(gr,eye(2));
        sysGR=series(sysG,sysGr);
        [AR,BR,CR,DR]=ssdata(sysGR);

        G = C*inv(w_(j)*eye(size(A))-A)*B-D;
        GR = CR*inv(w_(j)*eye(size(AR))-AR)*BR-DR;
        E = (GR - G)*inv(G);
        S = svd(E);
        aux3= 1/(S(1));
        if aux3 <= inverro(j),
            inverro(j) = aux3;
        end;

    end;

end;



figure(1)
sigma(sysG,'b')
grid
hold on

figure(2)
grid
hold on

figure(3)
grid
hold on

aux=[0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0];
  for i=1:10
        
      
      gr=tf(625, [1 50*aux(i) 625]);
        sysGr=series(gr,eye(2));
        
        
        sysGR=series(sysG,sysGr);
        
        figure(1)
        sigma(sysGR,'r')
       legend('G(s)','GR(s)')
        sysE = (sysGR - sysG)*inv(sysG);
        
        figure(2)
        sigma(sysE,'c')
         legend('E(s)')
        
        figure(3)
        sigma(inv(sysE),'g')
         legend('E^{-1}(s)')

pause
    end;
    
     figure(3)
    w=logspace(-2,3,100);
    semilogx(w,20*log10(inverro),'m');