% ls_converge Tracking with the least squares procedure.
%
% SYNOPSIS:
% function [x_new] = ls_converge (xgu, nals, sspos, dist, sv)
%
% DESCRIPTION: Return the [latitude longitude] (radians) of a position
% based on the least squares tracking method.
%
% ARGUMENTS:
% xgu is the guess at the position, usually the previous position.
% Stored as [lat lon] degrees.
% If the drift is to be computed it is stored as the third
% element of the vector, xgu(3).
%
% nals is the number of sound sources.
%
% sspos is the sound source positions corresponding to distances.
%
% dist is distance to sound source, km.
%
% sv is the sound velocity (km/sec) as determined from the current
% temperature.
%
% Code by Roger Goldsmith, WHOI
% kindly provided by Heather D. Hunt, WHOI
%
function [x_new] = ls_converge (xgu, nals, sspos, dist, sv)
RAD = 180/pi;
KM_PER_DEG = 111.324;
mtry = 30;
rate = sv;
% fprintf ('LS1 %10.4f %10.4f\n', dist(1)/sv, dist(2)/sv);
% fprintf ('LS2 %10.4f %10.4f %10.4f\n', dist(1)/sv, dist(2)/sv, dist(3)/sv);
drft_init = 0.0;
drft = 0.0;
do_time = 0;
oldpos(1:2) = xgu(1:2);
%oldpos(2) = xgu(2);
if (length (xgu) > 2)
do_time = 1;
drft_init = xgu(3);
drft = drft_init;
end % if
%
xpos = xgu/RAD;% convert 'oldpos' (degrees) to 'xpos' (radians).
itry = 0; % start value for number of iterations performed.
cradius = 100.0; % the start value for the convergence radius
while cradius > 0.05
alpha_beta = 0.0;
alpha_sqd = 0.0;
alpha_tau = 0.0;
beta_sqd = 0.0;
beta_tau = 0.0;
sum_alpha = 0.0;
sum_beta = 0.0;
sum_tau = 0.0;
% Loop through the sources collecting terms. Throw out the source if zero.
nals_act = 0;
for i = 1:nals
%if (dist(i) >= 0) % NOT A GOOD TEST, LOOK FOR TIME = 0
if (~isnan(dist(i))) % Is this a better approach?
% Get the distance from last position to source.
% Contrary to the documentation in ELLIPK2, the bearing is returned in degrees.
sosopos = [sspos(i,1) sspos(i,2)];
[db(1), db(2)] = artoa.data.calculateEllipk2 (xpos, sosopos/RAD);
dst = db(1);
beta = cos (db(2)/RAD )/rate;
alpha = sin (db(2)/RAD )/rate;
tau = (dist(i) - dst)/rate - drft_init + drft;
% fprintf ('TAU: %3d %2d %8.2f %8.2f %8.2f %8.2f %8.2f\n', itry,i,dist(i),dst,drft_init,drft,tau);
alpha_beta = alpha_beta + alpha*beta;
alpha_sqd = alpha_sqd + alpha*alpha;
alpha_tau = alpha_tau + alpha*tau;
beta_sqd = beta_sqd + beta*beta;
beta_tau = beta_tau + beta*tau;
% fprintf ('LS3 %d %7.2f %7.2f %7.2f %6.2f\n', i, dist(i), dst, tau, db(2) );
if (do_time == 1)
sum_alpha = sum_alpha + alpha;
sum_beta = sum_beta + beta;
sum_tau = sum_tau + tau;
end % if
nals_act = nals_act + 1; % the actual number of available sosos.
end % if distance was greater zero
end % for i = loop over all sources
if (itry == 0 & nals_act ~= nals)
if (nals_act < 2) % less than two sources available
fprintf (' *** NOT ENOUGHT VALID POINTS.\n');
x_new = NaN*size (oldpos);
return;
end % if
end % if
% Set the test criteria to one degree.
stest = sin (1.0/RAD) * nals_act/rate;
stest = stest*stest;
%
% Compute corrections.
alpha_sqd = alpha_sqd - sum_alpha*sum_alpha/nals_act;
alpha_beta = alpha_beta - sum_alpha*sum_beta/nals_act;
alpha_tau = alpha_tau - sum_alpha*sum_tau/nals_act;
beta_sqd = beta_sqd - sum_beta*sum_beta/nals_act;
beta_tau = beta_tau - sum_beta*sum_tau/nals_act;
denom = alpha_sqd*beta_sqd - alpha_beta*alpha_beta;
if (denom == 0)
denom = stest;
end % if
if (abs (denom) < stest)
denom = sign (denom)*stest;
end % if
xx = (alpha_tau*beta_sqd - alpha_beta*beta_tau)/denom;
yy = (alpha_sqd*beta_tau - alpha_beta*alpha_tau)/denom;
% Update guess then test radius for threshold.
cradius = xx*xx + yy*yy;
if (do_time == 1)
drft_corr = (-sum_tau + xx*sum_alpha + yy*sum_beta)/nals_act;
cradius = cradius + (drft_corr*rate).^2;
drft = drft + drft_corr;
end % if
% fprintf ('LS4 %3d %10.2f %10.4f%10.6f %10.3f%10.5f %10.2f%10.4f\n', itry, -sum_tau, xx, sum_alpha, yy, sum_beta, drft_corr, drft );
if (cradius > 0)
cradius = sqrt (cradius);
else
cradius = 0.0;
end % if
bear = atan2 (xx, yy);
t1p = sin (oldpos(1)/RAD) * cos (cradius/KM_PER_DEG/RAD) + ...
cos (oldpos(1)/RAD) * sin (cradius/KM_PER_DEG/RAD) * ...
cos (bear);
t1 = cos ((90.0-oldpos(1))/RAD) * cos (cradius/KM_PER_DEG/RAD) + ...
sin ((90.0-oldpos(1))/RAD) * sin (cradius/KM_PER_DEG/RAD) * ...
cos (bear);
t1_rad = acos (t1);
t1p_rad = acos (t1p);
t1_deg = 90.0 - t1_rad*RAD;
t1p_deg = 90.0 - t1p_rad*RAD;
t2 = sin (cradius/KM_PER_DEG/RAD) * sin (bear) / sin (t1_rad);
t2p = sin (cradius/KM_PER_DEG/RAD) * sin (bear) / sin (t1_rad);
if (abs (t2) > 1.0)
t2 = sign (t2);
end % if
if (abs (t2p) > 1.0)
t2p = sign (t2p);
end % if
t2_rad = asin (t2) + oldpos(2)/RAD;
t2p_rad = asin (t2p) + oldpos(2)/RAD;
t2_deg = t2_rad*RAD;
t2p_deg = t2p_rad*RAD;
xpos = [t1_deg/RAD t2_rad];
xposp = [t1p_deg/RAD t2p_rad];
% fprintf ('LS5 %3d %5.3f %6.2f %6.2f %6.2f %6.1f %8.4f %9.4f\n', itry, denom, xx, yy, cradius, bear*RAD, xpos(2)*RAD, xpos(1)*RAD)
itry = itry + 1;
if (itry > mtry)
fprintf (' *** NO CONVERGENCE AFTER %d ITERATIONS.\n', mtry);
x_new = NaN*size (oldpos);
return;
end % if
oldpos = xpos*RAD;
%oldpos(2) = xpos(2)*RAD;
oldposp = xposp*RAD;
%oldposp(2) = xposp(2)*RAD;
oldpos = xposp*RAD;
%oldpos(2) = xposp(2)*RAD;
end % while cradius
x_new = xpos;
if (do_time == 1)
x_new(3) = drft;
end % if
%fprintf (' #S%2d ', nals_act);
%fprintf('ls-');
clear alpha alpha_beta alpha_sqd alpha_tau;
clear beta beta_sqd beta_tau
clear cradius;
clear db denom dst;
clear i itry;
clear rate;
clear ssq stest;
clear sosopos;
clear sum_alpha sum_beta sum_tau;
clear t1 t1_rad t1_deg;
clear tau;
clear xpos oldpos;
clear xx yy;