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

alpha = 1/2; 
beta = 1/4; 
sigmaw2 = 1; 

r1 = alpha / ( 1 - beta );
r2 = ( alpha^2 + beta - beta^2 ) / ( 1 - beta ); 
sigmax2 = sigmaw2 / ( 1 - alpha^2 - 2 * alpha * beta * r1 - beta^2 );

C = [ sigmax2, sigmax2*r1 ; sigmax2*r1, sigmax2 ]

% iterate to compute the discrete Ricatti Equation: 
% 
% 
F   = [ alpha, beta; 1, 0 ]; 
C_w = [ sigmaw2, 0; 0, 0 ]; 
H   = [ 1, 0 ]; 
C_v = 1; 

fprintf('iterating the Ricatti equation\n'); 
P_n = C; 
for ii=1:10
  P_np1 = F * P_n * F.' + C_w - F * P_n * (H.') * ( 1 / ( H * P_n * H.' + C_v ) ) * H * P_n * F.'; 
  fprintf('||P_n-P_np1|| = %10.6f\n', norm(P_np1-P_n)); 
  P_n = P_np1;
end
disp('P_n');
P_n