% Esempio: decoposizione canonica di Kalman

A=[0  0 0  0  0  0 ...
  ;0 -2 0  0  0  0 ...
  ;0  0 1  0  0  0 ...
  ;0  0 0 -1  0  0 ...
  ;0  0 0  1 -1  0 ...
  ;0  1 0  0  0 -2]

B=[0 1 ...
  ;0 0 ... 
  ;0 0 ...
  ;1 0 ...
  ;0 0 ...
  ;0 0]

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

D=[0 0 ...
  ;0 0]


%help ctrb
%CTRB  Compute the controllability matrix.
% 
%    CO = CTRB(A,B) returns the controllability matrix [B AB A^2B ...]

R = ctrb(A,B)

% help obsv
%  OBSV  Compute the observability matrix.
%  
%     OB = OBSV(A,C) returns the observability matrix [C; CA; CA^2 ...]

O=obsv(A,C)


% trovo lo spazio generato dalle colonne di R
% rref mette la matrice in forma canonica per righe attraverso operazioni
% elementari e restituisce gli indici delle colonne pivot (quelle a cui
% appartengono i leading coefficients)

[~, vpivot] = rref(R);
Xr=R(:,vpivot)  %estrae da R le colonne i cui indici sono nel vettore vpivot

% oppure, usando la decomposizione ai valori singolari
[U,SIGMA,V]=svd(R)
Xr=U(:,1:rank(R))

% ora le colonne di Xr sono una base per il sottospazio raggiungibile


%trovo base per il nucleo di O, cioč il sottospazio non osservabile
Xno=null(O)

% il sottospazio ortogonale a quello non osservabile
Xo=null(Xno','r')



% il sottospazio ortogonale a quello raggiungibile (ortogonale a tutte le
% colonne, ovvero a tutte le righe della trasposta)
Xnr=null(Xr','r')


%calcolo T
%intersezione fra Xr e Xno
T1=null([Xnr,Xo]','r')

%intersezione fra Xr e il sottospazio ortogonale a T1
T2=null([Xnr,T1]','r')

%intersezione fra Xno e il sottospazio ortogonale a T1 
T3=null([Xo,T1]','r')

T4=null([T1 T2 T3]','r')

T=[T1 T2 T3 T4]

% la forma canonica č dunque:
hatA=T^-1*A*T
hatB=T^-1*B
hatC=C*T
hatD=D

% nella funzione di trasferimento compaiono solamente i modi raggiungibili
% e osservabili:
sistema=ss(A,B,C,D)
tf(sistema)