function P2()
% Checks the function Elevations.

close all
randn('seed',0)
rand('seed',0)

% Test Problem parameters...

% Measurement error
NoiseFactor = .1;
% Number of measurements taken at each point
m = 7;
% Generate n = N^2 uniformly spaces grid points in a square with vertices
% (5,5),(-5,5),(-5,-5), and(5,-5)
N = 30; n = N^2;

% Index the measurement points, compute the true elevations,
% and display a contour plot...

xVals = linspace(-5,5,N);
yVals = linspace(-5,5,N);
k = 0;
Elevations_true = zeros(N,N);
for i=1:N
for j=1:N
k = k+1;
x(k) = xVals(j);
y(k) = yVals(i);
Elevations_true(i,j) = f(xVals(j),yVals(i));
end
end
contour(xVals,yVals,Elevations_true,20);
title('Contour Plot Based on True Elevations')
shg

% Set up the sparse, noisy measurement matrix E...

E = sparse(n,n);
for i=1:n
% The surveyor visits (x(i),y(i)) and estimates the elevation
% differentials associated with m points...
%jdx = Nearest(i,x,y,m);
jdx = Random(n,i,m); % m random points
jdx = Nearest(i,x,y,m); % m nearest points
for k=1:m
j = jdx(k);
E(i,j) = f(x(j),y(j)) - f(x(i),y(i)) + NoiseFactor*randn(1,1);
end
end

% Compute the vector of true elevations...

True_h = zeros(n,1);
for i=1:n
True_h(i) = f(x(i),y(i));
end

% Estimate the elevations..
My_h = ElevationsCVL(x,y,E,True_h(1));
Your_h = Elevations(x,y,E,True_h(1));

% Checking your answer...
MeMinusYou = max(abs(My_h - Your_h));
TrueMinusYou = max(abs(True_h - Your_h));
% Contour plot based on your estimated elevations..
k = 0;
E_You = zeros(N,N);
for i=1:N
for j=1:N
k = k+1;
E_You(i,j) = Your_h(k);
end
end
figure
contour(xVals,yVals,E_You,20);
title('Contour plot based on your estimated elevations.')
xlabel(sprintf('MeMinusYou = %10.3e TrueMinusYou = %10.3e Noise = %6.4f',...
MeMinusYou,TrueMinusYou,NoiseFactor))
shg

function h = ElevationsCVL(x,y,E,h1)
% Obtain the index and measurement information from E
n = length(x);
[ie,je,Evals] = find(E);
% Compute the number of measurements...
m = length(Evals);
A = sparse(m,n);
b = zeros(m,1);
% Set up A and b...
for k=1:m
i = ie(k);
j = je(k);
% The kth equation is concerned with the kth measurment and it is
% and it is given by h_{j}-h_{i} \approx E(i,j)
% Get the weight factor...
dij = sqrt((x(i)-x(j))^2 + (y(i)-y(j))^2);
q = 1/dij;
A(k,j) = q;
A(k,i) = -q;
b(k) = Evals(k)*q;
end
% There are clever ways of vecxtorizing the above, but this wasn't required
% for full credit.
b = b - A(:,1)*h1;
htilde = A(:,2:n)\b;
h = [h1;htilde];

function z = f(x,y)
% An elevation function
% Sum of three gaussians...
a1 = 2.5; b1 = 2; m1 = 10; tau1 = .2;
z1 = m1*exp(-tau1*((x - a1)^2 + (y-b1)^2));
a2 = -3; b2 = 3; m2 = 8; tau2 = .4;
z2 = m2*exp(-tau2*((x - a2)^2 + (y-b2)^2));
a3 = .5; b3 = -3; m3 = 10; tau3 = .1;
z3 = m3*exp(-tau3*((x - a3)^2 + (y-b3)^2));
z = z1+z2+z3;

function idx = Nearest(i,x,y,m)
% x and y are column vectors with dimension >=m+1
% idx is a length-m vector with the property that the m closest points
% to (x(i),y(i)) are (x(idx(k)),y(idx(k)), k=1:m.
xi = x(i);
yi = y(i);
d = (x-xi).^2 + (y-yi).^2;
[s,idx] = sort(d);
idx = idx(2:m+1);

function idx = Random(n,i,m)
% Returns m random integers between 1 and n not including i.
[s,idx] = sort(randn(n,1));
idx = idx(find(idx~=i));
idx = idx(1:m);