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

close all; clc; clear; 

% Our discrete coefficient matrices:
Phi = [ 1, 1; -1, 0.4 ]; 
Lambda = [ 0; 1 ]; 

n = size(Phi,2); % the state dimension
N = 20; % the number of steps

randn('seed',0); rand('seed',0);

% sample the dynamic process:
X = zeros(n,N);
for ii=1:N-1
  X(:,ii+1) = Phi * X(:,ii) + Lambda * randn(1);
end

f1 = figure();
x1 = plot( X(1,:), '-xg' ); hold on; 
xlabel('timestep'); ylabel('state value' );

f2 = figure();
x2 = plot( X(2,:), '-xg' ); hold on; 
xlabel('timestep'); ylabel('state value' );

% the mean starts at 0 and never changes since m_k = Phi m_{k-1}:
m = zeros(n,N);

figure(f1); x1m = plot( m(1,:), '-r' ); 

figure(f2); x2m = plot( m(2,:), '-r' ); 

% iterate the covariance equation: P_k = Phi P_{k-1} Phi' + Lambda * Q_{k-1} * Lambda'
%
Q = [ 0.1801, 0.2006 ; 0.2006, 0.4515 ]; % <- *discrete* process noise covariance... derived from problem 3 in this section... and the lambda is already taken care of
P = zeros( n^2, N );
for ii=1:N-1,
  Pold = P(:,ii); 
  Pold = reshape( Pold, [2,2] ); 
  Pnew = Phi * Pold * Phi' + Q;
  P(:,ii+1) = Pnew(:);
end

c = norminv(0.95);
figure(f1); 
x1up = plot( m(1,:) + c*sqrt(P(1,:)), '-.r' );
x1um = plot( m(1,:) - c*sqrt(P(1,:)), '-.r' );

legend( [x1,x1m,x1up,x1um], {'x1','x1 mean','x1 upper limit','x1 lower limit'}, 'location', 'northwest' );

%saveas( gcf, '../..//WriteUp/Graphics/Chapter4/sect_2_prob_2_x1_plot', 'epsc' );


c = norminv(0.95);
figure(f2); 
x2up = plot( m(2,:) + c*sqrt(P(4,:)), '-.r' );
x2um = plot( m(2,:) - c*sqrt(P(4,:)), '-.r' );

legend( [x2,x2m,x2up,x2um], {'x2','x2 mean','x2 upper limit','x2 lower limit'}, 'location', 'northwest' );

%saveas( gcf, '../..//WriteUp/Graphics/Chapter4/sect_2_prob_2_x2_plot', 'epsc' );