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

fh = figure; hold on; 

% do a representative number of these problems: 
% 
%rand('seed',0); randn('seed',0); 

% initial conditions/distribution assumed for the state x(0): 
n    = 2; 
mu_0 = zeros(n,1);
P_0  = zeros(n);

% the timestep size:
deltaT = 0.5; 

% the dynamic model noise variance: 
Q        = deltaT * [ 2, 0; 0, 0 ];

% dynamic equation coefficient: 
F        = [ -2, 1; 1, -1 ]; 
Phi      = [ 1 - 2 * deltaT, deltaT ; deltaT, 1 - deltaT ] + 0.5 * ( F^2 ) * deltaT^2 ; 

% measurement coefficient: 
H        = [ 0, 1 ]; 

% measurement error correlation (something very small needed for convergence): 
R        = 1.e-6;

% My indexing convention: 
%   Matlab       Kalman Filtering Index
%     i=1   <=>         i=0
% 
Ntimesteps = 4;

% storage: 
% 
Pminus     = zeros(n*n,Ntimesteps); % = P_k(-) 
Pplus      = zeros(n*n,Ntimesteps); % = P_k(+) 

% seed/start the Kalman filter: (i=0 case) 
% -- 1: explicitly seed the Kalman filter with a zero expectation: 
% 
%Pminus(:,1) is not used 
Pplus(:,1) = P_0(:);

for ti=1:Ntimesteps,
  
  % get P(+): 
  Pp = reshape( Pplus(:,ti), [n,n] ); 

  % Perform propagation (prediction):
  % 
  Pm             = Phi * Pp * (Phi.') + Q; 
  Pminus(:,ti+1) = Pm(:); 
  
  % the Kalman gain: 
  % 
  K = Pm * (H.') / (H * Pm * H.' + R); 

  % Estimate new covariance
  %
  Pp            = Pm - K * H * Pm;
  Pplus(:,ti+1) = Pp(:);

end

Pminus
Pplus 

figure(fh); 
hpm = plot( Pminus(1,:), 'o-' ); hold on; 
hpp = plot( Pplus(1,:), 'o-r' ); %axis tight; 
legend( [hpm, hpp], {'var(v_1(-))', 'var(v_1(+))'}, 'location', 'southeast' ); 

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