From 2b2263a797c8efcdf2d350b4f90b6a2decb37274 Mon Sep 17 00:00:00 2001
From: Lewin Probst <info@emirror.de>
Date: Mon, 21 Oct 2019 09:37:05 +0200
Subject: [PATCH] Added ls_converge function to the vendor package.
---
lib/+artoa/+vendor/ls_converge.m | 200 +++++++++++++++++++++++++++++++
1 file changed, 200 insertions(+)
create mode 100644 lib/+artoa/+vendor/ls_converge.m
diff --git a/lib/+artoa/+vendor/ls_converge.m b/lib/+artoa/+vendor/ls_converge.m
new file mode 100644
index 0000000..acaa6a5
--- /dev/null
+++ b/lib/+artoa/+vendor/ls_converge.m
@@ -0,0 +1,200 @@
+% 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;
+
--
GitLab