% 
% 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: 
% 
for ii=1:3, 
    %rand('seed',0); randn('seed',0); 

    % initial conditions/distribution assumed for the state x(0): 
    mu_0 = 0; 
    sigma2_0 = 100; 

    % compute the actual value of our constant state x(i):
    x0 = mvnrnd(mu_0,sigma2_0); 

    % the measurement noise variance: 
    sigma2_v = 1; 

    % the dynamic model noise variance: 
    sigma2_w = 1; 

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

    % storage: 
    % 
    zhat            = zeros(1,Ntimesteps); 
    zi              = zeros(1,Ntimesteps); 
    xip1_given_i    = zeros(1,Ntimesteps); 
    cip1_given_i    = zeros(1,Ntimesteps); 
    s               = zeros(1,Ntimesteps); 
    k               = zeros(1,Ntimesteps); 
    xi_given_i      = zeros(1,Ntimesteps); 
    ci_given_i      = zeros(1,Ntimesteps); 

    % explicitly seed the Kalman filter with zero: 
    % 
    xip1_given_i(1) = mu_0;              % i=0 case 
    cip1_given_i(1) = sigma2_0;

    % take a random initial condition (around zero): 
    % 
    xip1_given_i(1) = mvnrnd(mu_0,sigma2_0); 
    cip1_given_i(1) = sigma2_0;

    for ti=1:Ntimesteps,

      % Estimate mean/covariance taking the observed measurement at ti into account 
      %
      zhat(ti)       = xip1_given_i(ti);
      s(ti)          = cip1_given_i(ti) + sigma2_v;
      k(ti)          = cip1_given_i(ti) / ( cip1_given_i(ti) + sigma2_v );
      zi(ti)         = x0 + mvnrnd(0,sigma2_w); % simulate measurement at timestep "ti":
      xi_given_i(ti) = xip1_given_i(ti) + k(ti) * ( zi(ti) - zhat(ti) ); 
      ci_given_i(ti) = cip1_given_i(ti) - s(ti)*k(ti)^2; 

      % Perform state propogation (execpt on the last timestep)
      % 
      if( ti<Ntimesteps )
        xip1_given_i(ti+1) = xi_given_i(ti); 
        cip1_given_i(ti+1) = ci_given_i(ti); 
      end

    end

    figure(fh); 
    plot( xip1_given_i ); hold on; 
    plot( x0*ones(1,Ntimesteps), '-g' )
    plot( zi, 'or' ); axis tight; 
end

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