%%% Modello linearizzato Pendolo Inverso su Carrello

clear all
close all
M=0.3; % Massa carrello
m=0.214; % Massa Pendolo
L=0.5; % Lunghezza asta
g=9.81; % Forza gravita'

A=[0  1    0    0;
   0  0 -m*g/M  0;
   0  0    0    1;
   0  0 (M+m)*L*g/M 0];

B=[0;
    1/M;
    0;
    -L/M];

C=[1 0 0 0; 0 0 1 0];

D=[0; 0];

sys=ss(A,B,C,D); % Creazione del sistema in spazio di stato
disp('************ Autovalori del sistema in anello aperto ***************')
lambda=eig(A)
disp('Il Sistema linearizzato attorno al punto di equilibrio scelto e'' instabile ')
T=0:.005:4; % Tempo di simulazione
r=zeros(length(T)); % Riferimento nullo 
x0=[0 0 .45 0]; % Stato iniziale
[Y,T,X] = initial(sys,x0,T); % Simulazione del sistema in anello aperto con
                             % condizione iniziale x0
% plot dei risultati
figure
plot(T,Y,T,r,'r')
% u=-B*K*X';
% plot(T,u,'g'); % Plot Ingressi
title('Simulazione del sistema in anello aperto con condizione iniziale x0')
legend('Posizione del carrello','Angolo del pendolo','Riferimento di posizione e angolare',0)
xlabel('Tempo [s]')

pause
%%% Raggiungibilita' \ Controllabilita' del Sistema
disp('************ Matrice di Raggiungibilita'' ***************')
con=ctrb(A,B)

sprintf('La dimensione del sottospazio lineare raggiungibile e'' %d ',rank(con))
sprintf('Il sistema e'' quindi completamente raggiungibile e controllabile ',rank(con))
pause

disp('************ Autovalori in anello chiuso desiderati ***************')
p1=[-40; -20; -2+i; -2-i] % Tali autovalori possono essere fissati 
                           % a piacere in quanto il sistema e' completamente raggiungibile
K=place(A,B,p1) % K1 e' un vettore di R^n, tale che:     lambda(A-BK) = p1
% N.B.: il comando 'acker' e' meno stabile numericamente e funziona solo per sistemi SISO                
lambda_an_chiuso=eig(A-B*K)
pause
disp('************ Simulazione in Anello Chiuso ***************')
T=0:.005:4; % Tempo di simulazione
r=zeros(length(T)); % Riferimento nullo 
%%% Matrici in anello chiuso
Ac=A-B*K;
Bc=B*K;
Cc=C-D*K;
Dc=D*K;% ...il riferimento e' nullo
%%%
sysc=ss(Ac,Bc,Cc,Dc); % Sistema equivalente in anello chiuso 
x0=[0 0 .45 0]; % Stato iniziale

[Y,T,X] = initial(sysc,x0,T); % Simulazione del sistema in anello chiuso con
                          % condizione iniziale x0
% plot dei risultati
figure
plot(T,Y,T,r,'r')
title('Simulazione del sistema in anello chiuso con condizione iniziale x0')
legend('Posizione del carrello','Angolo del pendolo','Riferimento di posizione e angolare',0)
xlabel('Tempo [s]')
pause
%%%%%%%%%%%%%%%% Inseguimento di riferimento
r1=0.25*ones(size(T));
r2=zeros(size(T));
r=[r1 r2 r2 r2];

[Y,T, X] = lsim(sysc,r,T); 

figure
plot(T,Y,T,r,'g')
title('Inseguimento di riferimento')
legend('Posizione del carrello','Angolo del pendolo','Riferimento di posizione e angolare',0)
xlabel('Tempo [s]')
pause

disp('************ Condizione iniziale ASSURDA ***************')
T=0:.005:4; % Tempo di simulazione
r=zeros(length(T)); % Riferimento nullo 
%%% Matrici in anello chiuso
Ac=A-B*K;
Bc=zeros(size(B)); % ...il riferimento e' nullo
Bc=B*K;
Cc=C-D*K;
Dc=zeros(size(D));
Dc=D*K;% ...il riferimento e' nullo
%%%
sysc=ss(Ac,Bc,Cc,Dc); % Sistema equivalente in anello chiuso 
x0=[0 0 1.5 0]; % Stato iniziale..corrisponde a 90'!!!!

[Y,T,X] = initial(sysc,x0,T); % Simulazione del sistema in anello chiuso con
                          % condizione iniziale x0
% plot dei risultati
figure
plot(T,Y,T,r,'r')
% u=-B*K*X';
% plot(T,u,'g'); % Plot Ingressi
title('Simulazione del sistema in anello chiuso con condizione iniziale x0')
legend('Posizione del carrello','Angolo del pendolo','Riferimento di posizione e angolare',0)
xlabel('Tempo [s]')
disp('************ !!! FUNZIONA TUTTO !!! ??? COME MAI ??? ***************')
pause
break
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%% Osservabilita' \ Ricostruibilita' del Sistema %%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('************ Progetto Stimatore di stato ***************')

disp('************ Matrice di Osservabilita'' ***************')
obs=obsv(A,C)

sprintf('La dimensione del sottospazio lineare non osservabile e'' %d ',length(A)-rank(obs))
sprintf('Il sistema e'' quindi completamente osservabile e ricostruibile ')
pause

disp('************ Poli dell''osservatore desiderati ***************')
po=[-40; -20; -2+i; -2-i]; % Tali autovalori possono essere fissati 
                           % a piacere in quanto il sistema e' completamente raggiungibile
L=place(A',C',po)' % L e' un vettore di R^n, tale che:     lambda(A-LC) = po

lambda_obs=eig(A-L*C)
pause

disp('************ Simulazione  ***************')
%%%% 
% Supponiamo di far evolvere il nostro sistema partendo da un istante
% iniziale qualsiasi e con una sequenza di 300 ingressi (lunghezza 
% del tempo di simulazione T precedente).
x0=rand(4,1);   % Stato Iniziale
%%% Matrici in anello chiuso
Ac=A-B*K;
Bc=B*K;
Cc=C-D*K;
Dc=D*K; % ...il riferimento e' nullo
%%%


%%%
% Proviamo a partire da uno stato stimato x_hat casuale, e usando le
% uscite e gli ingressi precedenti, e stimiamo l'evoluzione dello
% stato tramite lo stimatore L precedentemente calcolato.
Aco=[A,-B*K;L*C,A-B*K-L*C]; % Il vettore di stato e'   X=[x, x_hat];
Bco=[B*K;B*K];
Cco=[C,zeros(2,length(A))];
Dco=zeros(2,4);

T=0:0.005:10;

rr1=0.25*ones(size(T));
rr2=zeros(size(T));

rr=[rr1 ;rr2 ;rr2; rr2];

sysco=ss(Aco,Bco,Cco,Dco);
x_hat=rand(4,1);
XX_hat=x_hat;
disp('************ Plot dei Risultati ***************')

[YY0,T,XX0] = initial(sysco,[x0;x_hat],T); % Evoluzione libera
 
[YYr,T,XXr]=lsim(sysco,rr,T); % Inseguimento riferimento  r
figure
subplot(2,2,[1 2]);
plot(T,XX0(:,1),'g',T,XX0(:,5),'r')
title('Output-feedback: Simulazione Regolatore')
legend('Traiettoria Stato Stimato', 'Traiettoria Stato Vero')
subplot(2,2,[3 4]);
plot(T,XXr(:,1),'g*',T,XXr(:,5),'r')
title('Output-feedback: Inseguimento di riferimento')
%%%%










break


disp('************ Controllo LQR ***************')


R=1;
Q=C'*C;
[K2,S,p2]=lqr(A,B,Q,R);
Ac=A-B*K2;
Bc=zeros(size(B)); % ...il riferimento e' nullo
Bc=B*K2;
Cc=C-D*K2;
Dc=zeros(size(D));
Dc=D*K2;% ...il riferimento e' nullo
sysc=ss(Ac,Bc,Cc,Dc); % Sistema equivalente in anello chiuso 
x0=[0 0 .60 0]; % Stato iniziale

[Y,T,X] = initial(sysc,x0,T); % Simulazione del sistema in anello chiuso con
                          % condizione iniziale x0
% plot dei risultati
figure
plot(T,Y,T,r,'r')