SingleSource.m

function [positions, clockError] = SingleSource(pData, pSoundsourcePositions, pCombinationDetails, pStartEndPosition)
%SINGLESOURCE Estimates the trajectory by using only one soundsource.
%
%   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.


%% Check input
referencePositionStart = cellfun(@str2double, strsplit(pCombinationDetails.referencePosition{1}));
referencePositionEnd   = cellfun(@str2double, strsplit(pCombinationDetails.referencePositionEnd{1}));
if any(isnan(referencePositionStart)) || any(isnan(referencePositionEnd))
    error([mfilename ': SingleSource algorithm requires BOTH (Reference) Positions to be set!']);
end

%% Get required data
soundsourceName = strsplit(pCombinationDetails.soundsources{1});
soundsourceName = soundsourceName{1};

%% Prepare data for calculations
% get a date vector (does not matter which one)
dates = cell2mat(pData.(soundsourceName).date);
distances = cell2mat(pData.(soundsourceName).distance);
soundsourcePosition = pSoundsourcePositions.(soundsourceName);

% initialize return variables
positions = [ ...
    referencePositionStart; ...
    ];
endPosition = str2num(pCombinationDetails.referencePositionEnd{:});
clockError = [];

%% Algorithm
dateLength = length(dates);  % number of points to calculate trajectory for
timeStep = diff(dates(1:2))*86400;  % time between position fixes   %!!!  needs genearlization accorind to float's RAFOS schedule
averageHorizontalVelocity = (distance(str2num(pCombinationDetails.referencePositionEnd{:}),...
    str2num(pCombinationDetails.referencePosition{:}))*60*1852)...
    / ((pCombinationDetails.combinationEnd-pCombinationDetails.combinationBegin)*86400)   % in meter per second

j = 0
figure(10)

for HorizontalVelocity = averageHorizontalVelocity:.05: 5*averageHorizontalVelocity
    HorizontalVelocity
    j = j+1
    %clear positions
    posi(j,:,1) = NaN*ones(1,dateLength,1);
    posi(j,:,1) = NaN*ones(1,dateLength,1);

    for i = 1:dateLength
       
        currentDistance = distances(i);    % get current distance [km] according to current toa
        [lat1,lon1] = scircle1( ...
            soundsourcePosition(1), ...
            soundsourcePosition(2), ...
            km2deg(currentDistance));      % calculate circle around soundsource
%         plot(lon1,lat1,'r'); hold on       % calculate circle around last float position

        if i == 1
            lastFloatPosition(1) = referencePositionStart(1);
            lastFloatPosition(2) = referencePositionStart(2);
            [dummy,oldbearing] = distance(referencePositionStart(1),referencePositionStart(2),referencePositionEnd(1),referencePositionEnd(2));
        else
            %continue
        end
        
        [lat2,lon2] = scircle1( ...
            lastFloatPosition(1), ...
            lastFloatPosition(2), ...
            km2deg(HorizontalVelocity*timeStep/1000)); 
%          plot(lon2,lat2,'b'); hold on; grid on
        
         [lat,lon] = scxsc(soundsourcePosition(1),soundsourcePosition(2),km2deg(currentDistance),...
                     lastFloatPosition(1), lastFloatPosition(2),km2deg(HorizontalVelocity*timeStep/1000) );
         %% so we have usually 2 solution, which one to select?  --> the one closer towards the final point
  
%         [helper] = distance(lat,lon,endPosition(1),endPosition(2));   %%%!  besser: der punkt der sich auf der Verbindungsachse zwischen A und B in Richtung B bewegt
          [helper,newbearing] = distance( lastFloatPosition(1), lastFloatPosition(2),lat,lon);   %%%!  besser: der punkt der sich auf der Verbindungsachse zwischen A und B in Richtung B bewegt
    
        
        if ~isnan(helper(1))  %  intersection found
%             helperindi = find(helper == min(helper));
             helper = abs(newbearing - oldbearing);
             helperindi = find(helper == min(helper));
             oldbearing = newbearing(helperindi);

            lastFloatPosition(1) = lat(helperindi);
            lastFloatPosition(2) = lon(helperindi);
            posi(j,i,1) =  lastFloatPosition(1);
            posi(j,i,2) =  lastFloatPosition(2);
     
       
            plot(lastFloatPosition(2),lastFloatPosition(1),'xk')
            plot(posi(j,:,2),posi(j,:,1),'-b'); hold on

        else isnan(helper(1))
            if size(posi,1) == 1
                            plot(posi(:,2),posi(:,1),'or'); hold on
            else
            plot(posi(j,:,2),posi(j,:,1),'-r'); hold on
            end
            %           averageHorizontalVelocity = averageHorizontalVelocity + .01*averageHorizontalVelocity
            
            break
        end

 
    end
    
drawnow
  
    
end

validdata = find(~isnan(posi(:,end,1)))
helper = distance(referencePositionEnd(1),referencePositionEnd(2),posi(validdata,end,1),posi(validdata,end,2))
jindi = find(helper == min(helper))

positions(1:dateLength,1) = posi(validdata(jindi),:,1)'
positions(1:dateLength,2) = posi(validdata(jindi),:,2)'

clockError = zeros(length(posi),1);  % ich vermute mal das dies der Unterschied gemessene toa zu der "von der errechneten Position abgeleiteten toa".   der ist null da wir ihn of 0 zwingen.

end