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

% the process noise covariance: 
Q = 1; 

% the AR model of measurement noise:
Psi = 0.2;
Q_nu = 0.064;

% the derived measurement's noise variance and process noise correlations:
M = Q * H'; 
R = H * Q * H' + Q_nu;

N = 50;

% xtruth holds x_0, x_1, \cdots, x_N (the true state values)
% z hold z_1, z_2, \cdots, z_N (measurements at each time point)
%
[xtruth,z] = sect_4_3_gen_xz(Phi, H, Q, Psi, Q_nu, 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_3_kfilter_z(Phi, H, Q, Psi, M, R, z);

hxhat_minus = plot( 1:N, xhatminus, '.-r' );
hxhat_plus = plot( 1:(N-1), xhatplus, '-r' );
legend( [hx,hz,hxhat_minus,hxhat_plus], {'truth', 'measurement', 'xhat(-)', 'xhat(+)'}, 'location', 'north' );

c = 1.96;
plot( 1:(N-1), xhatplus-c*sqrt(Pplus), '-c' );
plot( 1:(N-1), xhatplus+c*sqrt(Pplus), '-c' );

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