calculate.m 17.18 KiB
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.
% pStartEndPosition (optional) (double):
% A matrix containing the start and end position as row vectors.
%% 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
[involvedSoundsources, involvedSoundsourceNames] = ...
artoa.controller.track.getInvolvedSoundsources();
%% Initialize return variables
trajectory = false;
trajectoryDates = false;
trajectoryClockError = false;
trajectoryResiduals = false;
trajectoryTimeDivergenceToGps = false;
trajectoryVelocities = false;
trajectoryVariationResults = false;
%% 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, ...
pOffsetsParameter.offsets, ...
soundsourceStruct ...
);
end
%% 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) ...
);
% % 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) ...
);
end
% sort toa by date
[preparedData.(currentSoundsource.sourcename).toaDate, sortIndices] = ...
sort(preparedData.(currentSoundsource.sourcename).toaDate);
preparedData.(currentSoundsource.sourcename).toa = ...
preparedData.(currentSoundsource.sourcename).toa(sortIndices);
% shift toa data
[ ...
preparedData.(currentSoundsource.sourcename).toaDate, ...
preparedData.(currentSoundsource.sourcename).toa ...
] = artoa.toa.simulshift( ...
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] = ...
artoa.toa.doppcorr( ...
preparedData.(currentSoundsource.sourcename).toaDate, ...
preparedData.(currentSoundsource.sourcename).toa, ...
pFloatDetails.schedule ...
);
end
%% Get static data
floatReferenceTime = pFloatDetails.phasereftime;
trajectory = [];
trajectoryDates = [];
trajectoryClockError = [];
trajectorySegmentSize = [];
trajectoryResiduals = struct();trajectoryVariationResults = {};
trajectoryTimeDivergenceToGps = struct();
trajectoryTimeDivergenceToGps.latitude = [];
trajectoryTimeDivergenceToGps.longitude = [];
trajectoryTimeDivergenceToGps.date = [];
for i = 1:length(involvedSoundsourceNames)
trajectoryResiduals.(involvedSoundsourceNames{i}) = [];
trajectoryTimeDivergenceToGps.(involvedSoundsourceNames{i}) = [];
end
trajectoryVelocities = struct();
trajectoryVelocities.horizontalTotal = [];
trajectoryVelocities.horizontalLongitude = [];
trajectoryVelocities.horizontalLatitude = [];
trajectoryVelocities.vertical = [];
trajectoryVelocities.verticalDates = [];
% trajectoryTimeDivergenceToGps = struct();
% trajectoryTimeDivergenceToGps.duration = [];
% trajectoryTimeDivergenceToGps.date = [];
%% Calculate for every combination
for oCombination = 1:size(soundsourceCombinations, 1)
% set reference point to the last position of trajectory if not set
% manually
currentCombination = soundsourceCombinations(oCombination, :);
if isempty(currentCombination.referencePosition{1}) ...
&& isempty(trajectory) ...
&& islogical(pStartEndPosition)
return;
end
[ ...
segmentPositions, ...
segmentDates, ...
segmentClockError, ...
segmentResiduals ...
] = artoa.trajectory.calculateCombinationSegment( ...
preparedData, ...
currentCombination, ...
floatReferenceTime, ...
pOffsetsParameter.offsets(:, 'AppliedSoundspeed'), ...
currentCombination.backwardTracking(1), ...
pStartEndPosition ...
);
% 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;
trajectoryResiduals = false;
return
end
trajectory = [ ...
trajectory; ...
segmentPositions ...
];
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
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
% calculate horizontal velocities
[ ...
segmentHorizontalVelocityTotal, ...
segmentHorizontalVelocityLatitude, ...
segmentHorizontalVelocityLongitude ...
] = 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);
[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];
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
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;
% get all dates that intersect with segment
[~, segmentDatesIndex, segmentSatIndex] = intersect(floor(segmentInterpolatedDates), satDates);
intersectedSegmentPositions = segmentInterpolatedPositions(segmentDatesIndex, :);
intersectedSegmentDates = floor(segmentInterpolatedDates(segmentDatesIndex, :));
segmentSatPositions = satPositions(segmentSatIndex, :);
% get all soundsources of segment
segmentSoundsources = artoa.soundsources.extract( ...
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'} ...
);
[gpsDivergence, usedIndices] = artoa.data.calculateTimeDivergence( ...
repmat(currentSoundsource.position, size(segmentSatPositions, 1), 1), ...
segmentSatPositions, ...
pOffsetsParameter.offsets{currentSoundsource.sourcename, 'AppliedSoundspeed'} ...
);
if isempty(usedIndices) || isempty(soundsourceDivergence) || isempty(gpsDivergence)
continue;
end
difference = (soundsourceDivergence(usedIndices) - gpsDivergence(:));
trajectoryTimeDivergenceToGps.(currentSoundsource.sourcename) = [ ...
trajectoryTimeDivergenceToGps.(currentSoundsource.sourcename);
intersectedSegmentDates(usedIndices), difference
];
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];
end
end