%%%
%%% Kalman Filter design example for FDI
%%%

clear all, close all, clc

A = [1.1269   -0.4940    0.1129
     1.0000         0         0
          0    1.0000         0];

B = [-0.3832       % This input is the control
      0.5919       % signal
      0.5191];

C = [1 0 0]; 

Tc = 1;

D = 0; % The model has 1 input and 1 output!!!

x0 = [0.001 0.005 -0.003];

Q = (0.3)^2; % Process noise = 20%
R = (0.3)^2; % Output noise = 20%

%%%
%%% Check the controllability of (A,B) and the 
%%% observability of (A,C), which must be equal
%%% to the dimension of A
%%%

rank(ctrb(A,B)) % == 3
rank(obsv(A,C)) % == 3

[Pkf,Ekf,Kkft] = dare(A',C',B*Q*B',R); % Dual CARE  
                                       % for discrete
Kkf = Kkft';                           % time systems!

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

Akf = A - Kkf * C;
Bkf = [B Kkf];
Ckf = C;
Dkf = zeros(1,2); % 1 output (row of Dkf) and 2 inputs 
                  % (columns of Dkf)

%%%
%%% Comparison with the output dynamic observer
%%%

v = [0.01 0.015 0.017];
Ko = place(A',C',v)';
Ao = A - Ko*C;
Bo = [B Ko];
Co = C;
Do = zeros(1,2);

return