%%%
%%% UIO and KF designs
%%%

% It needs the A, B, C and D matrices

%%%
%%% Preliminary tests
%%%

% Controllability

rank(ctrb(A,B(:,1))) % == 3
rank(ctrb(A,B(:,2))) % == 3

% Observability

rank(obsv(A,C(1,:))) % == 3
rank(obsv(A,C(2,:))) % == 3

% UIO

rank(B(:,1)), rank(C*B(:,1)) % Must be equal
rank(B(:,2)), rank(C*B(:,2)) % Must be equal

H1 = B(:,1)*pinv(C*B(:,1));
rank(obsv((A-H1*C*A),C)) % == 3

H2 = B(:,2)*pinv(C*B(:,2));
rank(obsv((A-H2*C*A),C)) % == 3

%%%
%%% KF design for the 2 outputs
%%% 

q1 = (0.1)^2;  % Noise variance on input #1
q2 = (0.05)^2; % Noise variance on input #2
Q = diag([q1 q2]);

r1 = (7)^2;   % Noise variance on output #1
r2 = (0.6)^2; % Noise variance on output #2

%%%
%%% KF for output #1
%%%

v = [0.75 0.80 0.85]; % Eigenvaues for the 2 UIOs

[Pkf1,Ekf1,Kkft1] = dare(A',C(1,:)',B*Q*B',r1); % KF #1 gain  
Kkf1 = Kkft1';                                    

%%% Kalman filter matrices: apart from the Kalman gain, 
%%% it is an output observer!

Akf1 = A - Kkf1 * C(1,:);
Bkf1 = [B Kkf1];
Ckf1 = C(1,:);
Dkf1 = zeros(1,3); % 1 output and 3 inputs

%%%
%%% KF for output #2
%%%

[Pkf2,Ekf2,Kkft2] = dare(A',C(2,:)',B*Q*B',r2); % KF #1 gain  
Kkf2 = Kkft2';                                    

Akf2 = A - Kkf2 * C(2,:);
Bkf2 = [B Kkf2];
Ckf2 = C(2,:);
Dkf2 = zeros(1,3); % 1 output and 3 inputs


%%%
%%% UIO for isolation of the input #2
%%% just look at the matrix T1*B!
%%%

E1 = B(:,1);
H1 = E1*pinv(C*E1);
T1 = eye(size(H1*C)) - H1*C;
K11  = place( (A-H1*C*A)' , C', v )';
F1 = A-H1*C*A - K11*C;
K21  = F1*H1;
K1 = K11 + K21;

Auio1 = F1;
Buio1 = [T1*B K1];
Cuio1 = eye(3);
Duio1 = [zeros(3,2) H1];

%%%
%%% UIO for isolation of the input #1
%%% just look at the matrix T2*B!
%%%


E2 = B(:,2);
H2 = E2*pinv(C*E2);
T2 = eye(size(H2*C)) - H2*C;
K12  = place( (A-H2*C*A)' , C', v )';
F2 = A-H2*C*A - K12*C;
K22  = F2*H2;
K2 = K12 + K22;

Auio2 = F2;
Buio2 = [T2*B K2];
Cuio2 = eye(3);
Duio2 = [zeros(3,2) H2];

return