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

clear functions; 
clear; 
close all; 
clc; 

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

tau = 30; % in seconds
ref_pt_1 = [ 0;    0 ];
ref_pt_2 = [ 10^5; 0 ]; 

n = 4; % the state dimension

F = zeros(n,n);
F(1,3) = 1;
F(2,4) = 1;
F(3,3) = -1/tau;
F(4,4) = -1/tau;

G = zeros(n,2);
G(3,1) = 1/tau;
G(4,2) = 1/tau; 

% Now descretize the continous system t_k(1) = t_0 (the initial time)
%
t_0    = 0.0; 
deltaT = 2.0; % in seconds 
N      = 5*60 / deltaT;
t_k    = t_0 : deltaT : 5*60; % time in seconds

Phi = expm(F*deltaT);

% We dont have F^{-1} (it does not exist) so use the Taylor expansion approximation
expApprox = eye(n);
term      = - F * deltaT ;
for ii=2:20
  expApprox = expApprox + ( term^(ii-1) )/factorial(ii);
end
Gamma = Phi * expApprox * G * deltaT;

Q = diag( [   4,   4, 0.25, 0.25  ] ); % process noise covariance
R = diag( [ 100, 100,    4, 0.001 ] ); % measurement noise covariance

% for problem #4 we need to increase the elements on the diagonal:
%Q = Q + 10 * diag( ones(n,1) );
%R = R + 10 * diag( ones(n,1) );

x_0 = [0; 25000; 10; 0]; % for problem #2
P_0 = diag( [100,100,4,4] );
u_k = [ 20; 0 ]; % a constant vector 

x_0 = [0; 25000; 10; 0]; % for problem #3
P_0 = diag( [0,0,0,0] );
u_k = [ 0; 20 ]; % a constant vector 

% Simulate from the system
%
[xtruth, z_k] = sect_6_2_gen_xz( x_0, Phi, Gamma, u_k, Q, ref_pt_1, ref_pt_2, R, N);

fhs = [];
hxtruths = [];
for ii=1:n
  fhs = [ fhs; figure ];
  hxtruths = [ hxtruths ; plot( t_k, xtruth(ii,:), '-g' ); ]; hold on; 
  %hz = plot( t_k(2:end), z_k(ii,:), 'm.' ); 
  xlabel('time'); ylabel(['true state x(',num2str(ii),')']); 
end

[xhatminus,pminus,xhatplus,pplus] = sect_6_2_extended_kf(x_0, P_0, Phi, Gamma, u_k, Q, R, z_k, t_k, ref_pt_1, ref_pt_2);

for ii=1:n
  figure(fhs(ii)); hold on; 
  hxhat = plot( t_k, xhatplus(ii,:), '-k' ); hold on; 

  % Plot confidence bounds: 
  state = xhatplus(ii,:);
  svar  = squeeze(pplus(ii,ii,:));
  herror = plot( t_k, state(:) - 1.96*sqrt(svar(:)), '-.r' );
  plot( t_k, state(:) + 1.96*sqrt(svar(:)), '-.r' );

  if( ii==1 )
    legend( [hxtruths(ii),hxhat,herror], {'truth', 'extended KF','95% confidence bounds'}, 'location', 'southeast');
  end
  
  fn = [ '../../WriteUp/Graphics/Chapter4/sect_6_prob_2_plot_x', num2str(ii), '.eps' ];
  fn = [ '../../WriteUp/Graphics/Chapter4/sect_6_prob_3_plot_x', num2str(ii), '.eps' ];
  fn = [ '../../WriteUp/Graphics/Chapter4/sect_6_prob_4a_plot_x', num2str(ii), '.eps' ];
  fn = [ '../../WriteUp/Graphics/Chapter4/sect_6_prob_4b_plot_x', num2str(ii), '.eps' ];
  %saveas( gcf, fn, 'epsc' ); 

end