Patrick Gloster
An explanation of the functions I have written in Matlab
Distance between two points
The function F(a,b) takes two vectors representing coordinates and calculates the
difference between them. This is done by finding the vector ab=(b-a) and multiplying
it by its transpose. The code is given below:
function out = F(X1,X2)
out=sqrt((X1-X2)'*(X1-X2));
Circle functions
Circles can be plotted by inputting the coordinates of the centre of the circle and the
radius. This is done by plotting many points in polar coordinates and incrementing the
angle by a small fraction each time. This is best explained when viewed with the
code:
function blackcircle(x,y,r)
N=256;
t=(0:N)*2*pi/N;
plot( r*cos(t)+x, r*sin(t)+y,'-k');
axis('square')
N is just the number of increments; by increasing N the circle will become smoother
and a more accurate circle. We define t as ranging from 0 to 2π in increments of 2π/N.
We then plot ( x+rcos(t) , y +rsin(t) ) where (x,y) is the centre of the circle. This plots
a circle about (x,y) with radius r. The colour of the circle can be changed by altering
the letter which is in purple in the 4th line of code. For example to draw a blue circle
one would replace ‘-k’ with ‘-b’. In fact I defined functions for various colours, each
represented by the appropriate function name: blackcircle(x,y,r) , bluecircle(x,y,r) ,
redcircle (x,y,r) , greencircle(x,y,r).
The locate function
The locate function is the name I gave to a function which I used to calculate the
position of a point given 3 observer coordinates and 2 measurements from their
coordinates to the point we are finding (by which I mean we know the position of 3
points and for 2 of these points, we have a distance to the point we are trying to
locate).
First my function converts the inputs, which are given as separate x and y coordinates
4
e.g. (4,6), into vectors i.e. (4,6) becomes   . This is so that the function F can be
6
used on them.
The function then focuses on two observer points and the point we are seeking.
Patrick Gloster
For example, in this diagram (x1,y1) and (x2,y2) are the observer points, and (x,y) is
the point we are trying to locate. Our measurements are L1 and L2, and we can
determine
(x,y)
L2
L1
L1sin(α+β)
(x2,y2)
α
L3
(x1,y1)
x-direction
L1cos(α+β)
β
L3 using the function F acting on X1=(x1,y1) and X2=(x2,y2) i.e. F(X1,X2) =L3. We
can then use the cosine rule to calculate the angle α. My function then selects the
observer coordinate which is furthest to the left (i.e. the observer coordinate which
has the smallest value of x) and calculates β, where β is the angle between the positive
x-direction and L3. We can see that to get from (x1,y1) to (x,y) we add L1cos(α+β) in
the x-direction and L1sin(α+β) in the y direction; thus (x,y) is given by
(x1+ L1cos(α+β), y1+ L1sin(α+β)).
Of course, this only gives us one answer, when we know there are two possible
solutions. The other coordinate is the mirror image of (x,y) in L3. This is given by
(x1+ L1cos(α-β), y1- L1sin(α-β)).
Finally, the code selects which of these is closest to the third point (x3,y3) by
comparing the distances and choosing the one with the smallest value.
This is the code:
function out=locate(x1,y1,L1,x2,y2,L2,x3,y3)
X1=[x1;y1];
X2=[x2;y2];
X3=[x3;y3];
L3=F(X1,X2);
alpha=acos((L1^2+L3^2-L2^2)/(2.*L1.*L3));
if x1<=x2
beta=atan((y2-y1)/(x2-x1));
x01=x1+L1.*cos(alpha+beta);
y01=y1+L1.*sin(alpha+beta);
x02=x1+L1.*cos(alpha-beta);
Patrick Gloster
y02=y1-L1.*sin(alpha-beta);
X01=[x01;y01];
X02=[x02;y02];
else
beta=atan((y2-y1)/(x1-x2));
x01=x2+L1.*cos(alpha+beta);
y01=y1+L1.*sin(alpha+beta);
x02=x2+L1.*cos(alpha-beta);
y02=y1-L1.*sin(alpha-beta);
X01=[x01;y01];
X02=[x02;y02];
end
if F(X01,X3)<F(X02,X3)
out=[x01;y01]
else out=[x02;y02]
end
The simulation
My simulation is run by entering simulation(N) where N is the number of samples we
want to generate. The user chooses 4 observer coordinates and is then given the
chance to jump to a particular simulation and proceed from that point e.g. if you are
looking at 100 samples and want to look at the 75th onwards you can jump to that
sample right at the start. The program then asks the user to input percentage errors
for each of the 4 measurements.
The program then randomly generates a ‘true’ position; that is, the position we want
to find an approximation to. It then calculates the true distances from each observer
point to this ‘true’ position. Pseudorandom measurements are generated using a
random number generator normally distributed, the percentage error for each
measurement, and the true distances.
Having done this the program uses the locate function to calculate an approximate
position of the point, using three of the measurements. Using this approximate
position an initial value, it then uses the Gaussian method of least squares to compute
a new position, performing many iterations until the vector is changing by a distance
of less than 0.0000000001. This could be changed in the code if necessary or the code
could be adapted so the user can choose to change if while the program is running.
There is an iterations cap in the program to prevent the number of iterations exceeding
1 million and risking crashing the program.
For each randomly generated position (each sample) the user is given the option to
skip to the final answer or to proceed step by step as each new, more accurate position
is calculated. These are plotted on a graph which has 4 circles representing the
measurements (each circle is centred on an observer coordinate), a blue cross for
every intermediate value calculated for the position we are looking for and a magenta
cross for the final answer.
This is the code for the simulation:
function simulation(N)
a=0.0000000001; %This details the value which dx must
%be smaller than for the iteration to stop
%This could be a user defined input if necessary
prompt={'x1','y1','x2','y2','x3','y3','x4','y4'};
name='Observer coordinates:'; %Choose the 4 observer coordinates
numlines=1;
Patrick Gloster
defaultanswer={'0','0','10','0','0','10','10','10'};
answer0=inputdlg(prompt,name,numlines,defaultanswer);
x1=str2num(answer0{1});
y1=str2num(answer0{2});
x2=str2num(answer0{3});
y2=str2num(answer0{4});
x3=str2num(answer0{5});
y3=str2num(answer0{6});
x4=str2num(answer0{7});
y4=str2num(answer0{8});
X1=[x1;y1]; %Puts the observer coordinates into vector notation
X2=[x2;y2];
X3=[x3;y3];
X4=[x4;y4];
rand('seed',0) ;
randn('seed',0);
prompt={'If you would like to jump to the nth simulation, enter the
number of the simulation you would like to jump to. Otherwise, leave
as 1'};
name='Jump to:'; %allows the user to jump to a particular simulation
numlines=1;
defaultanswer={'1'};
answer=inputdlg(prompt,name,numlines,defaultanswer);
simulation_number=str2num(answer{1});
prompt={'Error 1','Error 2', 'Error 3', 'Error 4'};
name='Measurement errors:'; %allows the user to input the error
associated with each measurement
numlines=1;
defaultanswer={'1','1','1','1'};
answer2=inputdlg(prompt,name,numlines,defaultanswer);
error1=str2num(answer2{1}).*0.01;
error2=str2num(answer2{2}).*0.01;
error3=str2num(answer2{3}).*0.01;
error4=str2num(answer2{4}).*0.01;
D=[error1^2;error2^2;error3^2;error4^2]; %These two lines create the
weight matrix P,
P=diag(D);
% a diagonal matrix whose diagonal
elements are the errors
k0=1;
while(k0<simulation_number)
rand(2,1);
randn;
randn;
randn;
randn;
k0=k0+1;
end
while (simulation_number<N+1)
X=10.*rand(2,1); %Generate a random 'true' position
D1=F(X,X1); %Work out the true distances from each of the observation
points
D2=F(X,X2);
D3=F(X,X3);
D4=F(X,X4);
number_of_iterations=0;
Patrick Gloster
L1=D1+D1.*randn.*error1; %Generate measurements by adding random
error to true distances
L2=D2+D2.*randn.*error2;
L3=D3+D3.*randn.*error3;
L4=D4+D4.*randn.*error4;
X0=locate(x1,y1,L1,x2,y2,L2,x3,y3); %Computes a rough position of the
point
greencircle(x1,y1,L1) %plots measurements and calculated position
hold on
redcircle(x2,y2,L2)
hold on
bluecircle(x3,y3,L3)
hold on
blackcircle(x4,y4,L4)
hold on
andy=1;
moddX=a+1; %Sets mod(dX) to an initial value greater than a
options.Resize='on';
options.WindowStyle='normal';
options.Interpreter='tex';
prompt={'Would you like to look at individual iterations? Type 1 for
yes, otherwise leave as 0'};
name='Iterations:'; %allows the user to jump to a particular
simulation
numlines=1;
defaultanswer={'0'};
examine=inputdlg(prompt,name,numlines,defaultanswer,options);
%examine=questdlg('Would you like to look at individual
iterations?','Input','yes','no','no');
%This gives the user the opportunity to look at each consecutive
calculated
%position
while (moddX>=a)
W=([F(X0,X1) F(X0,X2) F(X0,X3) F(X0,X4)]-[L1 L2 L3 L4])'; %Misclosure
vector
A=[((X0-X1)/F(X0,X1))';((X0-X2)/F(X0,X2))';((X0-X3)/F(X0,X3))';((X0X4)/F(X0,X4))']; %the Design matrix
dX=-1*inv(A'*P*A)*A'*P*W; %Computes perturbation to X0 using standard
result
moddX=sqrt(dX'*dX); %Size of perturbation
X0=X0+dX; %New value for position of point
number_of_iterations=number_of_iterations+1;%This counts how many
iterations are performed
if(str2num(examine{1})==1)&&(andy~=0)
plot(X0(1,1),X0(2,1),'x','MarkerSize',12,'MarkerEdgeColor','b')
hold on
options.Resize='on';
options.WindowStyle='normal';
options.Interpreter='tex';
prompt={'Type 1 to jump to the final answer. Leave as 0 to proceed to
next iteration'};
name='Proceed?:'; %allows the user to jump to a particular simulation
Patrick Gloster
numlines=1;
defaultanswer={'0'};
moveon=inputdlg(prompt,name,numlines,defaultanswer,options);
%moveon=questdlg('Proceed to next iteration?','Input','plot
next','Jump to final value','plot next');
%If the user has chosen to look at individual iterations they can
jump to
%the final value
if (str2num(moveon{1})==1)
andy=0;
end
if (number_of_iterations>1000000) %Limits iterations so the program
doesn't get stuck
moddX=a-1;
end
end
end %This process will be repeated until changes in X0 are smaller
than a
simulation_number
X
calculated=X0+dX
number_of_iterations
plot(calculated(1,1),calculated(2,1),'x','MarkerSize',12,'MarkerEdgeC
olor','m')
hold;
if simulation_number<N
options.Resize='on';
options.WindowStyle='normal';
options.Interpreter='tex';
prompt={'Type 1 to exit the simulation. Leave as 0 to proceed to the
next simulation'};
name='Proceed?:'; %allows the user to jump to a particular simulation
numlines=1;
defaultanswer={'0'};
progress=inputdlg(prompt,name,numlines,defaultanswer,options);
if(str2num(progress{1})==0) %Gives the user the option to continue to
the next simulation or exit
simulation_number=simulation_number+1;
else simulation_number=N+1;
end
else helpdlg('Simulation complete','Patrick says');
simulation_number=simulation_number+1;
end
end
The gauss2 function
The gauss2 function was written to perform the least squares approximation for the
position of our point, always starting with the same ‘true’ position, but different
Patrick Gloster
measurements (randomly generated with the errors) so that we could examine how the
calculated positions were distributed about the true position. In this case typing
gauss2(N) will calculate N different sets of random measurements and therefore N
different calculated positions. The code is fairly similar to that of the simulation, but
this time the random ‘true’ position is only generated once, and it is just the
pseudorandom measurements which are generated many times. Again the position is
calculated for each set of random measurements by iterating the least squares method.
Finally the distance between the calculated and observer coordinates is found and a
histogram is plotted of the difference between these distances and the true distances
for each measurement. Results show normal distributions centred about 0.
Note for the gauss2 function ‘a’ (the maximum size of the perturbation vector) was
made smaller to stop the program crashing due to length of time.
This is the code for gauss2:
function gauss2(N)
format long;
clear distanceoff
vectoroff1=0;
vectoroff2=0;
vectoroff3=0;
vectoroff4=0;
simulation_number=1;
a=0.0001; %This details the value which dx must
%be smaller than for the iteration to stop
%This could be a user defined input if necessary
prompt={'x1','y1','x2','y2','x3','y3','x4','y4'};
name='Observer coordinates:'; %Choose the 4 observer coordinates
numlines=1;
defaultanswer={'0','0','10','0','0','10','10','10'};
answer0=inputdlg(prompt,name,numlines,defaultanswer);
x1=str2num(answer0{1});
y1=str2num(answer0{2});
x2=str2num(answer0{3});
y2=str2num(answer0{4});
x3=str2num(answer0{5});
y3=str2num(answer0{6});
x4=str2num(answer0{7});
y4=str2num(answer0{8});
X1=[x1;y1]; %Puts the observer coordinates into vector notation
X2=[x2;y2];
X3=[x3;y3];
X4=[x4;y4];
rand('seed',0);
randn('seed',0);
prompt={'Error 1','Error 2', 'Error 3', 'Error 4'};
name='Measurement errors:'; %allows the user to input the error
associated with each measurement
numlines=1;
defaultanswer={'1','1','1','1'};
answer2=inputdlg(prompt,name,numlines,defaultanswer);
error1=str2num(answer2{1}).*0.01;
error2=str2num(answer2{2}).*0.01;
error3=str2num(answer2{3}).*0.01;
error4=str2num(answer2{4}).*0.01;
D=[error1^2;error2^2;error3^2;error4^2]; %These two lines create the
weight matrix P,
Patrick Gloster
P=diag(D);
% a diagonal matrix whose diagonal
elements are the percentage errors
X=10.*rand(2,1);%Generate a random 'true' position
D1=F(X,X1); %Work out the true distances from each of the observation
points
D2=F(X,X2);
D3=F(X,X3);
D4=F(X,X4);
number_of_iterations=0;
while (simulation_number<N+1)
L1=D1+D1.*randn.*error1; %Generate measurements by adding random
error to true distances
L2=D2+D2.*randn.*error2;
L3=D3+D3.*randn.*error3;
L4=D4+D4.*randn.*error4;
X0=locate(x1,y1,L1,x2,y2,L2,x3,y3); %Computes a rough position of the
point
moddX=a+1; %Sets mod(dX) to an initial value greater than a
while (moddX>=a)
W=([F(X0,X1) F(X0,X2) F(X0,X3) F(X0,X4)]-[L1 L2 L3 L4])'; %Misclosure
vector
A=[((X0-X1)/F(X0,X1))';((X0-X2)/F(X0,X2))';((X0-X3)/F(X0,X3))';((X0X4)/F(X0,X4))'];%the Design matrix
dX=-1*inv(A'*P*A)*(A'*P*W); %Computes perturbation to X0 using
standard result
moddX=sqrt(dX'*dX); %Size of perturbation
X0=X0+dX; %New value for position of point
number_of_iterations=number_of_iterations+1;%This counts how many
iterations are performed
if (number_of_iterations>1000000) %Limits iterations so the program
doesn't get stuck
moddX=a/10;
simulation_number
('Iterations exceed 1000000')
end
end %This process will be repeated until changes in X0 are smaller
than a
dX;
X0;
moddX;
F(dX,[0;0]);
simulation_number=simulation_number+1;
X;
calculated=X0+dX;
offL1=F(calculated,X1); %Calculates distances between calculated
point and
offL2=F(calculated,X2); %observer points
offL3=F(calculated,X3);
Patrick Gloster
offL4=F(calculated,X4);
%relerror1=abs(L1-offL1)/L1; %Calculates relative error of each
measurement
%relerror2=abs(L2-offL2)/L2;
%relerror3=abs(L3-offL3)/L3;
%relerror4=abs(L4-offL4)/L4;
vectoroff1(end+1)=(D1-offL1);
vectoroff2(end+1)=(D2-offL2);
vectoroff3(end+1)=(D3-offL3);
vectoroff4(end+1)=(D4-offL4);
end
vectoroff1=vectoroff1(2:end);
vectoroff2=vectoroff2(2:end);
vectoroff3=vectoroff3(2:end);
vectoroff4=vectoroff4(2:end);
subplot(2,2,1);
hist(vectoroff1,1000)
[n1,xout1]=hist(vectoroff1,1000);
plot(xout1,n1)
title('L1');
Plot1=fit(xout1',n1','gauss1')
subplot(2,2,2);
hist(vectoroff2,1000)
[n2,xout2]=hist(vectoroff2,1000);
plot(xout2,n2)
title('L2');
Plot2=fit(xout2',n2','gauss1')
subplot(2,2,3);
hist(vectoroff3,1000)
[n3,xout3]=hist(vectoroff3,1000);
plot(xout3,n3)
title('L3');
Plot3=fit(xout3',n3','gauss1')
subplot(2,2,4);
hist(vectoroff4,1000)
[n4,xout4]=hist(vectoroff4,1000);
plot(xout4,n4)
title('L4');
Plot4=fit(xout4',n4','gauss1')
helpdlg('Simulation complete','Patrick says');