Circular.m

function [positions, clockError] = Circular(pData, pSoundsourcePositions, pCombinationDetails, pStartEndPosition)
%HYPERBOLIC Hyperbolic tracking algorithm. Requires three soundsources.
%
%   Parameters:
%       pData (struct):
%           Data structured by soundsource. The toa is already corrected
%           (e.g. doppler correction etc.)
%       pSoundsourcePositions (struct):
%           The soundsource positions [deg].
%       pCombinationDetails (table):
%           Contains all information of the current soundsource
%           combination.
%       pStartEndPosition (double, logical):
%           Set to false if it has not been passed by artoa. Contains the
%           desired start and end position as a matrix of row vectors.
%
%
%   Returns:
%       positions (double):
%           The calculated list of segment positions as a X x 2 column vector.
%       clockError (double):
%           The estimated clock error.

%% Get reference position and all soundsource names that are involved
referencePosition = cellfun(@str2double, strsplit(pCombinationDetails.referencePosition{1}));
soundsourceNames = strsplit(pCombinationDetails.soundsources{1});

% %% Check if three soundsources are available
% if length(soundsourceNames) ~= 3
%     errordlg(['Hyperbolic tracking algorithm requires three soundsources. ' num2str(length(soundsourceNames)) ' selected!']);
%     error(['Hyperbolic tracking algorithm requires three soundsources. ' num2str(length(soundsourceNames)) ' selected!']);
% end

%% Prepare data for calculations
% get a date vector (does not matter which one)
dates = cell2mat(pData.(soundsourceNames{1}).date);
pDistances = NaN(length(dates), length(soundsourceNames));
soundsourcePositions = NaN(length(soundsourceNames), 2);
soundVelocity = NaN(length(soundsourceNames), 1);
% prepare distances to, and positions of, all soundsources
for oSoundsource = 1:length(soundsourceNames)
    pDistances(:, oSoundsource) = ...
        cell2mat(pData.(soundsourceNames{oSoundsource}).distance);
    soundsourcePositions(oSoundsource, :) = ...
        pSoundsourcePositions.(soundsourceNames{oSoundsource});
    soundVelocity(oSoundsource) = pData.(soundsourceNames{oSoundsource}).soundspeed{1};
end

% initialize return variables
positions = referencePosition;
clockError = [];

% Calculate the actual segment positions
for oDates = 1:length(dates)
    distances = pDistances(oDates, :);
    if any(isnan(distances))
        positions = [positions; [NaN NaN]];
        clockError = [clockError; NaN(1, 3)];
        continue;
    end
    positions = [positions; ...
        rad2deg(artoa.vendor.xnavai(deg2rad(soundsourcePositions), distances, deg2rad(referencePosition), 2)) ...
    ];
    clockError = [clockError; NaN(1, 3)];
    disp([' *** Date: ' num2str(dates(oDates)) ' converged at position: ' num2str(positions(end, 1)) ', ' num2str(positions(end, 2))]);
end

positions = positions(2:end, :);

end