%   
% 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; randn('seed',0); rand('seed',1);

plot_x23 = false;
increase_diagonal = false;

dt = 0.1;

F = [ -0.8 0.1 0; 0, -0.5, 0.1; 0, 0, 0.1 ];
n = size(F,1);

Phi    = expm( F * dt )
L      = eye(n)
FinvL  = F \ L;
Lambda = Phi * FinvL - FinvL

H = [ 1, 0, 0 ];

%Q = dt * eye(n); % a continious approximation
%R = 0.25/dt;
Q = eye(n);
R = 0.25;
if( increase_diagonal )
  A = 10;
  Q = Q + A * eye(n);
  R = R + A;
end

T = 5; % total time of simulation
N = T/dt; % number of timesteps 

ts = 0:dt:T;

addpath( '../../../FullBNT-1.0.4/Kalman/' );
addpath( '../../../FullBNT-1.0.4/KPMstats/' );

x0 = zeros(n,1);
[x,y] = sample_lds( Phi, H, Q, R, x0, N+1 );

f1 = figure();
x1 = plot( ts, x(1,:), '-g' ); hold on; 
y1 = plot( ts, y, '-ok' ); 
legend( [x1,y1], {'state 1','measurment 1'} );
xlabel('time (sec)'); ylabel('x1');

if( plot_x23 )
  f2 = figure(); 
  x2 = plot( ts, x(2,:), '-g' ); hold on; 
  
  f3 = figure();
  x3 = plot( ts, x(3,:), '-g' ); hold on; 
end

% Perform Kalman filtering on this system:
%
P0 = zeros(n,n);
[xhat, V, VV, loglik, innovations, xpred, Vpred, S, Sinv] = kalman_filter(y, Phi, H, Q, R, x0, P0);

% Plot the Kalman filtering results 
figure(f1);
hx1hat = plot( ts, xhat(1,:), '-xr' ); 
legend( [x1,y1,hx1hat], {'state 1','measurment 1','state 1 Kalman estimate'}, 'location', 'southwest' );

axis( [0,T,-15,+15] );

if( ~increase_diagonal )
  %saveas( gcf, '../../WriteUp/Graphics/Chapter4/sect_3_prob_3_x1_plot.eps', 'epsc' );
else
  %saveas( gcf, '../../WriteUp/Graphics/Chapter4/sect_3_prob_4_x1_plot.eps', 'epsc' );
end

% print the final covariance: 
VV(:,:,end)

% Over the initial time frame ~[0,2] the estimates of x_2 and x_3 are very poor... and we don't plot them.
%
if( plot_x23 )
  figure(f2); 
  hx2hat = plot( ts, xhat(2,:), '-xr' );
  legend( [x2,hx2hat], {'state 2','state 2 Kalman estimate'} );

  figure(f3); 
  hx3hat = plot( ts, xhat(3,:), '-xr' );
  legend( [x3,hx3hat], {'state 3','state 3 Kalman estimate'} );
end