% Esempio di problema di sensitivita' mista H infinito
% Modello linearizzato della dinamica di un aereo

clear all
close all
clc

s=tf([1 0],1);

%%%% Parametri dell'impianto %%%%

% u1: angolo spoiler (decimi di grado)
% u2: accelerazione data dai propulsori (m/s^2)
% u3: angolo elevatore (gradi)
% x1=y1: altitudine rispetto punto di lavoro (m)
% x2=y2: velocita' in avanti (m/s)
% x3=y3: angolo di pitch (gradi)
% x4: velocita' di pitch (gradi/s)
% x5: velocita' verticale (m/s)

A= [0 0 1.1320 0 -1;
    0 -0.0538 -0.1712 0 0.0705
    0 0 0 1 0
    0 0.0485 0 -0.8556 -1.013
    0 -0.2909 0 1.0532 -0.6859];
B=[0 0 0
    -0.120 1 0
    0 0 0
    4.4190 0 -1.665
    1.5750 0 -0.0732];
C=[1 0 0 0 0
    0 1 0 0 0
    0 0 1 0 0];
D=zeros(3,3);

% f.d.t. unitaria
UNO=eye(size(D));

Gs=ss2sym(A,B,C,D) %%% impianto simbolico
G=sym2tf(Gs); % impianto in fdt

% Osservare che definendo l'impianto in tf e poi passando al simbolico
% si ottengono risultati spuri nel calcolo di poli e zeri
% (commentare le due linee precedenti e scommentare le due successive)
%G=tf(ss(A,B,C,D))
%Gs=tf2sym(G)

% Calcolo poli e zeri
[m,p,z]=smform(Gs);
p=double(p)
z=double(z)
pause

W1=(s+6)^2/((s+0.0001)*(s+0.6)); % Su S
W2=[]; % Su R
W3=(s+10)*(s+50)/(500*(1+0.0001*s)^2); % Su T

% Risposta in frequenza dei pesi
figure
sigma(W1,'g',W3,'r')
pause

% Calcolo controllore come soluzione del problema di
% sensitivita' mista H infinito

[K,CL,GAM]=mixsyn(G,W1,W2,W3); % Calcolo controllore
L=G*K; % Guadagno d'anello
S=inv(UNO+L) % Sensitivita'
T=UNO-S % Sensitivita' complementare
R=K*S % Riferimento-comando

% Calcolo simbolico delle funzioni di anello.
% Facile che produca un 'out of memory'
%Ks=tf2sym(K) % Controllore simbolico
%Ss=simple(inv(UNO+Gs*Ks)) % Sensitivita' simb
%Ts=simple(UNO-Ss); % Sensitivita' complementare simb
%S=sym2tf(Ss); % S In tf
%T=sym2tf(Ts); % T in tf

% Grafico S vs. peso GAM/W1
figure
sigma(S,'g',GAM/W1,'g-.')

% Grafico T vs. peso GAM/W3
figure
sigma(T,'g',GAM/W3,'g-.')

% Grafico R vs. peso GAM/W2
figure
%sigma(R,'g',GAM/W2,'g-.')
sigma(R)