clear clc close
iter = 0;
lat_tgt = 34.0151;
long_tgt = 71.5249;
z_s = 0.01/375;
while iter < 100
iter=iter+1;
long_rand = rand/10;
lat_rand = (-0.5 + (0.5+0.5)*rand)/10;
long_rand = (-0.5 + (0.5+0.5)*rand)/10;
lat_tgt = lat_tgt+lat_rand; long_tgt = long_tgt+long_rand;
lats = [34.1989 34.0105 34.067894 34.1166 lat_tgt]; longs = [72.0231 71.9876 71.992783 72.0216 long_tgt];
[x,y] = grn2eqa(lats,longs,[34.1166, 72.0216]);
z=[0 0 0 0];
c=2.997924580*10^8;
z_s=z_s+(-0.5 + (0.5+0.5)*rand)/37500;
t1 = (sqrt((x(5)-x(4))^2+(y(5)-y(4))^2+(z_s-z(4))^2)-sqrt((x(5)-x(1))^2+(y(5)-y(1))^2+(z_s-z(1))^2))/c;
t2 = (sqrt((x(5)-x(4))^2+(y(5)-y(4))^2+(z_s-z(4))^2)-sqrt((x(5)-x(2))^2+(y(5)-y(2))^2+(z_s-z(2))^2))/c;
t3 = (sqrt((x(5)-x(4))^2+(y(5)-y(4))^2+(z_s-z(4))^2)-sqrt((x(5)-x(3))^2+(y(5)-y(3))^2+(z_s-z(3))^2))/c;
syms xs ys zs
eqn1 = sqrt((xs-x(4))^2+(ys-y(4))^2+(zs-z(4))^2)-sqrt((xs-x(1))^2+(ys-y(1))^2+(zs-z(1))^2)-(c*t1)==0;
eqn2 = sqrt((xs-x(4))^2+(ys-y(4))^2+(zs-z(4))^2)-sqrt((xs-x(2))^2+(ys-y(2))^2+(zs-z(2))^2)-(c*t2)==0;
eqn3 = sqrt((xs-x(4))^2+(ys-y(4))^2+(zs-z(4))^2)-sqrt((xs-x(3))^2+(ys-y(3))^2+(zs-z(3))^2)-(c*t3)==0;
sol = solve([eqn1, eqn2, eqn3], [xs ys zs]);
m = 1;
for n = 1:length(sol.xs)
possibleSol(1,m) = double(sol.xs(n));
possibleSol(2,m) = double(sol.ys(n));
possibleSol(3,m) = double(sol.zs(n));
m=m+1;
end
idx = possibleSol(3,:) < 0 | any(imag(possibleSol) ~=0);
possibleSol(:, idx) = [];
[lat,long] = eqa2grn(possibleSol(1),possibleSol(2),[34.1166, 72.0216]);
posSolMoving(:,iter) = possibleSol;
x_s=x(length(x)); y_s=y(length(y)); x = x(1:length(x)-1); y = y(1:length(y)-1);
figure(1)
hold on
grid on
view(3);
plot3(x,y,z, 'ro', 'LineWidth', 2, 'MarkerSize', 10);
plot3(posSolMoving(1,:),posSolMoving(2,:),posSolMoving(3,:), 'b+', 'LineWidth', 4, 'MarkerSize', 4)
plot3(x_s,y_s,z_s,'g+', 'LineWidth', 2, 'MarkerSize', 4)
if iter > 1
q = quiver3(posSolMoving(1,iter-1),posSolMoving(2,iter-1),posSolMoving(3,iter-1),posSolMoving(1,iter)-posSolMoving(1,iter-1),posSolMoving(2,iter)-posSolMoving(2,iter-1),posSolMoving(3,iter)-posSolMoving(3,iter-1),0);
q.LineWidth=0.75;
end
legend({'Receivers', 'Source','exactvalue'})
view(-14.3,33.2)
pause(1)
end