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

omega_n = 6.28; 
zeta = 0.3; 
a = -omega_n^2; 
b = -2*zeta*omega_n; 

dt = 0.1; 

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

% Use the previous code to compute the discretized noise covariance matrix Q(\Delta t):
q_C = 1;
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 ... for the low-signal-to noise case comment this 

T = 2;
N = T/dt;
tms = 0:dt:T;

n = 2; % the state dimension

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

% Case 1: H_1 = I (plotted as blue)... the truth is plotted as green and measurements are plotted in black.
% Case 2: H_2 = ; (plotted as cyan)
% Case 3: H_3 = ; (plotted as magenta)
%
H_1 = eye(2); R_1 = [ 0.1, 0; 0, 0.1 ]; % the measurement noise covariance
H_2 = [1 0];  R_2 = 0.1;
H_3 = [0 1];  R_3 = 0.1;

% generate a trajectory with the above system and measurement models: 
% x is the TRUE state (t=0)
% y is the measurements 
%
randn('seed',0); rand('seed',0);
[x1,y1] = sample_lds( Phi, H_1, Q , R_1, zeros(n,1), N+1 ); randn('seed',0); rand('seed',0);
[x2,y2] = sample_lds( Phi, H_2, Q , R_2, zeros(n,1), N+1 ); randn('seed',0); rand('seed',0);
[x3,y3] = sample_lds( Phi, H_3, Q , R_3, zeros(n,1), N+1 ); 

[xhat1, V1, VV, loglik, innovations, xpred, Vpred] = kalman_filter(y1, Phi, H_1, Q, R_1, zeros(n,1), eye(n,n) ); 
[xhat2, V2, VV, loglik, innovations, xpred, Vpred] = kalman_filter(y2, Phi, H_2, Q, R_2, zeros(n,1), eye(n,n) ); 
[xhat3, V3, VV, loglik, innovations, xpred, Vpred] = kalman_filter(y3, Phi, H_3, Q, R_3, zeros(n,1), eye(n,n) ); 

% First plot: 
%
figure();
hx = plot( tms, x1(1,:), '-g' ); hold on;
hxh1 = plot( tms, xhat1(1,:), '-b' );
angle_measurement = y1(1,:);
hxm = plot( tms, angle_measurement, 'ko' );
xlabel('time (sec)'); ylabel( 'angle (deg)' );
legend( [hx,hxh1,hxm], {'true angle','angle estimate: H_1', 'angle measurement'}, 'location', 'southwest' );

%saveas( gcf, '../../WriteUp/Graphics/Chapter4/example_4_3_1_plot_1', 'epsc' ); 

% Second plot:
%
figure();
hx = plot( tms, x1(1,:), '-g' ); hold on;
hxh2 = plot( tms, xhat2(1,:), '-c' );
hxh3 = plot( tms, xhat3(1,:), '-m' );
xlabel('time (sec)'); ylabel( 'angle (deg)' );
legend( [hx,hxh2,hxh3], {'true angle','angle estimate: H_2', 'angle estimate: H_3'}, 'location', 'southwest' );

%saveas( gcf, '../../WriteUp/Graphics/Chapter4/example_4_3_1_plot_2', 'epsc' ); 

% Third plot:
%
figure();
hx = plot( tms, x1(2,:), '-g' ); hold on;
hxh2 = plot( tms, xhat2(2,:), '-c' );
xlabel('time (sec)'); ylabel( 'angle (deg)' );
legend( [hx,hxh2], {'true angle rate','angle rate estimate: H_2'}, 'location', 'northeast' );

%saveas( gcf, '../../WriteUp/Graphics/Chapter4/example_4_3_1_plot_3', 'epsc' );

% Print the final covariances values:
%
P1 = V1(:,:,end);
P2 = V2(:,:,end);
P3 = V3(:,:,end);

fprintf('H1:  '); fprintf('%10.6f ',[ P1(1,1), P1(1,2), P1(2,2) ]); fprintf('\n');
fprintf('H2:  '); fprintf('%10.6f ',[ P2(1,1), P2(1,2), P2(2,2) ]); fprintf('\n');
fprintf('H3:  '); fprintf('%10.6f ',[ P3(1,1), P3(1,2), P3(2,2) ]); fprintf('\n');