clear all
close all

delta = [0 0 0 0];
%Transponder Details
tot = 0.003;  %Transponder trnaround time

%Initial Guesses
Co = 1500;  %Speed of sound, approx 1500 m/s
Yo = [563654 4075168 -1500];  %approx position of anchor (Easting, Northing, and Depth)

epsilon  = 0.0001;

%Fixed Measurement Points (Easting, Northing, and Depth)
 X = [ 
    564223, 4075120, -3.5; % Fixed Pt 1
    563447, 4075522, -3.5; % Fixed Pt 2
    564330, 4075636, -3.5;
    563654, 4075168, -3.5;
    562360, 4074896, -3.5;
    562733, 4073669, -3.5;
    564224, 4073626, -3.5; % Fixed Pt N
];
 
tt = [             %Two-way Travel Times
     2.1795, 2.1783, 2.1795, 2.1809, 2.1806, 2.1798;  % Measurement 1
     2.1057, 2.1059, 2.0988, 2.0972, 2.0972, 2.10096; % Measurement 2
     2.3241, 2.3246, 2.3279, 2.3288, 2.3309, 2.3323;
     2.0277, 2.0277, 2.0292, 2.0262, 2.0321, 2.0286;
     2.6786, 2.6905, 2.6916, 2.6935, 2.6932 ,2.6953;
     3.1069, 3.1062, 3.1065, 3.106, 3.1071, 3.1058;
     3.0061, 3.0151, 3.0278, 3.0316, 3.0343, 3.0372   % Measurement N
 ];
 
 %Convert to one way travel times
 tt = (tt-tot)/2;
 
 
 N = length(tt);    %Number of measurements
 
iterations = 0;
 while((norm(delta) > epsilon) || iterations == 0)
 iterations= iterations+1;
 Yo = [Yo(1)+delta(1) Yo(2)+delta(2) Yo(3)+delta(3)];
 Co = Co + delta(4);

 
 tto = sqrt((X(:,1)-Yo(1)).^2+(X(:,2)-Yo(2)).^2+(X(:,3)-Yo(3)).^2)/Co;
 
 f = tto-mean(tt')';
 
 B = [((X(:,1)-Yo(1))./tto)/(Co*Co) ((X(:,2)-Yo(2))./tto)/(Co*Co) ((X(:,3)-Yo(3))./tto)/(Co*Co)  tto./Co];
 
 delta = B\f
 
 %delta = lscov(B,f,ones(length(f)))
end 

iterations
Yo(1)
Yo(2)
Yo(3)
Co