%   
% 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',10); rand('seed',10);

% Lets consider *4* values for omega_n: 
zeta = 0.1;
dt = 0.1; 
T = 30;
q_C = 1000;

% The discrete systems for the various parallel filters we will run: 
omega_1 = sqrt(3.6);
[t_k,N,Phi_1,Q_1] = weathervane_system( omega_1, zeta, q_C, dt, T );

omega_truth = 2.0; 
[t_k,N,Phi_2,Q_2] = weathervane_system( omega_truth, zeta, q_C, dt, T ); 
Phi_truth = Phi_2; Q_truth = Q_2; 

omega_3 = sqrt(4.4);
[t_k,N,Phi_3,Q_3] = weathervane_system( omega_3, zeta, q_C, dt, T );

omega_4 = sqrt(4.8);
[t_k,N,Phi_4,Q_4] = weathervane_system( omega_4, zeta, q_C, dt, T );

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 = 2*[ 10, 0; 0, 10 ]; % the measurement noise covariance

% generate a trajectory with the above system and measurement models: 
% x is the TRUE state 
% z_k are the measurements 
%
[x_truth,z_k] = sample_lds( Phi_truth, H, Q_truth, 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_2_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_2_plot_x2', 'epsc' ); 

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

% Run the parallel Kalman filters:
%
[xplus_1, Pplus_1, VV, loglik, innovations_1, xminus_1, Vpred, S_1] = kalman_filter(z_k, Phi_1, H, Q_1, R, zeros(n,1), eye(n,n) ); 
[xplus_2, Pplus_2, VV, loglik, innovations_2, xminus_2, Vpred, S_2] = kalman_filter(z_k, Phi_2, H, Q_2, R, zeros(n,1), eye(n,n) ); 
[xplus_3, Pplus_3, VV, loglik, innovations_3, xminus_3, Vpred, S_3] = kalman_filter(z_k, Phi_3, H, Q_3, R, zeros(n,1), eye(n,n) ); 
[xplus_4, Pplus_4, VV, loglik, innovations_4, xminus_4, Vpred, S_4] = kalman_filter(z_k, Phi_4, H, Q_4, R, zeros(n,1), eye(n,n) ); 

% Evaluate the likelihood of each parameter setting: pr[z_k|\hat{x}[p_i,(-)]]
%
like_1 = zeros(1,N);
like_2 = zeros(1,N);
like_3 = zeros(1,N);
like_4 = zeros(1,N);
for k=1:N+1
  like_1(k) = mvnpdf( innovations_1(:,k), zeros(n,1), S_1(:,:,k) );
  like_2(k) = mvnpdf( innovations_2(:,k), zeros(n,1), S_2(:,:,k) );
  like_3(k) = mvnpdf( innovations_3(:,k), zeros(n,1), S_3(:,:,k) );
  like_4(k) = mvnpdf( innovations_4(:,k), zeros(n,1), S_4(:,:,k) );
end
all_likes = [ like_1; like_2; like_3; like_4 ];

I = 4; 

% the prior probability of each hypothesis:
Pr0 = ones(I,1)/I;

% recursivly compute the posteriori probabilty of each parameter:
Pr = zeros(I,N+1);
for k=1:(N+1)
  if( k==1 )
    prior = Pr0;
  else
    prior = Pr(:,k-1);
  end
  
  Pr(:,k) = all_likes(:,k) .* prior / sum(all_likes(:,k) .* prior);
  
end

f4 = figure(); hold on; colors='rgbkcm';
all_hs = [];
for ii=1:I
  all_hs = [ all_hs, plot( Pr(ii,:), ['-',colors(ii)] ) ];
end
legend( all_hs, {'Phi\_1','Phi\_truth','Phi\_3','Phi\_4'}, 'location', 'best' );
%saveas( gcf, '../../WriteUp/Graphics/Chapter4/example_4_7_2_plot_probs', 'epsc' ); 

% Next plot the a posteriori state estimates (best guess after averaging over all parameters): 
%
mean_xhatplus = zeros(n,N+1);
for k=1:(N+1)
  mean_xhatplus(:,k) = Pr(1,k) * xplus_1(:,k) + Pr(2,k) * xplus_2(:,k) + Pr(3,k) * xplus_3(:,k) + Pr(4,k) * xplus_4(:,k);
end

figure(f1);
hxhat1 = plot( t_k, mean_xhatplus(1,:), '-r' ); 
legend( [hx1,hx1m,hxhat1], {'true angle', 'angle measurement','average estimate'}, 'location', 'southwest' );
%saveas( gcf, '../../WriteUp/Graphics/Chapter4/example_4_7_2_plot_x1', 'epsc' ); 

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

% Next plot the estimate value of omega_n:
%
mean_omega = zeros(1,N+1);
for k=1:(N+1)
  mean_omega(k) = Pr(1,k) * omega_1 + Pr(2,k) * omega_truth + Pr(3,k) * omega_3 + Pr(4,k) * omega_4;
end

figure(f3);
hxhat3 = plot( t_k, mean_omega, '-r' ); 
legend( [hx3,hxhat3], {'true value omega_n', 'mean estimate of omega_n'}, 'location', 'best' );
%saveas( gcf, '../../WriteUp/Graphics/Chapter4/example_4_7_2_plot_x3', 'epsc' );