%
% 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.7, -0.15; 0.03, 0.79 ];
H = eye(2);

% the process noise covariance (in the form needed):
Q = [ 0.15; 0.21 ] * [ 0.15; 0.21 ]'; 

% the AR model of measurement noise:
Psi = 0.5 * eye(2);
Q_nu = 0.05 * eye(2);

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

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_4_gen_xz(Phi, H, Q, Psi, Q_nu, 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' );

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

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

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

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

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

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

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