Skip to content
Snippets Groups Projects
Commit 6a56ae57 authored by leprob001's avatar leprob001
Browse files

Initial version, algorithm by Olaf Boebel.

parent 73d4b8a5
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
VERSION
+ 1
− 1
View file @ 6a56ae57
310
\ No newline at end of file
311
\ No newline at end of file
plugins/tracking/SingleSource.m
+ 141
− 66
View file @ 6a56ae57
function [positions, clockError] = SingleSource(pData, pSoundsourcePositions, pCombinationDetails, pStartEndPosition)
%SINGLESOURCE Implements the single source tracking algorithm
%
% 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 only one sound source is selected
if length(soundsourceNames) ~= 1 || isempty(soundsourceNames)
error('Only one or at least one soundsource requires to be specified for SingleSource tracking!');
end
%% Get required data for single source tracking
pData.(soundsourceNames{1}).horizontalVelocity = ...
[ ...
NaN; ...
calculateHorizontalVelocity( ...
[pData.(soundsourceNames{1}).date{:}]', ...
[pData.(soundsourceNames{1}).distance{:}]' ...
) ...
];
averageHorizontalVelocity = mean( ...
pData.(soundsourceNames{1}).horizontalVelocity( ...
~isnan(pData.(soundsourceNames{1}).horizontalVelocity) ...
) ...
);
%% Return NaN because this is a sample
positions = NaN(size(pData.(soundsourceNames{1}).date, 1), 2);
clockError = [];
error('Tracking method implementation not finished yet!');
%% Helper functions
function [hvel] = calculateHorizontalVelocity(dates, distances)
% calculate the average speed in m/s
% distances are stored in km
% dates as rafos dates
diffDistances = diff(distances) * 1000; % now in [m]
diffDates = diff(dates) * 24 * 3600; % now in [s]
hvel = abs(diffDistances ./ diffDates);
end
end
\ No newline at end of file
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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment