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

% the eigenvalues of F are all less than one so the system is stable
F = [ 0.95, 0.0 ; 0.0, 0.999 ]; 
H = [ 1, 1 ]; 

% 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 
rank(G)
eig(G)

% construct the controlabilty matrix:
M=2;
cont_M = []; 
for j=0:(M-1)
  cont_M = [ cont_M, F^j ];
end
cont_M 
rank(cont_M)

C_x_inf = eye(2);
C_w = [ 0.0975, 0 ; 0, 0.002  ];

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