% 
% Written by:
% -- 
% John L. Weatherwax                2009-02-24
% 
% email: wax@alum.mit.edu
% 
% Please send comments and especially bug reports to the
% above email address.
% 
%-----

close all; drawnow; 
clc; 
clear; 

F = [ 0.66, 0.32 ; 0.12, 0.74 ];
H = [ 1.2, 2.4 ]; 

% both eigenvalues of F are less than one ... 
eig(F)

% the observability Grammian ... we can sum to infinity since the sum converges: 
G = zeros(2); 
for j=1:20000, 
  G_add = ( H * F^j )' * ( H * F^j );
  if( mod(j,1000)==0 ) fprintf('(%10d) norm G_add= %10.6f\n', j, norm(G_add) ); end; 
  G = G + G_add; 
end
G 

% note that G is not of full rank ... the system is not observable
rank(G)
eig(G)

% is this system controllable?  Construct the controlability matrix (L=I):
M=2;
cont_M = []; 
for j=0:(M-1)
  cont_M = [ cont_M, F^j ];
end
cont_M 

% note that the rank of cont_M is 2 ... the system is controlable
rank(cont_M)


% compute the steady state solution to the Ricatti equation (by iteration): 
%
C_w = eye(2); 
C_v = 1; 

P = C_w; 
for j=1:200, 
  P_old = P; 
  P_new = F * P * (F') + C_w - F * P * (H') * ( 1 / (H * P * (H') + C_v) ) * H * (P') * (F');
  delta_P = P_new - P_old; 
  if( mod(j,20)==0 ) fprintf('(%10d) norm delta_P= %10.6f\n', j, norm(delta_P) ); end; 
  P = P_new; 
end
P
eig(P)

% are the eigenvalues of the steady-state Kalman filter stable: 
% 
K = P * (H') * ( 1 / (H * P * (H') + C_v) ) ;
tmp = ( eye(2) - K * H ) * F ; 
eig(tmp)

% lets find the solution of the discrete Lyapunov equation: 
%
C_x_inf = eye(2);
%C_w = [ 0.0975, 0 ; 0, 0.002  ];
C_w = eye(2);

for j=1:20000, 
  %if( mod(j,1000)==0 ) fprintf('(%10d) norm G_add= %10.6f\n', j, norm(G_add) ); end; 
  C_x_inf = F * C_x_inf * (F') + C_w; 
end
C_x_inf
eig(C_x_inf)


return;