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

Phi = 0.8;
H = 1;
Q = 1;
M = 0.25;
%M = 0;
%M = -0.25;
R = 0.1; 

N = 50;

[xtruth,z] = sect_4_2_gen_xz(Phi, H, Q, M, R, N);

figure();
hx = plot( 0:N, xtruth, '-og' ); hold on;
xlabel('timestep' ); ylabel('state/measurement');
hz = plot( 1:N, z, 'xk' );

[xhatplus,Pplus,xhatminus,Pminus] = sect_4_2_kfilter_z(Phi, H, Q, M, R, z);

hxhat = plot( 0:N, xhatplus, '-r' );
legend( [hx,hz,hxhat], {'truth', 'measurement', 'xhat(+)'}, 'location', 'north' );
c = 1.96;
plot( 0:N, xhatplus-c*sqrt(Pplus), '-c' );
plot( 0:N, xhatplus+c*sqrt(Pplus), '-c' );

fn = [ '../../WriteUp/Graphics/Chapter4/sect_4_prob_2_M_', num2str(M), '.eps' ];
%saveas( gcf, fn, 'epsc' );