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

% Use the previous code to compute the discretized noise covariance matrix Q(\Delta t):
omega_n = 6.28; 
zeta = 0.3; 
a = -omega_n^2; 
b = -2*zeta*omega_n; 

q_C = 1;
L = [ 0; 1 ];
L_Qc_LT = L * q_C * L';

Nsteps = 100; % number of integration steps 





% Pick a different value (smaller) of Delta t and repeat this calculation:
% 
dt = 0.01; 

Q = example_4_2_2_compute_qkm1( a, b, L_Qc_LT, dt, Nsteps );
Q

F = [ 0, 1; -omega_n^2, -2*zeta*omega_n ];
Phi = expm( F * dt );
Phi

% iterate the covariance update equation for a while: 
%
T = 4; % final time in seconds
N = ceil(T/dt);

% Save the evolution of uncertainty: 
p11 = zeros(1,N);
p12 = zeros(1,N); 
p22 = zeros(1,N);
Pkm1 = zeros(2);
for ki=1:N
  Pk = Phi * Pkm1 * Phi' + Q;
  
  p11(ki) = Pk(1,1);
  p12(ki) = Pk(1,2);
  p22(ki) = Pk(2,2);
  
  Pkm1 = Pk;
end



fp22 = figure; hold on; 
plot( (1:N)*dt, p22 * 10, '.g', 'markersize', 6 );
title(['p_{22} scaled \Delta t=',num2str(dt)]);
xlabel('seconds');

fp11 = figure;
plot( (1:N)*dt, p11 * 10^3, '.g', 'markersize', 6 ); hold on; plot( (1:N)*dt, p12 * 10^2, '.g', 'markersize', 6 );
title(['p_{11} and p_{12} scaled \Delta t=',num2str(dt)]);
xlabel('seconds');





% Pick a larger value of Delta t and repeat this calculation:
% 
dt = 0.1; 

Q = example_4_2_2_compute_qkm1( a, b, L_Qc_LT, dt, Nsteps );
Q

F = [ 0, 1; -omega_n^2, -2*zeta*omega_n ];
Phi = expm( F * dt );
Phi

% iterate the covariance update equation for a while: 
%
T = 4; % final time in seconds
N = ceil(T/dt);

% Save the evolution of uncertainty: 
p11 = zeros(1,N);
p12 = zeros(1,N); 
p22 = zeros(1,N);
Pkm1 = zeros(2);
for ki=1:N
  Pk = Phi * Pkm1 * Phi' + Q;
  
  p11(ki) = Pk(1,1);
  p12(ki) = Pk(1,2);
  p22(ki) = Pk(2,2);
  
  Pkm1 = Pk;
end

figure(fp22); 
plot( (1:N)*dt, p22 * 10, 'xk', 'markersize', 10 );
title(['p_{22} scaled']);
xlabel('seconds');

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


figure(fp11); 
plot( (1:N)*dt, p11 * 10^3, 'xk', 'markersize', 10 ); hold on; plot( (1:N)*dt, p12 * 10^2, 'xk', 'markersize', 10 );
title(['p_{11} and p_{12} scaled']);
xlabel('seconds');

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