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


Cmis = [1 0 0 0;0 0 1 0]; % Vengono misurate solo 2 uscite: posizione 
                          % X e angolo Theta!
Dmis = zeros(2,1);        % 2 uscite misurate ed 1 ingresso

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

%%% 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}
%%%

Gmod = Bpend;
Hmod = zeros(2,1);

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

sysmis = ss(Apend,[Bpend Gmod],Cmis,[Dmis Hmod]);

W = 0.05;                      % = E{ww'},     
V = diag([0.001 0.0001]);      % = E{vv'}

%[Kalman_Filter_sys,Kkf,Pkf] = kalman(sysmis,W,V);

%[Kkft,Pkf,Ekf] = lqr(Apend',Cmis',Gmod*W*Gmod',V); % CARE duale!

%%% oppure

[Pkf,Ekf,Kkft] = care(Apend',Cmis',Gmod*W*Gmod',V); % CARE duale!

Kkf = Kkft'; 

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

Akf = Apend - Kkf * Cmis;
Bkf = [Bpend Kkf];
Ckf = eye(4);
Dkf = zeros(4,3);