
clear
close all

% Program for the earthquake hypocentre location problem
% Problem taken from Stein + Wysession, Introduction to seismology, Chapter
% 7.2
% Version with added Gaussian Noise


nr = 10;     % number of receivers
nd = 3;      % number of space dimensions

% Initialize Station coordinates [km]
xr=[ 35 9 0; 
    -44 10 0;
    -11 -25 0;
     23 -39 0;
     42 -27 0;
    -12 50 0;
    -45 16 0;
      5 -19 0;
     -1 -11 0;
     20 11 0];

% Velocity model [km/s]
v=5;
 
% true model vector m (x, y, z, origin time)
m0=[0 0 10 0]';

% Calculate true data using hypo_forward.m
d0=hypo_forward(m0,xr,v)';

%%%%% NOISE %%%%%%%%%%%%%%%%%%%%%%%%%%%
% We now add some noise to the data and investigate the consequences

% We define a Gaussian with std .1 se
s=0.1;  % in s this is for the hypocentre location problem
% Define interval and discrete space
t=-5:.01:5;
% Calculate pdf
gauss=1./(sqrt(2*pi)*s)*exp(-.5/s^2*t.^2);

% Add error to data and check
disp(' Error analysis : ')
for i=1:length(d0),
    d0e(i)=d0(i)+draw_from_pdf(t,gauss);
    disp(sprintf(' Original: %g5 Noisy: %g5 Difference: %g5 ',d0(i),d0e(i),d0(i)-d0e(i)))
end
d0e=d0e';  % transpose to column vector

% End of initialization of  noisy data

% Definite initial model for inverse problem
m=[3 4 20 2]';

maxni = 5;  % maximum number of iterations

% Iterative inversion

for ii=1:maxni,
    
    % calculate data difference for current model m
    dd = d0e - hypo_forward(m,xr,v)'; 
    
    % Calculate misfit energy
    err(ii) = (dd'*dd);
    disp(' -----------------------------------------------')
    disp(sprintf(' Misfit energy at iteration %i : %g ',ii,err(ii)));
    disp(' True solution vs. estimate : ')
    disp(sprintf(' x_true = %g km, x_est = %g km',m0(1),m(1))) 
    disp(sprintf(' y_true = %g km, y_est = %g km',m0(2),m(2))) 
    disp(sprintf(' z_true = %g km, z_est = %g km',m0(3),m(3))) 
    disp(sprintf(' t_true = %g  s, t_est = %g  s',m0(4),m(4))) 
    
    % initialize the system matrix G with the partial derivatives
    G = hypo_gradient(m,xr,v);
    
    % Solve the inverse problem for dm
    dm = inv(G'*G)*G'*dd;
    % update model
    m=m+dm;      

    
    
end    

plot(log10(err))
ylabel(' Log Misfit energy (s^2) ')
xlabel(' Iteration number ')
title(' Convergence behaviour - noisy case ')


% Analyse covariances

% Calculate  model covariance matrix sM = s^2 * (G'*G)-1;
sM = s*s * inv(G'*G);

% Error analysis
disp(' Error Analysis from Model covariance matrix ')
disp(sprintf(' Final x: %g5 km, Error (std): %g5 km ',m(1),sqrt(sM(1,1))));
disp(sprintf(' Final y: %g5 km, Error (std): %g5 km ',m(2),sqrt(sM(2,2))));
disp(sprintf(' Final z: %g5 km, Error (std): %g5 km ',m(3),sqrt(sM(3,3))));
disp(sprintf(' Final t0: %g5 km, Error (std): %g5 km ',m(4),sqrt(sM(4,4))));

% Visualization of errors using an error ellipsoid

% Calculate eigenvalues of sM submatrix
[V,D] = eig(sM(1:3,1:3));

figure
[x, y, z] = ellipsoid(m(1),m(2),m(3),sqrt(D(1,1)),sqrt(D(2,2)),sqrt(D(3,3)),30);
surfl(x, y, z)
colormap copper
axis equal
title(' Error ellipe, Standard deviations ')
xlabel(' x (km )')
ylabel(' y (km) ')
zlabel(' z (km) ')

figure
[x, y, z] = ellipsoid(m(1),m(2),m(3),sqrt(sM(1,1)),sqrt(sM(2,2)),sqrt(sM(3,3)),30);
surfl(x, y, z)
colormap copper
axis equal
title(' Error ellipe, Eigendirections ')
xlabel(' x (km )')
ylabel(' y (km) ')
zlabel(' z (km) ')
