% 
% 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.65, -0.06; -0.375, 0.65 ]; 
H = [ 1, 1 ]; 
G = [ 2 ; 5 ];
C_w = G' * G; % the state noise covariance 
C_v = 1; % the measurement noise covariance 

% compute the eigenvalues of F (all are less than one ... system is stable)
evalue = eig(F)

% the observability Grammian: 
G = zeros(2); 
for j=1:200, 
  G_add = ( H * F^j )' * ( H * F^j );
  if( mod(j,20)==0 ) fprintf('(%10d) norm G_add= %10.6f\n', j, norm(G_add) ); end; 
  G = G + G_add; 
end
G 
rank(G)
eig(G)

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

% the rank of this matrix is 2 = M so this system is controllable
rank(cont_M)

% because we know that this exists lets compute the steady state solution to the Ricatti equation 
% (by iteration) and determine the prediction covariance matrix P(\inft)
%
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)

% compute the steady-state innovation matrix: 
%
S = H * P * H.' + C_v;
S

% compute the steady-state Kalman gain matrix:
%
K = P * (H.') * inv(S);
K

% compute the error covariance matrix (the discrete Lyapunov equation):
%
C_x_inf = 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)