%%%
%%% Progetto filtro di Kalman per il problema LQG del maglev
%%%

%%% Matrici note: A_maglev,B_maglev,C_maglev,D_maglev


%%% Verificare l'osservabilita' della coppia (Apend,Cmis)
no = rank(obsv(A_maglev,C_maglev))

%%% Filtro di Kalman con 3 ingressi e 4 uscite

%%%
%%% Si fa riferimento al filtro di Kalman per il modello
%%%
%%%       .
%%%       x = Ax + Bu + Gw            {State equation}
%%%       y = Cx + Du + Hw + v        {Measurements}
%%%

G_maglev = B_maglev;

%%% Verificare la controllabilita' della coppia (A_maglev,Gmod)
nc = rank(ctrb(A_maglev,G_maglev))

W = 1;                                 % = E{ww'},     
V = diag([0.00001 0.00001 0.01]);      % = E{vv'}

[Pkf,Ekf,Kkft] = care(A_maglev',C_maglev',G_maglev*W*G_maglev',V); % CARE duale!

Kkf = Kkft'; 

%%% Matrici del filtro di Kalman: E' un osservatore dello stato!

Akf = A_maglev - Kkf * C_maglev;
Bkf = [B_maglev Kkf];
Ckf = eye(3);
Dkf = zeros(3,4);

return