Newer
Older
function [segmentPositions, segmentDates, segmentClockError, segmentResiduals] = calculateCombinationSegment(pCorrectedData, pCombinationDetails, pFloatReferenceTime, pSoundVelocity, pTrackingMethod)
%CALCULATECOMBINATIONSEGMENT Combination segment calculation.
% Using the input data, the function calculates the segment positions,
% its corresponding dates as well as the clock error (in case of hyperbolic calculation)
%
% Parameters:
% pCorrectedData (struct):
% Data structured by soundsource. The toa is already corrected
% (e.g. doppler correction etc.)
% pCombinationDetails (table):
% The combination table that is produced by the track parameter
% window.
% pFloatReferenceTime (double):
% Vector containing the float reference time, in the
% format [hour, minute]
% pSoundVelocity (double):
% Sound velocity next to the sound sources that are used.
% If hyperbolic is being used, it needs to be a vector containing
% at least as many entries as the count of sound sources being
% used.
% For all other methods, only one value needs to be set.
% Unit: [m/s]
% pTrackingMethod (string/char):
% Contains the tracking algorithm that should be used.
% Available methods:
% least square
% exclusive least square
% circular
% hyperbolic
%
%
% Returns:
% segmentPositions (double):
% The calculated list of segment positions as a column vector.
% segmentDates (double):
% The corresponding rafos dates of the segment positions.
% clockError (double):
% The estimated clock error.
% residuals (table):
% The calculated residuals corresponding to segmentDates for
% every soundsource.
%% Initialize return variables
segmentPositions = [];
segmentDates = [];
segmentClockError = [];
segmentVelocities = struct();
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
%% Get sound sources
soundsourceNames = unique(strsplit(pCombinationDetails.soundsources{1}, ' '));
%% Get tracking method
trackingMethod = lower(pTrackingMethod);
%% Get start and end point
segmentStart = pCombinationDetails{1, 1};
segmentEnd = pCombinationDetails{1, 2};
%% Get reference position
referencePosition = cellfun(@str2double, strsplit(pCombinationDetails.referencePosition{1}));
%% Filter toa data
intersectedToaDates = pCorrectedData.(soundsourceNames{1}).toaDate;
soundsourcePositions = [pCorrectedData.(soundsourceNames{1}).position];
for i = 2:length(soundsourceNames)
[intersectedToaDates] = intersect(intersectedToaDates, pCorrectedData.(soundsourceNames{i}).toaDate);
soundsourcePositions = [soundsourcePositions; pCorrectedData.(soundsourceNames{i}).position];
end
%% Remove all dates that are out of bounds
intersectedToaDates = intersectedToaDates( ...
intersectedToaDates >= segmentStart ...
& intersectedToaDates <= segmentEnd ...
);
distanceToSoundsources = {};
%segmentPositions = [pStartposition];
segmentPositions = referencePosition;
for oDates = 1:length(intersectedToaDates)
currentDateValue = intersectedToaDates(oDates);
distanceToSoundsources{oDates} = [];
for i = 1:length(soundsourceNames)
currentName = soundsourceNames{i};
selection = pCorrectedData.(currentName).toaDate == currentDateValue;
filteredValue = pCorrectedData.(currentName).toa(selection);
%filteredData.(currentName).toa(oDates) = filteredValue(1);
%filteredValue = pCorrectedData.(currentName).toaDate(selection);
%filteredData.(currentName).toaDate(oDates) = filteredValue(1);
% relativeToa = ( ...
% filteredValue(1) ...
% + (pFloatReferenceTime(1) * 60 + pFloatReferenceTime(2)) * 60 ...
% - (pCorrectedData.(currentName).soundsourceReferenceTime(1) * 3600 + pCorrectedData.(currentName).soundsourceReferenceTime(2) * 60 - pCorrectedData.(currentName).leapseconds) ...
% );
windowStartTime = ...
pFloatReferenceTime(1) * 3600 ...
+ pFloatReferenceTime(2) * 60 ...
- pCorrectedData.(currentName).soundsourceReferenceTime(1) * 3600 ...
- pCorrectedData.(currentName).soundsourceReferenceTime(2) * 60;
leapseconds = pCorrectedData.(currentName).leapseconds;
floatWindowStart = pCorrectedData.(currentName).floatWindowStart * 60;
relativeToa = filteredValue(1) ...
+ windowStartTime ...
+ leapseconds ...
- floatWindowStart;
% calculate distance to the source
distanceToSoundsources{oDates} = [ ...
distanceToSoundsources{oDates}; ...
relativeToa * (pSoundVelocity(i)/1000)
];
% distanceToSoundsources{oDates} = [ ...
% distanceToSoundsources{oDates}; ...
% (...
% filteredValue(1) ... %filteredData.(currentName).toa(oDates) ...
% + (pFloatReferenceTime(1) * 60 + pFloatReferenceTime(2)) * 60 ...
% - (pCorrectedData.(currentName).soundsourceReferenceTime(1) * 3600 + pCorrectedData.(currentName).soundsourceReferenceTime(2) * 60 - pCorrectedData.(currentName).leapseconds) ...
% ) * (pSoundVelocity/1000)
% ];
end
if strcmp(trackingMethod, 'hyperbolic') && (length(soundsourceNames) < 3)
trackingMethod = 'circular';
end
switch trackingMethod
case {'least square', 'exclusive least square'}
for oDates = 1:length(intersectedToaDates)
if strcmp(pTrackingMethod, 'exclusive least square') ...
&& (sum(~isnan(distanceToSoundsources{oDates})) ~= length(soundsourcePositions))
segmentPositions = [segmentPositions; [NaN, NaN]];
segmentClockError = [segmentClockError; NaN(1, length(soundsourceNames))];
continue;
end
segmentPositions = [segmentPositions; ...
rad2deg(artoa.vendor.ls_converge(segmentPositions(end, :), length(soundsourceNames), soundsourcePositions, distanceToSoundsources{oDates}, pSoundVelocity(1)/1000)) ...
];
segmentClockError = [segmentClockError; NaN(1, length(soundsourceNames))];
end
case 'circular'
for oDates = 1:length(intersectedToaDates)
distances = distanceToSoundsources{oDates};
segmentPositions = [segmentPositions; ...
rad2deg(artoa.vendor.xnavai(deg2rad(soundsourcePositions), distances, deg2rad(referencePosition), 2)) ...
];
segmentClockError = [segmentClockError; NaN(1, length(soundsourceNames))];
end
case 'hyperbolic'
for oDates = 1:length(intersectedToaDates)
distances = distanceToSoundsources{oDates};
if any(isnan(distances))
segmentPositions = [segmentPositions; [NaN NaN]];
segmentClockError = [segmentClockError; NaN(1, length(soundsourceNames))];
continue;
end
segmentPositions = [segmentPositions; ...
rad2deg(artoa.vendor.xnavai(deg2rad(soundsourcePositions), distances, deg2rad(referencePosition), 3)) ...
];
currentClockError = NaN(1, 3);
for oClockError = 1:3
currentClockError(oClockError) = ...
( ...
artoa.data.calculateEllipk4(segmentPositions(end, :), ...
deg2rad(soundsourcePositions(oClockError,:))) - distances(oClockError) ...
) / (pSoundVelocity(oClockError)/1000);
end
segmentClockError = [segmentClockError; currentClockError];
end
% if length(unique(sosonr)) < 3; % check if 3 sources have been specified in track panel
% fprintf(1,'Hyperbolic tracking not possible: only 2 sound sources specified, using circular tracking\r');
% pnt=xnavai(sosopos/RAD,dist,refpos/RAD,2); % use circular by default
% else
% if (isnan(dist(1)) | isnan(dist(2)) | isnan(dist(3))); % check if data for all 3 sources is availble
% tpnt=[tpnt;[NaN,NaN]];
% else
% pnt=xnavai(sosopos/RAD,dist,refpos/RAD,3);
% clkerror(1) = (artoa.data.calculateEllipk4(pnt,sosopos(1,:)/RAD)-dist(1))/svsoso(1);
% clkerror(2) = (artoa.data.calculateEllipk4(pnt,sosopos(2,:)/RAD)-dist(2))/svsoso(2);
% clkerror(3) = (artoa.data.calculateEllipk4(pnt,sosopos(3,:)/RAD)-dist(3))/svsoso(3);
% end
% end
end
%% Store toa dates
segmentDates = intersectedToaDates;
%% Remove reference position from trajectory
segmentPositions = segmentPositions(2:end, :);
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
%% Calculate residuals
% initalize variables
segmentResiduals = struct();
for i = 1:length(soundsourceNames)
segmentResiduals.(soundsourceNames{i}) = [];
end
% calculate residuals for every timestep
for oDates = 1:length(intersectedToaDates)
[tmpResiduals, ~] = artoa.float.calculateResiduals( ...
segmentPositions(1, :), ...
distanceToSoundsources{oDates}, ...
soundsourcePositions, ...
pSoundVelocity ...
);
% split up for storing it as table
for i = 1:length(soundsourceNames)
segmentResiduals.(soundsourceNames{i}) = [ ...
segmentResiduals.(soundsourceNames{i}); ...
tmpResiduals(i) ...
];
end
end