%  control_reg.m
%  Kap. 5.4  
%  COMPENSATOR DESIGN
%       * Root Locus design 
%       * Ppole placement 
%       * LQG design

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Setup

clear all
format compact
echo off
pause off

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% switches for seperate preformance
if exist('alle') == 0 , alle = 1 ; end

if exist('rloc')  == 0 , rloc  = 0 + alle ; end
if exist('plac')  == 0 , plac  = 0 + alle ; end
if exist('lqopt') == 0 , lqopt = 0 + alle ; end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  COMPENSATOR DESIGN
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       * Root Locus design 
if rloc == 1 ,
    echo on
    sys = zpk([],[-0.1 -1-i -1+i],1)

    figure
    rlocus (sys)
    [r,k] = rlocus(sys) ;
    
    [ k(1)   , k(max(find(k<inf))) ]
    [ r(:,1) , r(:,max(find(k<inf))) ]

    axis([-1.5 0.5 -2 2]);
    set(gcf,'PaperPosition',[1 1 20 20])
    if exist('druck') == 1 , print('-depsc','bsp_control_reg_rlocus.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_reg_rlocus_sw.eps') , end

    pause    
    [kf,rf] = rlocfind (sys)
    sgrid
    
    axis([-2 1 -2 2]);
    set(gcf,'PaperPosition',[1 1 20 20])
    if exist('druck') == 1 , print('-depsc','bsp_control_reg_rlocfind.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_reg_rlocfind_sw.eps') , end

    pause
    p = [-0.9-0.6i]
    [kff,rff] = rlocfind (sys,p)

    hold on
    plot(p,'ks')
    plot(rff,'kd')

    axis([-2 1 -2 2]);
    set(gcf,'PaperPosition',[1 1 20 20])
    if exist('druck') == 1 , print('-depsc','bsp_control_reg_rlocfindp.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_reg_rlocfindp_sw.eps') , end

    pause
    options.location = 'forward' ; 
    options.sign = -1 ;
    sisotool(zpk([],[-0.1 -1-i -1+i],1),1,options) ;
    
    echo off
end % if rloc == 1 ,


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       * Pole placement 
if plac == 1 ,
    echo on

    sys = ss(tf(100,[1 2 100])) ;
 
    pause
    polo = 3*real(pole(sys)) + imag(pole(sys))/3*i ;
    [ pole(sys) polo ]

    pause
    l = place(sys.a',sys.c',polo).' 
    est = estim(sys,l,1,1) ;
                      
    pause
    t = 0:0.01:4 ;
    [y,t,x] = step(sys,t) ;
    xo0 = [0.5 -0.4] ;
    [yo,to,xo] = lsim(est,[ones(size(t)) y],t,xo0) ;

    pause
    figure
    clf
    subplot(221), zoom on, grid on
        plot(t,x(:,1),'r-',t,xo(:,1),'b--')
        title('Zustand 1')
        legend('x_1','x_{1o}')
    subplot(223), zoom on, grid on
        plot(t,x(:,2),'r-',t,xo(:,2),'b--')
        title('Zustand 2')
        xlabel('Zeit t')
        legend('x_2','x_{2o}')
    subplot(122), zoom on, grid on
        plot(t,y,'r-',t,yo(:,1),'b--')
        title('Ausgang')
        xlabel('Zeit t')
        legend('y','y_o')

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

    
    pause          
    polo = 10*real(pole(sys)) + imag(pole(sys))/10*i ;
    polc =  5*real(pole(sys)) + imag(pole(sys))/ 5*i ;
    [ pole(sys) polo polc ]

    pause
    l = place(sys.a',sys.c',polo).' 
    k = place(sys.a,sys.b,polc)

    pause
    est  = estim(sys,l,1,1) ;
    rsys = reg (sys,k,l) ;
                           
    pause
    t = 0:0.01:4 ;
    [y,t,x] = step(sys,t) ;
    xo0 = [0.5 -0.04] ;
    [yo,to,xo] = lsim(est,[ones(size(t)) y],t,xo0) ;
    xc0 = [0 0] ;
    [yc,tc,xc] = lsim(rsys,ones(size(t)),t,xc0) ;

    pause
    figure
    subplot(221), zoom on, grid on
        plot(t,x(:,1),'r-',t,xo(:,1),'b--',t,xc(:,1),'k-.')
        title('Zustand 1')
        legend('x_1','x_{1o}','x_{1c}')
    subplot(223), zoom on, grid on
        plot(t,x(:,2),'r-',t,xo(:,2),'b--',t,xc(:,2),'k-.')
        title('Zustand 2')
        xlabel('Zeit t')
        legend('x_2','x_{2o}','x_{1c}')
    subplot(122), zoom on, grid on
        plot(t,y,'r-',t,yo(:,1),'b--',t,yc(:,1),'k-.')
        title('Ausgang')
        xlabel('Zeit t')
        legend('y','y_o','y_c')

    set(gcf,'PaperPosition',[1 1 20 9])
    if exist('druck') == 1 , print('-deps' ,'bsp_control_reg_zusreg.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_reg_zusreg_sw.eps') , end
    
    echo off
end % if plac == 1 ,
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       * LQG design
if lqopt == 1 ,
    echo on
    sys = ss (tf(1000,[1 10 1000])) ;
    strecke = ss (sys.a,[sys.b sys.b],sys.c,sys.d) ;
    strecke.inputname  = {'u' 'w'} ;
    strecke.outputname = {'y'} ;

    pause
    % Kalman-Filter 
    Qn = 0.1 ; Rn = 0.2 ;
    [kest,L,P] = kalman (strecke,Qn,Rn) 

    pause
    % Verrauschte Strecke 
    rausch   = ss(sys.a,[sys.b sys.b 0*sys.b],...
                [sys.c ; sys.c ],[0 0 0 ; 0 0 1] );
    rausch.inputname  = {'u' 'w' 'v'} ;
    rausch.outputname = {'y' 'yv'} ;

    pause
    % Zeitsignale 
    [u,tt] = gensig ('square',3,6,0.005) ;
    u = 2 * u ;
    randn('seed',0) ;
    w = sqrt(Qn) * randn(length(tt),1) ;
    v = sqrt(Rn) * randn(length(tt),1) ;

    pause
    [yideal,t]  = lsim (rausch,[u,0*w,0*v],tt) ;
    [yrausch,t] = lsim (rausch,[u,w,v],tt) ;
    [yestim,t]  = lsim (kest,[u,yrausch(:,2)],t) ;

    y  = yrausch(:,1) ;                 % System output
    yv = yrausch(:,2) ;                 % white noise system output  
    ye = yestim(:,1) ;                  % estimated output
    emess = y - yv ;                    % meassurement error   
    ebeob = y - ye ;                    % estiimator error      
                                      
    [ cov(emess) cov(ebeob) ]
                                       
    pause
    figure
    subplot(311)
        plot (t,yideal(:,1))
        title('Ausgangssignal yideal ohne Rauschen')
        a=axis; axis([a(1) a(2) -5 max(yideal(:,1))+1])
        a(3) = floor(min([min(yideal(:,1)) min(yideal(:,1))])) ;
        a(4) = ceil (max([max(yideal(:,1)) max(yideal(:,1))])) ;
        axis([a(1) a(2) a(3) a(4)])
        % set(get(gca,'Title'),'fontsize',14)
        set(gca,'xticklabel','')
    subplot(312)
        plot (t,yv,'k:',t,y,'r-',t,ye,'b--')
        title('Verrauschte Ausgangssignale')
        legend('yv','y','ye',4)
        a=axis; 
        a(3) = floor(min([min(y) min(ye)])) ;
        a(4) = ceil (max([max(y) max(ye)])) ;
        axis([a(1) a(2) a(3) a(4)])
        % set(get(gca,'Title'),'fontsize',14)
        set(gca,'xticklabel','')
    subplot(313)
        plot (t,emess,'k:',t,ebeob,'r-')
        title('Schätz- und Messfehler')
        xlabel('Zeit t')
        legend('emess','ebeob',4)
        a=axis; 
        a(3) = floor(min([min(emess) min(ebeob)])) ;
        a(4) = ceil (max([max(emess) max(ebeob)])) ;
        axis([a(1) a(2) a(3) a(4)])
        set(gca,'xticklabel','')

    set(gcf,'PaperPosition',[1 1 20 12])
    if exist('druck') == 1 , print('-deps' ,'bsp_control_reg_kalman.eps') , end
    if exist('druck') == 1 , print('-deps' ,'bsp_control_reg_kalman_sw.eps') , end

    echo off    
end % if lqopt == 1 ,
  
