I am unable to solve the error I am facing. It's with the ax,ay, az and I think the problem is either in the way I am implementing them or in the stuff in their equations, the xs,ys,zs or the X,Y,Z. Please if someone can have a look at the code below I commented which line the error is. Maybe someone can advise me how to fix it. Thanks.
clear all;
clc;
load data3.csv
c=299792458;
newdata=[];
shj=1;
i=1;
N=1;
SecOfDay=30;
counter_1=0;
for S=1:length(data3)
if(data3(S,6)>15)
newdata(i,:)=data3(S,:);
i=i+1;
end
end
while N<length(newdata)
for r=1:length(newdata)
if(SecOfDay==newdata(r,1))
counter_1=counter_1+1;
end
xs=newdata(N:counter_1,3);
ys=newdata(N:counter_1,4);
zs=newdata(N:counter_1,5);
dist=newdata(N:counter_1,6);
end
Fun = @(u) [sqrt((xs-u(1)).^2 + (ys-u(2)).^2 + (zs-u(3)).^2) - (c*u(4)) - (dist)];
x0 = [0;0;0;0];
u = lsqnonlin(Fun,x0);
SecOfDay=SecOfDay+30;
X=u(1);
Y=u(2);
Z=u(3);
ERROR=u(4);
r= sqrt((xs-X).^2-(ys-Y).^2-(zs-Z).^2);
ax(N:counter_1)=(xs-X)/r;
ay(N:counter_1)=(ys-Y)/r;
az(N:counter_1)=(zs-Z)/r;
N=counter_1+1;
H=[ax ay az 1];
for i=N:counter_1
HH=[H; ax(i) ay(i) az(i) 1];
end
end
