%   
% Written by:
% -- 
% John L. Weatherwax                2006-08-28
% 
% email: wax@alum.mit.edu
% 
% Please send comments and especially bug reports to the
% above email address.
% 
%-----

close all; clc; clear; randn('seed',0); rand('seed',1);

phi = 0.7;

Q = 10;
R = 1;
H = 1;

N = 2500; % the number of measurements

addpath( '../../../FullBNT-1.0.4/Kalman/' );
addpath( '../../../FullBNT-1.0.4/KPMstats/' );

x0 = 0; P0 = 0;
% generates measurements from x_0, x_1, ... x_{N}, x_{N+1}
[x_truth,z] = sample_lds( phi, H, Q, R, x0, N+1 ); % we need *one* extra measurment in z to compute q_k since it needs \hat{x}_{k+1} 

if( 0 )
  f1 = figure();
  ts = 0:N;
  hx1 = plot( ts, x_truth, '-g' ); hold on; 
  hz1 = plot( ts(2:end), z(2:end), 'xk' ); % skip the measurement from x_0 
  legend( [hx1,hz1], {'state','measurment'}, 'location', 'best' );
  xlabel('time (sec)'); ylabel('x');
end
  
% Part (b): Perform Kalman filtering on this system using the correct values of Q and R: 
%
[rbar,Rhat,qbar,Qhat] = sect_7_prob_3_noise_adaptive_filtering(z, phi, H, Q, R, x0, P0);

% Part (c): Perform Kalman filtering on this system with incorrect values of Q and R: 
%
[rbar_i ,Rhat_i ,qbar_i ,Qhat_i ] = sect_7_prob_3_noise_adaptive_filtering(z, phi, H, 20, 0.5, x0, P0);
[rbar_ii,Rhat_ii,qbar_ii,Qhat_ii] = sect_7_prob_3_noise_adaptive_filtering(z, phi, H,  8,   2, x0, P0);

% Plot the time evolution of each of the variables: 
%
fhrbar = figure();
rb1 = plot( rbar, '-g' ); hold on; 
rb2 = plot( rbar_i, '-r' ); 
rb3 = plot( rbar_ii, '-b' );
legend( [rb1,rb2,rb3], {'rbar (Q,R)=(10,1)','rbar (Q,R)=(20,0.5)', 'rbar (Q,R)=(8,2)'}, 'location', 'best' );
xlabel('time'); ylabel('rbar estimate'); title('rbar');
grid on;
%saveas( gcf, '../../WriteUp/Graphics/Chapter4/sect_7_prob_3_rbar_plot.eps', 'epsc' );

fhrhat = figure();
rh1 = plot( Rhat, '-g' ); hold on; 
rh2 = plot( Rhat_i, '-r' ); 
rh3 = plot( Rhat_ii, '-b' );
legend( [rh1,rh2,rh3], {'Rhat (Q,R)=(10,1)','Rhat (Q,R)=(20,0.5)', 'Rhat (Q,R)=(8,2)'}, 'location', 'best' );
xlabel('time'); ylabel('Rhat (measurement covariance) estimate'); title('Rhat');
grid on;
%saveas( gcf, '../../WriteUp/Graphics/Chapter4/sect_7_prob_3_Rhat_plot.eps', 'epsc' );

fhqbar = figure();
qb1 = plot( qbar, '-g' ); hold on; 
qb2 = plot( qbar_i, '-r' ); 
qb3 = plot( qbar_ii, '-b' );
legend( [qb1,qb2,qb3], {'qbar (Q,R)=(10,1)','qbar (Q,R)=(20,0.5)', 'qbar (Q,R)=(8,2)'}, 'location', 'best' );
xlabel('time'); ylabel('qbar estimate'); title('qbar');
grid on;
%saveas( gcf, '../../WriteUp/Graphics/Chapter4/sect_7_prob_3_qbar_plot.eps', 'epsc' );

fhrhat = figure();
qh1 = plot( Qhat, '-g' ); hold on; 
qh2 = plot( Qhat_i, '-r' ); 
qh3 = plot( Qhat_ii, '-b' );
legend( [qh1,qh2,qh3], {'Qhat (Q,R)=(10,1)','Qhat (Q,R)=(20,0.5)', 'Qhat (Q,R)=(8,2)'}, 'location', 'best' );
xlabel('time'); ylabel('Qhat (process noise covariance) estimate'); title('Qhat');
grid on;
%saveas( gcf, '../../WriteUp/Graphics/Chapter4/sect_7_prob_3_Qhat_plot.eps', 'epsc' );