% dinamica longitudinale di un aeromobile


A=[-0.0507     -3.861    0       -32.2;...
   -0.00117    -0.5164   1          0;...
   -0.000129    1.4168   -0.4932    0;...
   0              0        1        0]


B=[0 -0.0717 -1.645 0]'   

C=[0 0 1 0]


% calcoliamo la matrice di osservabilità


O= [C;...
    C*A;...
    C*A^2;...
    C*A^3]  
                 
                  
rank(O)

% il rango è 4, quindi il sistema
% è completamente osservabile, quindi gli autovalori dell'osservatore sono
% assegnabili a piacere

j=sqrt(-1) % per default in Matlab le variabili j e i indicano l'unità immaginaria, ma
           % è bene dare questa istruzione per evitare problemi nel caso in
           % cui in uno script precedente la variabile sia stata usata

% gli autovalori che si vogliono assegnare:
lambda1 = -0.1
lambda2 = -0.421
lambda3 = -0.587
lambda4 = -1


% posso usare la formula di Ackermann duale oppure trovare il problema
% duale e usare la formula di Ackermann primale. Scelgo la seconda strada
% [per esercizio provare con la prima strada]

% sto risolvendo il problema duale cioè l'assegnazione degli autovalori di A' - C'K
menoK=acker(A',C',[lambda1 lambda2 lambda3 lambda4])

L=menoK'    %siccome acker restituisce già il valore cambiato di segno, cioè -K,
            %NON bisogna cambiarlo di segno, ma solamente trasporlo!

% gli autovalori dell'osservatore sono quelli voluti:
eig(A-L*C)

pause 

% oppure la formula di Ackermann duale
L= polyvalm(poly([lambda1 lambda2 lambda3 lambda4]),A)*O^-1*[0 0 0 1]'


% ora il filtro di Kalman

W = 1e-4 % R_1
Gamma = [0 1 1 0]'
V = 1e-2 % R_2
 
% verifico le ipotesi di controllabilità e osservabilità
rank(obsv(A,C))

rank(ctrb(A,Gamma))

% abbiamo già visto [esercizio03] che l'istruzione
% Pottima = care(A,B,M'*Q*M,R)

% risolve l'equazione algebrica di Riccati 
% A' P + P A - P B R^-1 B' P + M' Q M = 0

% sfruttando la dualità e in particolare le sostituzioni [cfr slide 92 della parte 9]

% A -> A'
% B -> C'
% M -> Gamma'
% Q -> W
% R -> V

% si può risolvere l'equazione algebrica duale di
% Riccati che è quella che permette di ottenere il guadagno del filtro di
% Kalman


Pottima = care(A',C',(Gamma')'*W*(Gamma'),V)


% il guadagno ottimo è dunque
Lottimo = Pottima*C'*V^-1

% gli autovalori dell'osservatore ottimo sono
CLeig=eig(A-Lottimo*C)



% che sono a parte reale strettamente negativa, come assicurato dal teorema
% sul filtro di Kalman


% in alternativa esiste un'istruzione apposita:
% help kalman
%  --- help for ss/kalman ---
% 
%  KALMAN  Kalman state estimator.
%  
%     [KEST,L,P] = KALMAN(SYS,QN,RN,NN) designs a Kalman estimator KEST for
%     the continuous- or discrete-time plant SYS. For continuous-time plants
%        .
%        x = Ax + Bu + Gw            {State equation}
%        y = Cx + Du + Hw + v        {Measurements}
%  
%     with known inputs u, process disturbances w, and measurement noise v,
%     KEST uses [u(t);y(t)] to generate optimal estimates y_e(t),x_e(t) of 
%     y(t),x(t) by:
%         .
%        x_e  = Ax_e + Bu + L (y - Cx_e - Du)
%  
%        |y_e| = | C | x_e + | D | u
%        |x_e|   | I |       | 0 | 
%  
%     KALMAN takes the state-space model SYS=SS(A,[B G],C,[D H]) and the 
%     covariance matrices:
%  
%        QN = E{ww'},     RN = E{vv'},     NN = E{wv'}.
%  
%     The row size of QN specifies the length of w and NN is set to 0 when 
%     omitted. KALMAN returns the estimator gain L and the steady-state error 
%     covariance P (solution of the associated Riccati equation).

sistema=ss(A,[B Gamma],C,0)
[osservatore,Lottimo]=kalman(sistema,W,V)

%in alternativa ancora:
sistema_duale=ss(A',C',B',0)
Lottimo = lqr(sistema_duale,Gamma*W*Gamma',V)'  % <-- notare la trasposizione

% ESERCIZIO PER CASA: progettare un controllore a retroazione dello stato
% che assegni gli autovalori in
% -1.25 +- j2.2651 e -0.01 +- 0.095
%