% Jyvaskylä Summer School 2014
% Aku Seppanen
% University of Eastern Finland, Kuopio
% Department of Applied Physics

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Example 3.1: line-fitting %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

a_true = 1;
b_true = 2;
var_n = .05; % variance of noise n

x = [0 1 2 3 4]';
n = sqrt(var_n)*randn(length(x),1);
g = a_true*x + b_true + n; 

figure(1), clf
plot(x,g,'k*')

K = [x ones(size(x))];
f_LS = inv(K'*K)*K'*g; % LS-solution (could be written also as f_LS = K\g;
a_LS = f_LS(1)
b_LS = f_LS(2)

g_LS = a_LS*x + b_LS; 
figure(1), hold on,
plot(x,g_LS,'-')
title(['a_{true}=',num2str(a_true),', b_{true}=',num2str(b_true),...
    ',     a_{LS}=',num2str(a_LS),', b_{LS}=',num2str(b_LS)]);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Example 3.2: Fitting of a 2nd order polynomial %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

a_true = 2;
b_true = -5;
c_true = 5
var_n = .05; % variance of noise n

x = [0 1 2 3 4]';
n = sqrt(var_n)*randn(length(x),1);
g = a_true*x.^2 + b_true*x + c_true + n;

figure(1), clf
plot(x,g,'k*')

K = [x.^2 x ones(size(x))];
f_LS = inv(K'*K)*K'*g; % LS-solution (could be written also as f_LS = K\g;
a_LS = f_LS(1)
b_LS = f_LS(2)
c_LS = f_LS(3)

x_denser = linspace(min(x),max(x),100)';
g_LS = a_LS*x_denser.^2 + b_LS*x_denser + c_LS; 

figure(1), hold on,
plot(x_denser,g_LS,'-')
title(['a_{true}=',num2str(a_true),', b_{true}=',num2str(b_true),', c_{true}=',num2str(c_true),...
    ',     a_{LS}=',num2str(a_LS),', b_{LS}=',num2str(b_LS), ', c_{LS}=',num2str(c_LS)]);



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Example 3.3: line-fitting; only a single observation %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

a_true = 1;
b_true = 2;
var_n = .0005; % variance of noise n

x = [0]';
n = sqrt(var_n)*randn(length(x),1);
g = a_true*x + b_true + n; 

K = [x ones(size(x))];
f_LS = inv(K'*K)*K'*g; % Matlab is not happy about this...
rank(K) % rank of K is 1; underdetermined system of equations

f_MN = pinv(K)*g; %minimum norm solution
a_MN = f_MN(1)
b_MN = f_MN(2)

x_denser = [-1,0,1]';
g_MN = a_MN*x_denser + b_MN; 

figure(1), clf
plot(x,g,'k*'), hold on,
plot(x_denser,a_true*x_denser+b_true,'g-')
plot(x_denser,g_MN,'b--')
hold on
title(['a_{true}=',num2str(a_true),', b_{true}=',num2str(b_true),...
    ',     a_{MN}=',num2str(a_MN),', b_{MN}=',num2str(b_MN)]);
legend('Observation','y_{true}','y_{MN}')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Example 3.4: Gauss-Markov estimate; line-fitting %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

a_true = 1;
b_true = 2;
var_n = [.05 .05 100 .05 .05]'; % third noise component has a very large variance

x = [0 1 2 3 4]';
n = sqrt(var_n).*randn(length(x),1);

g_true = a_true*x + b_true 
g = g_true + n; 

figure(1), clf
plot(x,g,'k*')

K = [x ones(size(x))];

% LS estimate
f_LS = inv(K'*K)*K'*g; % LS-solution (could be written also as f_LS = K\g;
a_LS = f_LS(1)
b_LS = f_LS(2)

% Gauss-Markov estimate
Gamma_n = diag(var_n);
W = inv(Gamma_n);
f_GM = inv(K'*W*K)*K'*W*g; % LS-solution (could be written also as f_LS = K\g;
a_GM = f_GM(1)
b_GM = f_GM(2)

g_LS = a_LS*x + b_LS; 
g_GM = a_GM*x + b_GM; 

figure(1), hold on,
plot(x,g_true,'g-')
plot(x,g_LS,'b--')
plot(x,g_GM,'r--')
title(['a_{true}=',num2str(a_true),', b_{true}=',num2str(b_true),...
    ',     a_{LS}=',num2str(a_LS),', b_{LS}=',num2str(b_LS)...
    ',     a_{GM}=',num2str(a_GM),', b_{GM}=',num2str(b_GM)]);
legend('Observations','True line','LS-solution','Gauss-Markov solution')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Example 3.5: Gauss-Markov estimate; line-fitting; correlated noise  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

a_true = 1;
b_true = 2.09;

Gamma_n = [.0001 0 0 ;...
           0 10.0001 10;...
           0 10 10.0001]; 
% Observations 2 & 3 have large variances and are heavily correlated

x = [0 1 2]';
n = chol(inv(Gamma_n))\randn(length(x),1);

g_nonoise = a_true*x + b_true 
g = g_nonoise + n; 

figure(1), clf
plot(x,g,'k*'), hold on
plot(x,g_nonoise,'g-')

K = [x ones(size(x))];

% LS estimate
f_LS = inv(K'*K)*K'*g; % LS-solution (could be written also as f_LS = K\g;
a_LS = f_LS(1)
b_LS = f_LS(2)

% Gauss-Markov estimate
W = inv(Gamma_n);
f_GM = inv(K'*W*K)*K'*W*g; % LS-solution (could be written also as f_LS = K\g;
a_GM = f_GM(1)
b_GM = f_GM(2)

g_LS = a_LS*x + b_LS; 
g_GM = a_GM*x + b_GM; 

figure(1), hold on,
plot(x,g_LS,'b--')
plot(x,g_GM,'r--')
title(['a_{true}=',num2str(a_true),', b_{true}=',num2str(b_true),...
    ',     a_{LS}=',num2str(a_LS),', b_{LS}=',num2str(b_LS)...
    ',     a_{GM}=',num2str(a_GM),', b_{GM}=',num2str(b_GM)]);
legend('Observations','True g','LS-solution','Gauss-Markov solution')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Example 3.6: Non-linear LS  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear

a_true = 1;
b_true = 5;
x = linspace(0,1,20)';
var_n = .002;
Gamma_n = var_n*eye(length(x));
W = inv(Gamma_n);
n = sqrt(var_n)*randn(length(x),1);
g = a_true*exp(-b_true*x) + n;

x_dense = linspace(0,1,200)'; % dense grid for visualization
g_dense = a_true*exp(-b_true*x_dense);

figure(1), clf, plot(x_dense,g_dense,'g',x,g,'k+')

a_init = 10; % initial guess for a
b_init = .1; % initial guess for b
steplength = .5;
niter = 50; % max number of iterations
f_LS = zeros(2,niter);
f_LS(:,1) = [a_init; b_init];

% Gauss-Newton iteration
for k = 1:niter-1
    
   f_k = f_LS(:,k);
   h_f_k = f_k(1)*exp(-f_k(2)*x);
   J_k = [exp(-f_k(2)*x) -f_k(1)*x.*exp(-f_k(2)*x)];
   f_LS(:,k+1) = f_k + steplength*inv(J_k'*W*J_k)*(J_k'*W*(g-h_f_k));
   g_LS_dense = f_LS(1,k+1)*exp(-f_LS(2,k+1)*x_dense);
   figure(2), clf, plot(x_dense,g_dense,'g',x,g,'k+')
   hold on
   plot(x_dense,g_LS_dense,'r--')
   title(['Iteration k = ',num2str(k),', a = ',num2str(f_LS(1,k+1)),', b = ',num2str(f_LS(2,k+1))])
   legend('True g','Observations','LS-solution')
   pause(.05)
   
end





