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

k_1  = 1;
k_2  = 0.1; 
k_3u = 10; 
mu_q = 1; % process noise mean
var_q = 1.; % process noise spectral density (variance)
R = 1; % measurement noise covariance

t_0 = 0.0; 
t_final = 20.0;
N = 100; 

% Part (a): Simulate from the nonlinear system
%
[t_k, xtruth, z] = sect_6_1_gen_xz( k_1, k_2, k_3u, mu_q, var_q, R, t_0, t_final, N );

fh = figure; 
hxtruth = plot( t_k, xtruth, '-g' ); hold on; 
hz = plot( t_k, z, 'm.' ); 
xlabel('time'); ylabel('true state'); 

% Part (b)
%
[xhatminus,pminus,xhatplus,pplus] = sect_6_1_linear_kf( k_1, k_2, k_3u, mu_q, var_q, R, z, t_k );

figure(fh); hold on; 
hlinear = plot( t_k, xhatplus, '-k' );

% Plot confidence bounds: 
plot( t_k, xhatplus - 1.96*sqrt(pplus), '-.r' );
plot( t_k, xhatplus + 1.96*sqrt(pplus), '-.r' );

% Part (c)
%
[xhatminus,pminus,xhatplus,pplus] = sect_6_1_extended_kf( k_1, k_2, k_3u, mu_q, var_q, R, z, t_k );

figure(fh); hold on; 
hextended = plot( t_k, xhatplus, '-k' );

% Plot confidence bounds: 
plot( t_k, xhatplus - 1.96*sqrt(pplus), '-.r' );
plot( t_k, xhatplus + 1.96*sqrt(pplus), '-.r' );

legend( [hxtruth,hz,hlinear,hextended], {'truth', 'measurement z', 'linear KF', 'extended KF'}, 'location', 'best');

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