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

close all; drawnow; 
clc; 
clear; 

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

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

F = [  0.999*cos(0.1*pi), 0.999*sin(0.1*pi); 
      -0.999*sin(0.1*pi), 0.999*cos(0.1*pi) ]; 
      
H = [ 0, 0.5 ];

C_w  = [1, 0; 0, 0]; % process noise covariance 
C_v  = 1; % measurement noise covariance 

C_x0 = [ 0.01, 0; 0, 0.01 ]; % initial state uncertainty 
x_0  = [ 0; 0 ]; % initial state mean 

M = length(x_0); % the state dimension 
N = size(H,1); % the measurement vector dimension  

Niters = 1000; 


% generate a trajectory with the above system and measurement models: 
% x is the TRUE state (t=0)
% y is the measurements 
%
[x,y] = sample_lds( F, H, C_w, C_v, x_0, Niters ); 

[xhat, C, CC, loglik, ztilde, xpred, Cpred, S, Sinv] = kalman_filter(y, F, H, C_w, C_v, x_0, C_x0 );


% Lets get to stready state by dropping the first set of measurments: 
%
Niters = 300;
x = x(:,end-Niters+1:end);
y = y(end-Niters+1:end);
xhat = xhat(:,end-Niters+1:end);
C = C(:,:,end-Niters+1:end);
CC = CC(:,:,end-Niters+1:end);
ztilde = ztilde(:,end-Niters+1:end);
xpred = xpred(:,end-Niters+1:end);
Cpred = Cpred(:,:,end-Niters+1:end);
S = S(:,:,end-Niters+1:end);
Sinv = Sinv(:,:,end-Niters+1:end);

figure; 
subplot(3,3,7);
plot( y, '-' ); 
axis( [-Inf,+Inf,-10,+10] ); title('measurements'); 


subplot(3,3,1);
plot( x(1,:), '-' ); hold on; plot( xhat(1,:), ':' ); 
axis( [-Inf,+Inf,-34,+34] ); 
title('first state')


subplot(3,3,4); plot( x(2,:), '-' ); hold on; plot( xhat(2,:), ':' );
axis( [-Inf,+Inf,-34,+34] ); 
title('second state');


c_norm95 = norminv( 1 - 0.05/2 ); 


e1 = x(1,:)-xhat(1,:);
me1 = mean(e1);
se1 = std(e1);
subplot(3,3,2); plot( e1, '-' ); title('error in first state');
hold on; 
plot(  c_norm95*sqrt(squeeze(C(1,1,:))), '-r' ); 
plot( -c_norm95*sqrt(squeeze(C(1,1,:))), '-r' ); 
axis( [-Inf,+Inf,-5,+5] ); 


e2 = x(2,:)-xhat(2,:);
me2 = mean(e2);
se2 = std(e2);
subplot(3,3,5); plot( e2, '-' ); title('error in second state');
hold on; 
plot(  c_norm95*sqrt(squeeze(C(2,2,:))), '-r' ); 
plot( -c_norm95*sqrt(squeeze(C(2,2,:))), '-r' ); 
axis( [-Inf,+Inf,-5,+5] ); 

% Plot the innovations: 
%
subplot(3,3,8);
plot( ztilde, '-' ); 
axis( [-Inf,+Inf,-5,+5] ); title('innovations'); 


% Create the NESS (normalized estimation error squared): 
%                 = e(i|i)^T C(i|i)^{-1} e(i|i) 
% 
ev   = x - xhat; % the error in the state at each timestep 
NESS = zeros(1,Niters);
for ii = 1:Niters, 
  NESS(ii) = ev(:,ii)' * ( C(:,:,ii) \ ev(:,ii) );
end

subplot(3,3,3);
plot( NESS, '-' ); 
c_chi95 = chi2inv( 1-0.05, M ); 
hold on; plot( c_chi95*ones(1,Niters), '-r' ); 
axis( [-Inf,+Inf,0,10] ); 
title('nees');

inds = NESS>=c_chi95;
fprintf('percentage of NESS points greater than c_95=%10.6f\n', sum(inds)/length(inds) );

% Create the NIS (normalized innovation squared): 
%                 = ztilde(i|i)^T S(i)^{-1} ztilde(i|i) 
% 
NIS = zeros(1,Niters);
for ii = 1:Niters, 
  NIS(ii) = ztilde(:,ii)' * ( Sinv(:,:,ii) * ztilde(:,ii) );
end

subplot(3,3,6);
plot( NIS, '-' ); 
c_chi95 = chi2inv( 1-0.05, N ); 
hold on; plot( c_chi95*ones(1,Niters), '-r' ); 
axis( [-Inf,+Inf,0,8] ); 
title('nis');

inds = NIS>=c_chi95;
fprintf('percentage of NIS points greater than c_95=%10.6f\n', sum(inds)/length(inds) );


% Plot the Periodogram: 
%
if( size(ztilde,1)~=1 )
  error('only one measurment expected');
end
P_1k = (abs(fft(ztilde)).^2) / Niters; % the periodogram 
sigma12 = var(ztilde);

subplot(3,3,9);
plot( 2*P_1k/sigma12, '-' ); 

c_chi95 = chi2inv( 1-0.05, 2 );
hold on; plot( c_chi95*ones(1,Niters), '-r' ); title('periodogram');
axis( [-Inf,+Inf,0,10] ); 

inds = (2*P_1k/sigma12) >= c_chi95;
fprintf('percentage of P_1k points greater than c_95=%10.6f\n', sum(inds)/length(inds) );

saveas( gcf, '../../WriteUp/Graphics/Chapter8/dup_example_8_15', 'epsc' );