clc
clear all
close all

syms a 'o' 'y' 'z' 'w' 
P=[ 0 60 2; 50 0 2; 120 60 2; 50 140 2]; %Positionen der einzelnen Puntke [m]
c_w=1480; %Geschwindigkeit [m/s]
time=[   0.0269    0.0202    0.0406    0.0338    0.0471    0.0540]; %Zeit[s]

%Kugelgleichungen:

% sqrt((o-P(1,1)).^2+(y-P(1,2)).^2+(z-P(1,3))^2)=w+(time(2)-time(1))*c_w;
% sqrt((o-P(2,1)).^2+(y-P(2,2)).^2+(z-P(2,3))^2)=w+(time(3)-time(1))*c_w;
% sqrt((o-P(3,1)).^2+(y-P(3,2)).^2+(z-P(3,3))^2)=w+(time(4)-time(1))*c_w;
% sqrt((o-P(4,1)).^2+(y-P(4,2)).^2+(z-P(4,3))^2)=w+(time(5)-time(1))*c_w;

eq1=sqrt((o-P(1,1)).^2+(y-P(1,2)).^2+(z-P(1,3))^2)-w+(time(2)-time(1))*c_w;
eq2=sqrt((o-P(2,1)).^2+(y-P(2,2)).^2+(z-P(2,3))^2)-w+(time(3)-time(1))*c_w;
eq3=sqrt((o-P(3,1)).^2+(y-P(3,2)).^2+(z-P(3,3))^2)-w+(time(4)-time(1))*c_w;
eq4=sqrt((o-P(4,1)).^2+(y-P(4,2)).^2+(z-P(4,3))^2)-w+(time(5)-time(1))*c_w;

F11=char(eq1);
F22=char(eq2);
F33=char(eq3);
F44=char(eq4);
FFF= (['[' F11 ';' F22 ';' F33 ';' F44 ']' ]);

FFF2= regexprep(FFF,'o','x(1)');
FFF3= regexprep(FFF2,'y','x(2)');
FFF4= regexprep(FFF3,'z','x(3)');
FFF5= regexprep(FFF4,'w','x(4)');

FW= inline(FFF5);
options = optimset('display', 'off');
[x_s,resnorm]=lsqnonlin(FW,[0,0,0,0],[0,0,0,0],[200,200,200,200], options);

o_store(i,j)=x_s(1)
y_store(i,j)=x_s(2)
z_store(i,j)=x_s(3)
w_store(i,j)=x_s(4)