Select Git revision
calculate.m
calculate.m 15.40 KiB
function [trajectory, trajectoryDates, trajectoryClockError, trajectoryResiduals, trajectoryVelocities, trajectoryTimeDivergenceToGps] = calculate(pFloatDetails, pPressureAndDate, pSatData, pToaData, pSoundsources, pTrackingParameter, pSoundVelocity, pLeapsecondsMatrix)
%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.
% pSoundVelocity (double):
% x by 3 vector, containing the soundspeed values corresponding
% to the specified soundsources in the tracking parameter ->
% soundsource combination table.
% pLeapsecondsMatrix (double):
% The leapseconds matrix that is specified in the artoa.ini file.
%% 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;
%% 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 = [];
trajectoryResiduals = struct();
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})
currentCombination.referencePosition{1} = num2str(trajectory(end, :));
end
[segmentPositions, segmentDates, segmentClockError, segmentResiduals] = artoa.trajectory.calculateCombinationSegment( ...
preparedData, ...
currentCombination, ...%trajectory(end, :), ...
floatReferenceTime, ...
pSoundVelocity(oCombination, :), ...
pTrackingParameter.trackingMethodString ...
);
% 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];
% 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 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 = intersect(pPressureAndDate{2}, segmentDates); [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 = round(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, ...
pSoundVelocity(oCombination, oSoundsource) ...
);
[gpsDivergence, usedIndices] = artoa.data.calculateTimeDivergence( ...
repmat(currentSoundsource.position, size(segmentSatPositions, 1), 1), ...
segmentSatPositions, ...
pSoundVelocity(oCombination, oSoundsource) ...
);
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; floor(intersectedSegmentDates)];
end
end