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

% this is for the augemented system:
Phi = [ 0.7, -0.15, +0.15; 0.03, 0.79, 0.21; 0, 0, 0.5 ];
H = [ 1, 0, 0; 0, 1, 0 ];

% the process noise covariance (in the form needed):
Q = [ 0; 0; 0.5 ] * [ 0; 0; 0.5 ].';

% the measurement noise covariance:
R = 0.1 * eye(2);

N = 100;

% xtruth(:,) holds x_0, x_1, \cdots, x_N (the true state values in each colum)
% z(:,) hold z_1, z_2, \cdots, z_N (measurements at each time point in each colum)
%
[xtruth,z] = sect_4_5_gen_xz(Phi, H, Q, R, N);

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

f2 = figure();
hx2 = plot( 0:N, xtruth(2,:), '-og' ); hold on;
xlabel('timestep' ); ylabel('state x(2)/measurement');
hz2 = plot( 1:N, z(2,:), 'xk' );

f3 = figure();
hx3 = plot( 0:N, xtruth(3,:), '-og' ); hold on; 
xlabel('timestep' ); ylabel('state x(3)=w_k');

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

figure(f1);
hx1hat_plus = plot( 0:N, xhatplus(1,:), '-r' );
legend( [hx1,hz1,hx1hat_plus], {'truth', 'measurement', 'xhat(+)'}, 'location', 'north' );

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

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

figure(f2);
hx2hat_plus = plot( 0:N, xhatplus(2,:), '-r' );
legend( [hx2,hz2,hx2hat_plus], {'truth', 'measurement', 'xhat(+)'}, 'location', 'north' );

c = 1.96;
plot( 0:N, xhatplus(2,:)-c*sqrt(squeeze(Pplus(2,2,:)).'), '-c' );
plot( 0:N, xhatplus(2,:)+c*sqrt(squeeze(Pplus(2,2,:)).'), '-c' );

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

figure(f3);
hx3hat_plus = plot( 0:N, xhatplus(3,:), '-r' );
legend( [hx3,hx3hat_plus], {'truth', 'xhat(+)'}, 'location', 'north' );

c = 1.96;
plot( 0:N, xhatplus(3,:)-c*sqrt(squeeze(Pplus(3,3,:)).'), '-c' );
plot( 0:N, xhatplus(3,:)+c*sqrt(squeeze(Pplus(3,3,:)).'), '-c' );

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