Newer
Older
function [trajectory, trajectoryDates, trajectoryClockError, trajectoryResiduals, trajectoryVelocities, trajectoryTimeDivergenceToGps, trajectorySegmentSize, trajectoryVariationResults] = calculate(pFloatDetails, pPressureAndDate, pSatData, pToaData, pSoundsources, pTrackingParameter, pOffsetsParameter, pLeapsecondsMatrix, pOffsetVariations, pStartEndPosition)
%CALCULATETRAJECTORY Calculates the trajectory using the given details.
% Applies all required corrections to the data and calculates the trajectory
% segment by segment afterwards.
%
% The following corrections are applied for every soundsource individually:
% 1. Float drift is being added to TOA
% 2. Adds offsets and dates (sense is to be determined)
% 3. Corrective value of soundsource is being added
% 4. The TOA data is being interpolated
% 5. The TOA data is being matched to the next Rafos date
% 6. Doppler correction is applied if selected
%
% Parameters:
% pFloatDetails (struct):
% The float details that are stored in a .rfb file.
% pToaData (struct):
% The toaData object, according to artoa data specification.
% pSoundsources (struct):
% All soundsources that are available during the activity of the
% float.
% pTrackingParameter (struct):
% Tracking parameter according to artoa data specification.
% pLeapsecondsMatrix (double):
% The leapseconds matrix that is specified in the artoa.ini file.
% pOffsetVariations (double):
% The offsetvariations matrix, specified in artoa.ini file.
leprob001
committed
% pStartEndPosition (optional) (double):
% A matrix containing the start and end position as row vectors.
leprob001
committed
%% Parameter check
if ~exist('pStartEndPosition', 'var')
pStartEndPosition = false;
end
%% Get required data
soundsourceCombinations = pTrackingParameter.soundsourceCombinations;
uniqueToaDates = unique(pToaData.toaDate);
%% Get all sound sources that are involved
leprob001
committed
[involvedSoundsources, involvedSoundsourceNames] = ...
artoa.controller.track.getInvolvedSoundsources();
leprob001
committed
%% Initialize return variables
trajectory = false;
trajectoryDates = false;
trajectoryClockError = false;
trajectoryResiduals = false;
trajectoryTimeDivergenceToGps = false;
trajectoryVelocities = false;
trajectoryVariationResults = false;
leprob001
committed
%% Add drift and offsets to soundsources and float
soundsourceStruct = struct();
for i = 1:length(involvedSoundsourceNames)
soundsourceStruct.(involvedSoundsourceNames{i}) = involvedSoundsources{i};
end
if pOffsetsParameter.useOffsets
pToaData = artoa.toa.recalculate( ...
pFloatDetails, ...
pToaData, ...
soundsourceStruct ...
);
pToaData.toa = pToaData.toa - pToaData.empiricalShift;
leprob001
committed
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
%% Prepare data for every sound source
preparedData = struct();
for i = 1:length(involvedSoundsources)
currentSoundsource = involvedSoundsources{i};
preparedData.(currentSoundsource.sourcename) = struct();
% get toadata that is applied to soundsource and not deleted
currentToaSelection = ...
strcmp(pToaData.soundSource, {currentSoundsource.sourcename}) ...
& pToaData.status ~= 2;
if ~any(currentToaSelection)
return
end
% get toa and date of current sound source
currentToa = pToaData.toa(currentToaSelection);
currentToaDate = pToaData.toaDate(currentToaSelection);
% get rafos start and enddate of soundsource
rafosBeginEmission = artoa.convert.dmy2rd( ...
currentSoundsource.begemis(3), ...
currentSoundsource.begemis(2), ...
currentSoundsource.begemis(1) ...
);
rafosEndEmission = artoa.convert.dmy2rd( ...
currentSoundsource.endemis(3), ...
currentSoundsource.endemis(2), ...
currentSoundsource.endemis(1) ...
);
% get rafos date of float based on cycle (expecting only 1 cycle)
rafosFloatDateCycleBegin = artoa.convert.dmy2rd( ...
pFloatDetails.cycle(6), ...
pFloatDetails.cycle(5), ...
pFloatDetails.cycle(4) ...
);
rafosFloatDateCycleEnd = artoa.convert.dmy2rd( ...
pFloatDetails.cycle(13), ...
pFloatDetails.cycle(12), ...
pFloatDetails.cycle(11) ...
);
% % calculate soundsource offsets and dates
% [offsets, dates] = artoa.data.calculateOffsetsAndDates( ...
% [rafosFloatDateCycleBegin, rafosFloatDateCycleEnd], ...
% [rafosBeginEmission, rafosEndEmission], ...
% currentSoundsource.offset, ...
% currentSoundsource.drift, ...
% currentSoundsource.schedule ...
% );
%
% % check overlap between float and sound source deployment and add sound
% % source offset
% for oDates = 1:length(dates)
% kx = find(abs(currentToaDate - dates(oDates)) < 1/2);
% currentToa(kx) = currentToa(kx) - offsets(oDates);
% end
% interpolate toa data
[ ...
preparedData.(currentSoundsource.sourcename).toaDate, ...
preparedData.(currentSoundsource.sourcename).toa, ...
preparedData.(currentSoundsource.sourcename).toaIndex ...
] = artoa.data.interpolateRafosData( ...
currentToaDate, ...
currentToa, ...
pTrackingParameter.interpolationInterval, ...
pTrackingParameter.gapSize, ...
lower(pTrackingParameter.interpolationMethodString) ...
);
leprob001
committed
% % combine interpolated and original data and sort them
% preparedData.(currentSoundsource.sourcename).toaDate = ...
% [preparedData.(currentSoundsource.sourcename).toaDate; currentToaDate];
% preparedData.(currentSoundsource.sourcename).toa = ...
% [preparedData.(currentSoundsource.sourcename).toa; currentToa];
% Find the nearest neighbor rafos date
for oRafosDate = 1:length(preparedData.(currentSoundsource.sourcename).toaDate)
preparedData.(currentSoundsource.sourcename).toaDate(oRafosDate) = ...
artoa.data.findNearestNeighbor( ...
uniqueToaDates, ...
preparedData.(currentSoundsource.sourcename).toaDate(oRafosDate) ...
);
[preparedData.(currentSoundsource.sourcename).toaDate, sortIndices] = ...
sort(preparedData.(currentSoundsource.sourcename).toaDate);
preparedData.(currentSoundsource.sourcename).toa = ...
preparedData.(currentSoundsource.sourcename).toa(sortIndices);
[ ...
preparedData.(currentSoundsource.sourcename).toaDate, ...
preparedData.(currentSoundsource.sourcename).toa ...
leprob001
committed
] = artoa.toa.simulshift( ...
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
preparedData.(currentSoundsource.sourcename).toaDate, ...
preparedData.(currentSoundsource.sourcename).toa, ...
pFloatDetails.schedule, ...
pFloatDetails.windowstart, ...
currentSoundsource.reftime, ...
pFloatDetails.phasereftime ...
);
% save position
preparedData.(currentSoundsource.sourcename).position = currentSoundsource.position;
% save float window start
preparedData.(currentSoundsource.sourcename).floatWindowStart = pFloatDetails.windowstart;
% save soundsource reference time
preparedData.(currentSoundsource.sourcename).soundsourceReferenceTime = currentSoundsource.reftime;
% get leapseconds for soundsource
preparedData.(currentSoundsource.sourcename).leapseconds = artoa.data.calculateLeapseconds( ...
artoa.convert.dmy2rd(pFloatDetails.launchtime(3), pFloatDetails.launchtime(2), pFloatDetails.launchtime(1)), ...
artoa.convert.dmy2rd(currentSoundsource.begemis(3), currentSoundsource.begemis(2), currentSoundsource.begemis(1)), ...
pLeapsecondsMatrix ...
);
if isempty(preparedData.(currentSoundsource.sourcename).leapseconds)
preparedData.(currentSoundsource.sourcename).leapseconds = 0;
end
if ~pTrackingParameter.dopplerCorrection
continue
end
% apply doppler correction if selected
[preparedData.(currentSoundsource.sourcename).toaDate, preparedData.(currentSoundsource.sourcename).toa] = ...
leprob001
committed
artoa.toa.doppcorr( ...
preparedData.(currentSoundsource.sourcename).toaDate, ...
preparedData.(currentSoundsource.sourcename).toa, ...
pFloatDetails.schedule ...
);
end
%% Get static data
floatReferenceTime = pFloatDetails.phasereftime;
trajectory = [];
trajectoryDates = [];
trajectoryClockError = [];
trajectorySegmentSize = [];
trajectoryVariationResults = {};
leprob001
committed
trajectoryTimeDivergenceToGps = struct();
trajectoryTimeDivergenceToGps.latitude = [];
trajectoryTimeDivergenceToGps.longitude = [];
trajectoryTimeDivergenceToGps.date = [];
for i = 1:length(involvedSoundsourceNames)
trajectoryResiduals.(involvedSoundsourceNames{i}) = [];
leprob001
committed
trajectoryTimeDivergenceToGps.(involvedSoundsourceNames{i}) = [];
end
trajectoryVelocities = struct();
trajectoryVelocities.horizontalTotal = [];
trajectoryVelocities.horizontalLongitude = [];
trajectoryVelocities.horizontalLatitude = [];
trajectoryVelocities.vertical = [];
trajectoryVelocities.verticalDates = [];
leprob001
committed
% trajectoryTimeDivergenceToGps = struct();
% trajectoryTimeDivergenceToGps.duration = [];
% trajectoryTimeDivergenceToGps.date = [];
%% Calculate for every combination
for oCombination = 1:size(soundsourceCombinations, 1)
leprob001
committed
% set reference point to the last position of trajectory if not set
% manually
currentCombination = soundsourceCombinations(oCombination, :);
leprob001
committed
&& isempty(trajectory) ...
&& islogical(pStartEndPosition)
leprob001
committed
end
[ ...
segmentPositions, ...
segmentDates, ...
segmentClockError, ...
segmentResiduals ...
] = artoa.trajectory.calculateCombinationSegment( ...
preparedData, ...
currentCombination, ...
floatReferenceTime, ...
pOffsetsParameter.offsets(:, 'AppliedSoundspeed'), ...
leprob001
committed
currentCombination.backwardTracking(1), ...
pStartEndPosition ...
leprob001
committed
% remove NaNs from segment
validPositions = all(~isnan(segmentPositions), 2);
segmentPositions = segmentPositions(validPositions, :);
segmentDates = segmentDates(validPositions);
segmentClockError = segmentClockError(validPositions);
segmentResiduals = structfun(@(x) x(validPositions), segmentResiduals, 'UniformOutput', false);% segmentResiduals(validPositions);
if islogical(segmentPositions) && segmentPositions == false
trajectory = false;
trajectoryDates = false;
trajectoryClockError = false;
trajectoryDates = [trajectoryDates; segmentDates];
if size(segmentClockError, 2) == 2 && size(trajectoryClockError, 2) == 3
segmentClockError(:, 3) = NaN(size(segmentClockError, 1), 1);
end
trajectoryClockError = [trajectoryClockError; segmentClockError];
trajectorySegmentSize = [trajectorySegmentSize; size(segmentPositions, 1)];
% store residuals
for oInvolvedSoundsources = 1:length(involvedSoundsourceNames)
if isfield(segmentResiduals, involvedSoundsourceNames{oInvolvedSoundsources})
trajectoryResiduals.(involvedSoundsourceNames{oInvolvedSoundsources}) = [ ...
trajectoryResiduals.(involvedSoundsourceNames{oInvolvedSoundsources}); ...
segmentResiduals.(involvedSoundsourceNames{oInvolvedSoundsources}) ...
];
else
trajectoryResiduals.(involvedSoundsourceNames{oInvolvedSoundsources}) = [ ...
trajectoryResiduals.(involvedSoundsourceNames{oInvolvedSoundsources}); ...
NaN(size(segmentPositions, 1), 1) ...
];
end
end
% CALCULATE VARIATIONS
leprob001
committed
if length(strsplit(strtrim(currentCombination.soundsources{1}))) == 2 ...
&& ~artoa.trajectory.isInterpolationMethod(currentCombination.trackingMethod{1}) ...
&& pTrackingParameter.calculateVariations
[variationResults, ~, ~] = artoa.trajectory.calculateByOffsetVariations( ...
preparedData, ...
currentCombination, ...
floatReferenceTime, ...
pOffsetsParameter.offsets(:, 'AppliedSoundspeed'), ...
pTrackingParameter, ...
pOffsetVariations ...
);
trajectoryVariationResults = [trajectoryVariationResults, variationResults];
end
leprob001
committed
[ ...
segmentHorizontalVelocityTotal, ...
segmentHorizontalVelocityLatitude, ...
segmentHorizontalVelocityLongitude ...
leprob001
committed
] = artoa.trajectory.horizontalVelocity( ...
segmentPositions(:, 1), ...
segmentPositions(:, 2), ...
pTrackingParameter.interpolationInterval / 24 * 86400 ...
);
trajectoryVelocities.horizontalTotal = [trajectoryVelocities.horizontalTotal; segmentHorizontalVelocityTotal];
trajectoryVelocities.horizontalLatitude = [trajectoryVelocities.horizontalLatitude; segmentHorizontalVelocityLatitude];
trajectoryVelocities.horizontalLongitude = [trajectoryVelocities.horizontalLongitude; segmentHorizontalVelocityLongitude];
% calculate vertical velocities
% get intersecting dates
%verticalVelocityDates = NaN(size(segmentDates));
[verticalVelocityDates, ~, indicesDates] = intersect(pPressureAndDate{2}, segmentDates);
%verticalVelocityDates(indicesDates) = segmentDates(indicesDates);
leprob001
committed
[segmentVerticalVelocity, segmentVerticalDates] = artoa.trajectory.verticalVelocity( ...
verticalVelocityDates, ...
pPressureAndDate{1}(ismember(pPressureAndDate{2}, verticalVelocityDates)), ...
pTrackingParameter.interpolationInterval / 24 * 86400, ...
'average' ...
);
trajectoryVelocities.vertical = [trajectoryVelocities.vertical; segmentVerticalVelocity];
trajectoryVelocities.verticalDates = [trajectoryVelocities.verticalDates; segmentVerticalDates];
leprob001
committed
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
if isempty(segmentDates)
continue;
end
% Calculate time divergence to gps
% interpolate the trajectory linearly to have a position every day
[ ...
segmentInterpolatedDates, ...
segmentInterpolatedLat ...
] = artoa.data.interpolateRafosData( ...
segmentDates, ...
segmentPositions(:, 1), ...
24, ...
9999, ...
'linear' ...
);
[ ...
~, ...
segmentInterpolatedLon ...
] = artoa.data.interpolateRafosData( ...
segmentDates, ...
segmentPositions(:, 2), ...
24, ...
9999, ...
'linear' ...
);
segmentInterpolatedPositions = [ segmentInterpolatedLat, segmentInterpolatedLon];
% get all sat dates and positions
leprob001
committed
satDates = artoa.convert.dmy2rd(pSatData.day_sat, pSatData.month_sat, pSatData.year_sat);
satPositions = [pSatData.lat_sat(:), pSatData.lon_sat(:)];
tmpIndex = ~isnan(satDates) & ~any(isnan(satPositions), 2);
% remove nans
satDates = satDates(tmpIndex);
satPositions = satPositions(tmpIndex, :);
clear tmpIndex;
leprob001
committed
% get all dates that intersect with segment
[~, segmentDatesIndex, segmentSatIndex] = intersect(floor(segmentInterpolatedDates), satDates);
intersectedSegmentPositions = segmentInterpolatedPositions(segmentDatesIndex, :);
intersectedSegmentDates = floor(segmentInterpolatedDates(segmentDatesIndex, :));
segmentSatPositions = satPositions(segmentSatIndex, :);
leprob001
committed
% get all soundsources of segment
leprob001
committed
segmentSoundsources = artoa.soundsources.extract( ...
leprob001
committed
strsplit(soundsourceCombinations.soundsources{oCombination}), ...
pSoundsources ...
);
for oSoundsource = 1:length(segmentSoundsources)
currentSoundsource = segmentSoundsources{oSoundsource};
% calculate divergence for soundsource position
[soundsourceDivergence, ~] = artoa.data.calculateTimeDivergence( ...
repmat(currentSoundsource.position, size(intersectedSegmentPositions, 1), 1), ...
intersectedSegmentPositions, ...
pOffsetsParameter.offsets{currentSoundsource.sourcename, 'AppliedSoundspeed'} ...
leprob001
committed
);
[gpsDivergence, usedIndices] = artoa.data.calculateTimeDivergence( ...
repmat(currentSoundsource.position, size(segmentSatPositions, 1), 1), ...
segmentSatPositions, ...
pOffsetsParameter.offsets{currentSoundsource.sourcename, 'AppliedSoundspeed'} ...
leprob001
committed
);
if isempty(usedIndices) || isempty(soundsourceDivergence) || isempty(gpsDivergence)
continue;
end
difference = (soundsourceDivergence(usedIndices) - gpsDivergence(:));
leprob001
committed
trajectoryTimeDivergenceToGps.(currentSoundsource.sourcename) = [ ...
trajectoryTimeDivergenceToGps.(currentSoundsource.sourcename);
intersectedSegmentDates(usedIndices), difference
leprob001
committed
];
end
% save positions and dates that have been interpolated and used for time divergence
trajectoryTimeDivergenceToGps.latitude = [trajectoryTimeDivergenceToGps.latitude; intersectedSegmentPositions(:, 1)];
trajectoryTimeDivergenceToGps.longitude = [trajectoryTimeDivergenceToGps.longitude; intersectedSegmentPositions(:, 2)];
trajectoryTimeDivergenceToGps.date = [trajectoryTimeDivergenceToGps.date; intersectedSegmentDates];