%%% Tutorial su funzioni matlab utili per controlli automatici e teoria
%%% sistemi

clear all;
close all;

%%%% rappresentazione di polinomi tramite vettori
%% ex H1(s) = 4s^4+3s^3+s. H2(s) = s^2-2s+1 Elemento a sinistra corrisponde a s^0 

H1 = [4 3 0 1 0]
H2 = [1 -2 1]

%% moltiplicazione tra polinomi
%% ex H(s)=H1(s)*H2(s) = 4s^6-5s^5-2s^4+4s^3-2s^2+s

H = conv(H1,H2)

%%% radici di un polinomio, cieo' H2(s) = s^2-2s+1 = (s-1)(s-1)

r = roots(H)

%%% trovare un polinomio date le radici l = [ -2 3] H3=(s+2)(s-3)=s^2-s-6
%% poly e' l'inversa di roots

l1 = [-2 3]
H3 = poly(l1)

l2 = [-2+4i -2-4i]  % radici complesse coniugate
H4 = poly(l2)

%%%% trovare i residui di un funzione razionale
%%%% T(s)=num(s)/den(s)=r1/(s-p_1)+r2/(s-p2) ... fare help residue per
%%%% maggiori informazioni in particlare se poli con multiplicita' >1

num = [ 1 3 1];
den = [ 1 4 4 0];

[R,P,K] = residue(num,den)     % R vettore residui, P vettore polim, K residuo

%%%% funzioni di trasferimento definite come rapporto di polimoni descritti
%%%% in termini dei coefficenti dei vettori P=(s^2+3s+1)/(s^3+4s^2+4s)

num = [ 1 3 1];
den = [ 1 4 4 0];

P = tf(num,den)   %% P oggetto funzione trasferimento

%%% poli e zeri di una funzione di trasferimento

z = zero(P)
p = pole(P)

%%%%% funzione di trasferimento descritta mediante poli/zeri 
%%%%% P2 =K*(s-z1)(s-z2)/((s-p1)(s-p2)(s-p3)(s-p4))

z1 = [ 1 -2];
p1 = [ -1 -2 -1+2i -1-2i];
K = 3;

P2 = zpk(z1,p1,K)

%% e' possibile passare da una rappresentazione ad un altra automaticamente

P3 = tf(P2)   %% rappresentazione in polinomi
P4 = zpk(P)   %% rappresentazione in poli zeri

%%% per estrarre il numeratore o denominatore da una funzione di
%%% trasferimento, oppure gli zero, poli e guadagno si puo' operare come segue

[num2 den2] = tfdata(P4,'v')  %estrae la rappresentazione in num(s)/den(s)
[z,p,k] = zpkdata(P4,'v')    %estrae la rappresentazione in zpk da una finzione di trasferimento


%%% operazioni con funzioni di trasferimento

P5 = P2+P3
P6 = P2*P3
P7 = feedback(P2,P3)   %P7 = P2/(1+P2*P3)

%%%% realizzazione minima di una funzione di tf, cieo' cancella poli/zeri
%%%% comuni. Da utilizzare solo se poli/zeri sono stabili

P8 = minreal(P7)


%%%%%% rappresentazione in variabili di stato di una funzione di
%%%%%% trasferimento

sys1 = ss(P7)  %utilizza la forma canonica di controllo. sys1 e' un oggetto sys in matlab
                %per estrarre la forma amtriciale si suo ricorrere ai
                %seguenti comandi
A = sys1.a
B = sys1.b
C = sys1.c
D = sys1.d

%%%% oppure
[num den] = tfdata(P7,'v')
[A1 B1 C1 D1] = tf2ss(num,den)

%% oppure
[A2 B2 C2 D2] = ssdata(P7) 
%% oppure
[A2 B2 C2 D2] = ssdata(sys1)


%%%%% e possibile ottenere la funzione di trasferimento da spazio di stato a tf o zpk
%%%%% semplicemnte scrivendo

G = tf(sys1)
G1 = zpk(sys1)



%%%%%%%%% discretizzazione da tempo continuo a tempo discreto

%% funzione di trasferimento in z H_d(z) ottenuta da discretizzazione di
%% funzione di trasferimento continua in s H_s(s) con tempo di campionamento
%% Tc utilizzando il comando c2d . 

H_s = P
Tc = 0.1
H_d1 = c2d(H_s,Tc,'zho')  % metodo zero-holder
H_d1 = c2d(H_s,Tc,'tustin')  % metodo Tustin o trapezioidale o bilineare


% stesso comando anche per creare un sistema discreto da uno continuo in
% rappresentazione di variabili di stato , dx=A*x+B*u y=C*x  ---->
% x_(k+1)=A_d*x_k+Bd*u_k , y_k=Cd*x_k

sys_d = c2d(sys1,Tc);


% funzioni pole, zero funzianano anche su sistemi ss

pole(sys_d)
zero(sys_d)

%%%%%% calcolo di parametri utili da funzioni di trasferimento

P = zpk([ -3],[ -1 -2+3i -2-3i],3)
k = dcgain(P)       % guadagno a regime
b = bandwidth(P)    % banda
[Gm,Pm,Wcg,Wcp] = margin(P) % Gm margine di guadagno, Pm margine di pase, Wcg frequenza corrispondente al 
                            % margine di guadagno, Wcp frequenza di taglio
                            % o frequenza corrispondente al margine di fase


%% diagrammi utili
figure(1)
bode(P)  % grafico dei diagrammi di bode
figure(2)
nyquist(P)  % grafico di nyquist
figure(3)
rlocus(P)   % luogo delle radici con feedback proporzionale

k = [ 1 3 5 10]
r = rlocus(P,k)  % calcola i poli in catena chiusa per i corrispondenti valori retroazione con guadagno k ; 
                 % r e' una matrice NpxNk dove Np e' il numero di poli e Nk e' la lunghezza di k 


%% calcolo della funzione di trasferimento ad una particolare frequenza

omega = 10;
Pomega=evalfr(P,j*omega) %numero complesso in Pomega. Notare che si puo' valutare P per un numero complesso qualsiasi 
modulo = abs(Pomega)
fase = angle(Pomega)  %fase in radianti
fase = 180/pi*fase  %fase in gradi

%%%% risposta a gradino ed an impulso unitario di una funzione di
%%%% trasferiemnto

figure(4)
step(P)   %grafico
figure(5)
impulse(P)  %grafico

Tfin = 10 %tempo finale di simulazione
figure(6)
[y,t] = step(P,Tfin)   % no grafico, ma risultato in y, t
plot(t,y)  


%%%%%% funzioni per progettazione in variabili di stato

A = -[ 0 1 0; 0 1 1; 1 0 3];
B = [ 1 2 1]';
C = [ 1 0 1];


Cont = ctrb(A,B)   %matrice di controllabilita' [ B AB A^2B ..]
n=size(A,1)-rank(Cont)  % se n=0 allora (A,B) e' controllabile

Obs = obsv(A,C)  %matrice di controllabilita' [ C ; CA ; CA^2 ...]
n=size(A,1)-rank (Obs)  % se n=0 allora (A,B) e' controllabile

%%%% se (A,B) controllabile esiste u=-Kx tale che i poli di (A-BK) siano in
%%%% posizione desiderata

p = [-1 -2+i -2-i]; %posizione poli desiderata
K1 = acker(A,B,p)    %non molto robusta numericamente
K2 = place(A,B,p)    % fa esattamente la stessa cosa, ma non funziona con poli di motiplicita'>1. E' piu' robusta numericamente

%%%% se si richiede il guadagno L per uno stimatore tale che i poli di (A-L*C) siano in p basta utilizzare il
%%%% duale tramite le trasposte

L = place(A',C',p)'  

%%% se si vuole cambiare rappresentazione in variabile di stato rispetto ad
%%% una cambio di coordinate z=Tx dove T e' quadrata e invertibile

sys1 = ss(A,B,C,0)
T = [ 0 0 1; 1 2 1; 1 1 1]
sys2 = ss2ss(sys1,T)

%%% altri tipi di rappresentazioni dello stesso sistema dinamico

sys3 = canon(sys1,'modal')  %rappresentazione modale o di jordan
sys4 = canon(sys1,'companion')  %rappresentazione in forma canonica di controllo. 
                                %numericamente poco stabile, da evitare se dimensione di A e' grande
sys5 = balreal(sys1)    %realizzazione bilanciata utilizzando i grammiani di controllo e osservabilita'
                        % utilissima per ridurre la dinamica di systemi
                        % molto grandi. vedere toeria sistemi
                                
%%% grammiano di controllo e di ossevazione

Sc = gram(sys1,'c')
So = gram(sys1,'o')

%%%%% controllo ottimo LQR e LQG nel continuo

help lrq
help lqry
help kalman
%% esistono gli equivalenti per tempo discreto



%%%% IMPORTANTE: per capire come usare le funzioni utilizzare il comando
%%%% help nome funzione

help balreal

%%%%% altri comandi utili per controlli automatici e