%  control_ana.m
%  Kap. 5.3  
%  MODEL ANALYSIS
%      - GENERAL PROPERTIES 
%      - MODEL DYNAMICS
%      - TIME RESPONSE 
%      - FREQUENCY RESPONSE 
%      - STATE SPACE REALISATION 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% setup

clear all
format compact
echo off
pause off

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Switches for separate performance 
if exist('alle') == 0 , alle = 1 ; end

if exist('allg')    == 0 , allg    = 0 + alle ; end
if exist('dynamik') == 0 , dynamik = 0 + alle ; end
if exist('tantw')   == 0 , tantw   = 0 + alle ; end
if exist('fantw')   == 0 , fantw   = 0 + alle ; end
if exist('ordred')  == 0 , ordred  = 0 + alle ; end
if exist('ssreal')  == 0 , ssreal  = 0 + alle ; end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%      - GENERAL PROPERTIES 
if allg == 1 ,
    echo on
    sys = ss([ 1 2 ; 3 4 ],[ 1 1 ; 0 1 ],[ 3 1 ],0) ; 

    pause    
    class(sys)
    isa(sys,'cell')     

    pause    
    isct(sys)    
    isdt(sys)    
    issiso(sys)  

    sys = tf([1 3],[2 1 5]) 
    pause    
    isempty(sys) 
    hasdelay(sys)
    isproper(sys)

    pause    
    size(sys)    
    d  = size(sys)    
    Ns = size(sys,'order')    

    pause    
    sys = [ tf([1 3],[2 1 5]) , zpk([1 -1],[2 2],2) ] 
    d = size(sys)    
    echo off
end % if allg == 1 ,
             
             
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%      - MODEL DYNAMICS
if dynamik == 1 ,
    echo on
    % dcgain
    sys = [ tf(zpk([-1],[1 0],2)) , tf([2 1 -6],[2 1 1 -4 ]) ]
    dcgain(sys)

    % damp
    sys =  tf(1,[2 -3 1 2])
    damp(sys)
    
    pause
    % bandwidth
    sys = tf([2 1 -6],[2 1 1 -4 ])
    fb = bandwidth(sys)
    dB_dc = 20*log10(dcgain(sys))
    dB_fb = 20*log10(abs(freqresp(sys,fb)))
    dB_dc - 3
    dcgain(sys)
    abs(freqresp(sys,fb))/0.7079
    
    pause
    % zeros and poles, roots
    sys = zpk([-0.2 0.6],[-0.5 -0.7-0.8*i -0.7+0.8*i 0.3],2)
    pole(sys)
    [num,den] = tfdata(tf(sys),'v') ;
    den
    roots(den)
    [z,k] = zero(sys)    
                                               
    pause
    % esort und dsort
    pc = pole(sys);
    [sc,ndxc] = esort(pc);
    [ pc , sc , ndxc ]

    pause
    pd = pole(c2d(sys,1));
    [sd,ndxd] = dsort(pd) ;
    [ pd , sd , ndxd ]

    pause
    % Pole zero map 1
    sysd = c2d(sys,1)
    figure
        subplot(121)
        pzmap(sys)
        sgrid
        axis([-1 1 -1 1 ])
        subplot(122)
        pzmap(sysd)
        zgrid                                                    
                                    
    set(gcf,'PaperPosition',[1 1 24 10])
    if exist('druck') == 1 , print('-depsc','bsp_control_ana_pzmap.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_ana_pzmap_sw.eps') , end

    % Pole zero map 2
    Z = { [] [-6 1 1 ] ; [1 2] -1 };
    P = { [-2 1] [-5 1] ; [-4 -1] [-1 -1+i -1-i] };
    K = [2 3 ; 1 -1];
    
    H = zpk(Z,P,K)
    sys = H(2,2);

    figure
        iopzmap(sys,H)
                                    
    set(gcf,'PaperPosition',[1 1 20 15])
    if exist('druck') == 1 , print('-depsc','bsp_control_ana_iopzmap.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_ana_iopzmap_sw.eps') , end
                                                
    pause
    % Normen
    sys = tf(200,[1 2 100])
    [ninf,fpeak] = norm(sys,inf)
    figure
    bode(sys)
    % set(gcf,'PaperPosition',[0.05 2.5 9.0 4.0])
    if exist('druck') == 1 , print('-depsc','bsp_control_ana_hinf.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_ana_hinf_sw.eps') , end
    
    pause

    echo off
end % if dynamik == 1 ,

  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%      - TIME RESPONSE 
if tantw == 1 ,
    echo on        
    systf = tf(5,[1 1 10])    
    sys   = ss(systf) ;

    [y,t,x] = initial (sys,'r-',[4 -1],12) ;    % unforced response 
                                   
    pause
    figure
    subplot(221)  
        plot(t,y)
        grid on;               
        a = axis ;
        axis([a(1) max(t) a(3) a(4)]) 
        title('Unforced response')
        xlabel('Zeit t')
        ylabel('y (t)')
    subplot(223)  
        plot(t,x(:,1),'r-',t,x(:,2),'g--')
        grid on;               
        a = axis ;
        axis([a(1) max(t) a(3) a(4)]) 
        title('')
        xlabel('Zeit t')
        ylabel('x1, x2')
        legend('x1','x2')
    subplot(122)  
        plot(x(:,1),x(:,2))
        grid on;
        title('State trajectories')
        xlabel('x1')
        ylabel('x2')

    set(gcf,'PaperPosition',[1 1 20 10])
    if exist('druck') == 1 , print('-depsc','bsp_control_ana_initial.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_ana_initial_sw.eps') , end
                                     
    pause
    figure
    sys2in = [ tf(5,[1 1 10]) , tf(1,[1 1]) ] ;
    subplot(211)  
        impulse(sys2in);             % Impulse response
    subplot(212)  
        step (sys2in);               % Step response

    set(gcf,'PaperPosition',[1 1 21 12])
    if exist('druck') == 1 , print('-depsc','bsp_control_ana_impstep.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_ana_impstep_sw.eps') , end

    pause
    figure
    sys2in = [ tf(1,[1 1]) ; tf(1000,[1 5 1000]) ] ;
    [u,t] = gensig('square',3,10,0.01) ;   % beliebiges Eingangssignal
    [y,t] = lsim(sys2in,u,t) ;              % Antwort auf beliebiges Eingangssignal
    plot(t,u,'r--',t,y(:,1),'b-.',t,y(:,2),'g-')
    title('Antwort auf beliebiges Eingangssignal')
    legend('u(t)','PT_1','PT_2',1)
    xlabel('Zeit t')

    set(gcf,'PaperPosition',[1 1 20 8])
    if exist('druck') == 1 , print('-depsc','bsp_control_ana_lsim.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_ana_lsim_sw.eps') , end

    echo off
end % if tantw == 1 ,


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%      - FREQUENCY RESPONSE 
if fantw == 1 ,
    echo on

    % Frequency response
    sys = tf([3 -1],[-2 1 1])
    w = [-j , -2-j , -1+j]
    [ evalfr(sys,w(1)) ; evalfr(sys,w(2)) ; evalfr(sys,w(3))]
    H = freqresp(sys,w)

    pause
    % Bode plot
    sysPT1  = tf (1,[0.05 1]) 
    sysPT1d = c2d(sysPT1,0.05)  

    pause
    sysPT2 = tf (10,[1 0.05 10]) 
    d = [sqrt(2)/2 1.6 3] ;
    for n = 1:1:length(d) ,
        sysPT2 = [ sysPT2 ; tf(10,[1 d(n) 10]) ] ;
    end;
                        
    figure 
    subplot(121) , 
        bode(sysPT1,'r-',sysPT1d,'b--')
    subplot(122) , 
        hold on;               
        stil = {'r-' 'b-.' 'k-' 'g--'}
        for n = 1:1:size(sysPT2,1) ,
%            bode(sysPT2(n),stil{n});
            bode(sysPT2(n),stil{1+mod(n-1,size(sysPT2,1))}); 
        end;

    pause
    set(gcf,'PaperPosition',[1 1 20 9.5])
    if exist('druck') == 1 , print('-depsc','bsp_control_ana_bode.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_ana_bode_sw.eps') , end
                                                      
    % Resampling of sysPT1
    figure
    sysPT1dneu = d2d(sysPT1d,0.01)
    step(sysPT1,'r-',sysPT1d,'b-.',sysPT1dneu,'g--')
    legend('sysPT1','sysPT1d','sysPT1dneu',4)
    set(findobj('Tag','LTIdisplayAxes'),'XLim',[0,0.3])

    set(gcf,'PaperPosition',[1 1 20 9])
    if exist('druck') == 1 , print('-depsc','bsp_control_ana_stepPT1bode.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_ana_stepPT1bode_sw.eps') , end

    pause
    % Gain an d phase margins
    sys  = zpk([],[-1 -1 -1],4)

    figure 
    margin (sys)
    [Gm,Pm,Wcg,Wcp] = margin(sys)
    stabil = allmargin(sys)
    sys.ioDelay = stabil.DelayMargin + 0.01
    allmargin(sys)

    set(gcf,'PaperPosition',[1 1 20 11])
    if exist('druck') == 1 , print('-depsc','bsp_control_ana_margin.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_ana_margin_sw.eps') , end

    pause
    % Nyquist-Plot
    sys  = zpk([],[-1 -1 -1],4)
    sysd1 = c2d(sys,0.3) ;
    sysd2 = c2d(sys,1.5) ;
    sysd3 = c2d(sys,4.0) ;

    figure
    nyquist(sys,'r-',sysd1,'b-.',sysd2,'k-' ,sysd3,'g--')

    set(gcf,'PaperPosition',[1 1 15 9])
    if exist('druck') == 1 , print('-depsc','bsp_control_ana_nyquist.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_ana_nyquist_sw.eps') , end

    figure 
    subplot(121) , 
        nyquist(sysPT1,'r-',sysPT1d,'b--')
    subplot(122) , 
        hold on;               
        stil = {'r-' 'b-.' 'k-' 'g--'} ;
        for n = 2:1:size(sysPT2,1) ,
            nyquist(sysPT2(n),stil{1+mod(n-1,size(sysPT2,1))});  % ganz korrekt 
        end;

%    set(gcf,'PaperPosition',[0.25 2.5 8 3.2])
    if exist('druck') == 1 , print('-depsc','bsp_control_ana_nyquist2.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_ana_nyquist2_sw.eps') , end

    pause
    % Analyse mit dem LTI-Viewer
    [num,den] = ord2(10,0.2);
    sys = tf(num,den);
    h = ltiview({'step';'pzmap'},sys);
    ltiview('current',tf(dcgain(sys),[0.3 1]),h)

    echo off
end % if fantw == 1 ,
                                    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%      - STATE SPACE REALISATION 
if ssreal == 1 ,
    echo on
    
    A = [  6 -1 ;  1 4 ] 
    B = [ -2  2 ; -2 2 ] 
    C = [  1  0 ;  0 1 ] 
    
    pause            
    % Observability matrix
    Ob = obsv(A,C)
    rank(Ob)
    
    % Observability staircase form
    [Abar,Bbar,Cbar,T,k] = obsvf(A,B,C)

    pause            
    % Controllability matrix
    Co = ctrb(A,B)
    rank(Co)
    
    % Controllability staircase form
    [Abar,Bbar,Cbar,T,k] = ctrbf(A,B,C)
    echo off
end % if ssreal == 1 ,
