%Question Part D For 2D figure

clear all

RK1 =150 ;%RK is the measured range RK(m)

RK2 = 250;
RK3 =460 ;
RK4 =500 ;

XB = 0; %XB,YB and ZB are the initially position

YB = 0;
ZB = 0;

axis([-20,0,0,80]);
for i = 1:20
init_root = [XB;XB;ZB];

XX= [];
YX = [];
ZZ = [];

EE= [];
SQRT1=sqrt((100-XB)^2 + (100-YB)^2 + (20-ZB)^2);
SQRT2=sqrt((100-XB)^2 + (-100 - YB)^2 + (50 - ZB)^2);
SQRT3=sqrt((400-XB)^2 + (200 - YB)^2 + (20 - ZB)^2);
SQRT4=sqrt((400-XB)^2 + (-200 - YB)^2 + (40 - ZB)^2);

%The beacon position for set of four equations

F1_X_Y_Z = RK1 -SQRT1 ;

F2_X_Y_Z = RK2-SQRT2;
F3_X_Y_Z = RK3-SQRT3;
F4_X_Y_Z = RK4-SQRT4;

%The Jacobian function for the receivers:

%J_i = [F1_XB,F1_YB,F1_ZB ; F2_XB,F2_yb,F2_ZB ; F3_XB,F3_YB,F3_ZB ;F4_XB,F4_YB,F4_ZB];

F1=sqrt((100-XB)^2 + (100 - XB)^2 + (20 - ZB)^2);

F2=sqrt((100-XB)^2 + (-100 - YB)^2 + (50 - ZB)^2);
F3=sqrt((400-XB)^2 + (200 - YB)^2 + (20 - ZB)^2);
F4=sqrt((400-XB)^2 + (-200 - YB)^2 + (40 - ZB)^2);

F1_XB = (100 - XB)/F1;

F1_Yb = (100 - YB)/F1;
F1_Zb = (20 - ZB)/F1;

F2_XB = (100 - XB)/F2;

F2_YB = (-100 - YB)/F2;
F2_ZB = (50 - ZB)/F2;

F3_XB = (400 - XB)/F3;

F3_YB = (200 - YB)/F3;
F3_ZB = (20 - ZB)/F3;

F4_XB = (400 - XB)/F4;

F4_YB = (-200 - YB)/F4;
F4_ZB = (40 - ZB)/F4;

%The Jacobian function for the receivers:

J_i = [F1_XB,F1_YB,F1_ZB ; F2_XB,F2_YB,F2_ZB ; F3_XB,F3_YB,F3_ZB ;
F4_XB,F4_YB,F4_ZB];

%The Function Vector

F_i = [F1_X_Y_Z ; F2_X_Y_Z ; F3_X_Y_Z ; F4_X_Y_Z] ;

init_root1 = init_root - J_i \ F_i;

XB = init_root1(1,1);
YB = init_root1(2,1);
ZB = init_root1(3,1);

XX = [XX XB]; %Value of X

YY = [YY YB];%Value of Y
ZZ = [ZZ ZB];%Value of Z
Figure(1);
plot(XX, YY,'r*'); grid on;
xlabel('x');ylable('y');
hold on;
end
hold off;
%Question Part D For 3D figure
Close all
clear all

RK1 = 150;%RK is the measured range RK(m)

RK2 = 250;
RK3 = 460;
RK4 = 500;

XB = 0; %XB,YB and ZB are the initially position

YB = 0;
ZB = 0;

plot(XB, YB, 'b*'); hold on;

axis([-20,0,0,80]);
for i = 1:20
init_root = [XB;XB;ZB];

XX= [];
YX = [];
ZZ = [];

EE= [];
SQRT1=sqrt((100-XB)^2 + (100-YB)^2 + (20-ZB)^2);
SQRT2=sqrt((100-XB)^2 + (-100 - YB)^2 + (50 - ZB)^2);
SQRT3=sqrt((400-XB)^2 + (200 - YB)^2 + (20 - ZB)^2);
SQRT4=sqrt((400-XB)^2 + (-200 - YB)^2 + (40 - ZB)^2);

%The beacon position for set of four equations

F1_X_Y_Z = RK1 -SQRT1 ;

F2_X_Y_Z = RK2-SQRT2;
F3_X_Y_Z = RK3-SQRT3;
F4_X_Y_Z = RK4-SQRT4;

%The Jacobian function for the receivers:

%J_i = [F1_XB,F1_YB,F1_ZB ; F2_XB,F2_yb,F2_ZB ; F3_XB,F3_YB,F3_ZB ;F4_XB,F4_YB,F4_ZB];

F1=sqrt((100-XB)^2 + (100 - XB)^2 + (20 - ZB)^2);

F2=sqrt((100-XB)^2 + (-100 - YB)^2 + (50 - ZB)^2);
F3=sqrt((400-XB)^2 + (200 - YB)^2 + (20 - ZB)^2);
F4=sqrt((400-XB)^2 + (-200 - YB)^2 + (40 - ZB)^2);

F1_XB = (100 - XB)/F1;

F1_Yb = (100 - YB)/F1;
F1_Zb = (20 - ZB)/F1;

F2_XB = (100 - XB)/F2;

F2_YB = (-100 - YB)/F2;
F2_ZB = (50 - ZB)/F2;

F3_XB = (400 - XB)/F3;

F3_YB = (200 - YB)/F3;
F3_ZB = (20 - ZB)/F3;

F4_XB = (400 - XB)/F4;

F4_YB = (-200 - YB)/F4;
F4_ZB = (40 - ZB)/F4;

%The Jacobian function for the receivers:

J_i = [F1_XB,F1_YB,F1_ZB ; F2_XB,F2_YB,F2_ZB ; F3_XB,F3_YB,F3_ZB ;
F4_XB,F4_YB,F4_ZB];

%The Function Vector

F_i = [F1_X_Y_Z ; F2_X_Y_Z ; F3_X_Y_Z ; F4_X_Y_Z] ;

init_root1 = init_root - J_i \ F_i;

XB = init_root1(1,1);
YB = init_root1(2,1);
ZB = init_root1(3,1);
XX = [XX XB]; %Value of X
YY = [YY YB];%Value of Y
ZZ = [ZZ ZB];%Value of Z
Figure(2);
plot(XX, YY,'r*'); grid on;
xlabel('x');ylable('y');
hold on;
end
hold off;
%Question Part D For Errors

Close all
clear all

RK1 = 150;%RK is the measured range RK(m)

RK2 = 250;
RK3 = 460;
RK4 = 500;

XB = 0; %XB,YB and ZB are the initially position

YB = 0;
ZB = 0;

axis([-20,0,0,80]);
for i = 1:20
init_root = [XB;XB;ZB];

XX= [];
YX = [];
ZZ = [];

EE= [];
SQRT1=sqrt((100-XB)^2 + (100-YB)^2 + (20-ZB)^2);
SQRT2=sqrt((100-XB)^2 + (-100 - YB)^2 + (50 - ZB)^2);
SQRT3=sqrt((400-XB)^2 + (200 - YB)^2 + (20 - ZB)^2);
SQRT4=sqrt((400-XB)^2 + (-200 - YB)^2 + (40 - ZB)^2);

%The beacon position for set of four equations

F1_X_Y_Z = RK1 -SQRT1 ;

F2_X_Y_Z = RK2-SQRT2;
F3_X_Y_Z = RK3-SQRT3;
F4_X_Y_Z = RK4-SQRT4;

%The Jacobian function for the receivers:

%J_i = [F1_XB,F1_YB,F1_ZB ; F2_XB,F2_yb,F2_ZB ; F3_XB,F3_YB,F3_ZB ;F4_XB,F4_YB,F4_ZB];

F1=sqrt((100-XB)^2 + (100 - XB)^2 + (20 - ZB)^2);

F2=sqrt((100-XB)^2 + (-100 - YB)^2 + (50 - ZB)^2);
F3=sqrt((400-XB)^2 + (200 - YB)^2 + (20 - ZB)^2);
F4=sqrt((400-XB)^2 + (-200 - YB)^2 + (40 - ZB)^2);

F1_XB = (100 - XB)/F1;

F1_Yb = (100 - YB)/F1;
F1_Zb = (20 - ZB)/F1;

F2_XB = (100 - XB)/F2;

F2_YB = (-100 - YB)/F2;
F2_ZB = (50 - ZB)/F2;

F3_XB = (400 - XB)/F3;

F3_YB = (200 - YB)/F3;
F3_ZB = (20 - ZB)/F3;

F4_XB = (400 - XB)/F4;

F4_YB = (-200 - YB)/F4;
F4_ZB = (40 - ZB)/F4;

%The Jacobian function for the receivers:

J_i = [F1_XB,F1_YB,F1_ZB ; F2_XB,F2_YB,F2_ZB ; F3_XB,F3_YB,F3_ZB ;
F4_XB,F4_YB,F4_ZB];

%The Function Vector

F_i = [F1_X_Y_Z ; F2_X_Y_Z ; F3_X_Y_Z ; F4_X_Y_Z] ;

init_root1 = init_root - J_i \ F_i;

XB = init_root1(1,1);
YB = init_root1(2,1);
ZB = init_root1(3,1);

XX = [XX XB]; %Value of X

YY = [YY YB];%Value of Y
ZZ = [ZZ ZB];%Value of Z
K=0:1:20 %Incrementation
R = [RK1, RK2, RK3, RK4];
%The error as a function inorder to measured range values base on beacon
%position
 error1 = R(1) – F1;
error2 = R(2) – F2;
error3 = R(3) –F3;
error4 = R(4) –F4;

E = [error1 ; error2 ; error3 ; error4];

E_Val = [E sqrt(E' * E)]

Figure(3);

plot3(XX, YY, ZZ, 'm*'); grid on;

plot(XX, YY,'r*'); grid on;
plot (i, E, 'b*');
hold on;
title(‘Iteration VS Erroe’)
xlabel('Itetation');ylable('Error');
plot(XX,YY,’r’)
plot(XX,ZZ,’k’)
plot(YY,ZZ,’b’)
plot(ZZ,XX,’c’)
legend(‘f1’,’f2’,’f3’,’f4’)

end
hold off;

In part “c” of the graph, all the function converge to zero. The started point numbers of part “c” are
accurate to numerous significant digits; therefore round-off error is non-existent. However, in part “D”,
three significant digits are large round-off error as show in the graph.