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

savePlots = false; % whether to save our plots or not
a0_guess = -6.0; % an initial guess at the constant value of a

randn('seed',0); rand('seed',0); 

% Generate data from the *true* system: 
omega_n = 2;
zeta = 0.1; 
a = -omega_n^2; 
b = -2*zeta*omega_n; 

% Generate the grid we consider measurements on:
dt = 0.1; 
T = 40;
N = T/dt;
t_k = 0:dt:T;

% Compute the discrete system matrix Phi:
F = [ 0, 1; -omega_n^2, -2*zeta*omega_n ];
Phi = expm( F * dt );

% Use the previous code to compute the discretized noise covariance matrix Q(\Delta t):
q_C = 1000;
L = [ 0; 1 ];
L_Qc_LT = L * q_C * L';

Nsteps = 100; % number of integration steps 

Q = example_4_2_2_compute_qkm1( a, b, L_Qc_LT, dt, Nsteps ); % before multiplication of omega_n^4
Q = omega_n^4 * Q; % after multiplication of omega_n we have the complete process noise covariance

n = 2; % the state dimension

% do the Kalman filtering 
addpath( '../../../FullBNT-1.0.4/Kalman/' );
addpath( '../../../FullBNT-1.0.4/KPMstats/' );

% H = I
%
H = eye(2); R = [ 10, 0; 0, 10 ]; % the measurement noise covariance

% generate a trajectory with the above system and measurement models: 
% x is the TRUE state (t=0)
% y is the measurements 
%
[x_truth,z_k] = sample_lds( Phi, H, Q , R, zeros(n,1), N+1 ); 

% First plot the true states and measurements:
%
f1 = figure();
hx1 = plot( t_k, x_truth(1,:), '-g' ); hold on;
angle_measurement = z_k(1,:);
hx1m = plot( t_k, angle_measurement, 'ko' );
xlabel('time (sec)'); ylabel( 'angle (deg)' );
legend( [hx1,hx1m], {'true angle', 'angle measurement'}, 'location', 'southwest' );
%saveas( gcf, '../../WriteUp/Graphics/Chapter4/example_4_7_1_plot_x1', 'epsc' ); 

f2 = figure();
hx2 = plot( t_k, x_truth(2,:), '-g' ); hold on;
angle_measurement = z_k(2,:);
hx2m = plot( t_k, angle_measurement, 'ko' );
xlabel('time (sec)'); ylabel( 'angle rate (deg/s)' );
legend( [hx2,hx2m], {'true angle', 'angle rate measurement'}, 'location', 'southwest' );
%saveas( gcf, '../../WriteUp/Graphics/Chapter4/example_4_7_1_plot_x2', 'epsc' ); 

f3 = figure();
hx3 = plot( t_k, -( omega_n^2 ) * ones(size(t_k)), '-g' ); hold on;
xlabel('time (sec)'); ylabel( 'estimate of a' );
legend( [hx3], {'true value a'}, 'location', 'southwest' );
%saveas( gcf, '../../WriteUp/Graphics/Chapter4/example_4_7_1_plot_x3', 'epsc' ); 

% Use the extended Kalman filter on this system:
%
n_A = 3; % the extended state dimension
x_0 = [ 0; 0; a0_guess ]; % the extended state initial conditions
P_0 = zeros(n_A,n_A); P_0(n_A,n_A) = 20; 
H_A = zeros(2,3); H_A(1,1) = 1; H_A(2,2) = 1; 
Q_A = q_C; 
R_A = R;

[xhatminus,pminus,xhatplus,pplus] = example_4_7_1_extended_kf(x_0, P_0, b, H_A, Q_A, R_A, z_k, t_k);

% Next plot the a posteriori state estimates: 
%
figure(f1);
hxhat1 = plot( t_k, xhatplus(1,:), '-r' ); 
legend( [hx1,hx1m,hxhat1], {'true angle', 'angle measurement','estimate'}, 'location', 'southwest' );
if( savePlots )
  saveas( gcf, '../../WriteUp/Graphics/Chapter4/example_4_7_1_plot_x1', 'epsc' ); 
end

figure(f2);
hxhat2 = plot( t_k, xhatplus(2,:), '-r' ); 
legend( [hx2,hx2m,hxhat2], {'true angle', 'angle measurement','estimate'}, 'location', 'southwest' );
if( savePlots )
  saveas( gcf, '../../WriteUp/Graphics/Chapter4/example_4_7_1_plot_x2', 'epsc' ); 
end

s33 = sqrt( squeeze(pplus(3,3,:)) )';
figure(f3);
hxhat3 = plot( t_k, xhatplus(3,:), '-r' ); 
plot( t_k, xhatplus(3,:) - 1.97 * s33, '-.r' ); 
plot( t_k, xhatplus(3,:) + 1.97 * s33, '-.r' ); 
legend( [hx3,hxhat3], {'true value a', 'estimate of a'}, 'location', 'best' );
if( savePlots )
  saveas( gcf, '../../WriteUp/Graphics/Chapter4/example_4_7_1_plot_x3', 'epsc' ); 
end