From 62c7359540e86eaa795ed61a92d5c3b70e078d6b Mon Sep 17 00:00:00 2001
From: Lewin Probst <info@emirror.de>
Date: Fri, 6 Dec 2019 13:58:04 +0100
Subject: [PATCH] Updated offset handling and trajectory calculation.

---
 VERSION                                       |   2 +-
 .../+edit/+offsets/buttonCalculateOffsets.m   |  18 ++-
 .../+edit/+offsets/checkboxUseOffsets.m       |  25 +--
 .../+edit/+offsets/selectOffsetsAndDrift.m    |  69 --------
 .../+offsets/tableSoundsourceOffsetsEdit.m    |  30 +---
 .../+controller/+edit/+offsets/updateGui.m    |   2 +-
 .../+edit/+timeOfArrival/calculateGpsToas.m   |   9 +-
 .../+edit/recalculateToaGpsAndPlot.m          |  33 ++++
 lib/+artoa/+controller/+track/run.m           |   7 +-
 .../+offsets/createCalculationMatrices.m      | 132 +++++++++++++++
 .../+offsets/extractOffsetsDriftsFromSolved.m |   7 +-
 .../+offsets/filterCalculationMatrices.m      |  25 +++
 lib/+artoa/+offsets/solve.m                   | 152 ++----------------
 lib/+artoa/+toa/recalculate.m                 |  12 +-
 lib/+artoa/+trajectory/calculate.m            |  46 ++++--
 15 files changed, 282 insertions(+), 287 deletions(-)
 delete mode 100644 lib/+artoa/+controller/+edit/+offsets/selectOffsetsAndDrift.m
 create mode 100644 lib/+artoa/+controller/+edit/recalculateToaGpsAndPlot.m
 create mode 100644 lib/+artoa/+offsets/createCalculationMatrices.m
 create mode 100644 lib/+artoa/+offsets/filterCalculationMatrices.m

diff --git a/VERSION b/VERSION
index d257208..95c8a67 100644
--- a/VERSION
+++ b/VERSION
@@ -1 +1 @@
-112
\ No newline at end of file
+113
\ No newline at end of file
diff --git a/lib/+artoa/+controller/+edit/+offsets/buttonCalculateOffsets.m b/lib/+artoa/+controller/+edit/+offsets/buttonCalculateOffsets.m
index b7a8c59..1d4102e 100644
--- a/lib/+artoa/+controller/+edit/+offsets/buttonCalculateOffsets.m
+++ b/lib/+artoa/+controller/+edit/+offsets/buttonCalculateOffsets.m
@@ -13,7 +13,9 @@ satData = artoaWorkspace.satData;
 [
     artoaWorkspace.editOffsets.calculatedMatrixA, ...
     artoaWorkspace.editOffsets.calculatedMatrixB, ...
-    artoaWorkspace.editOffsets.calculatedMatrixX ...
+    artoaWorkspace.editOffsets.calculatedMatrixX, ...
+    ~, ...
+    ~ ...
 ] = artoa.offsets.solve( ...
     artoaDataInput.rfb, ...
     soundsources, ...
@@ -28,20 +30,22 @@ satData = artoaWorkspace.satData;
 %% Store offsets in table
 [offsets, drifts] = artoa.offsets.extractOffsetsDriftsFromSolved(artoaWorkspace.editOffsets.calculatedMatrixX);
 
-soundsourceNames = fieldnames(soundsources);
+soundsourceNames = fieldnames(artoa.controller.getSoundsourcesWithAppliedToa());
 for o = 1:length(soundsourceNames)
     if ~any(contains(artoaWorkspace.editOffsets.soundsourceOffsets.Properties.RowNames, soundsourceNames{o}))
         continue;
     end
-    artoaWorkspace.editOffsets.soundsourceOffsets{soundsourceNames{o}, 'OptimumTotalOffset'} = offsets(o);
-    artoaWorkspace.editOffsets.soundsourceOffsets{soundsourceNames{o}, 'OptimumTotalDrift'} = drifts(o);
+    artoaWorkspace.editOffsets.soundsourceOffsets{soundsourceNames{o}, 'OptimumTotalOffset'} = offsets(o + 1);
+    artoaWorkspace.editOffsets.soundsourceOffsets{soundsourceNames{o}, 'OptimumTotalDrift'} = drifts(o + 1);
 end
 
-artoa.controller.edit.offsets.tableSoundsourceOffsetsEdit();
+% store float in table
+artoaWorkspace.editOffsets.soundsourceOffsets{'Float', 'OptimumTotalOffset'} = offsets(1);
+artoaWorkspace.editOffsets.soundsourceOffsets{'Float', 'OptimumTotalDrift'} = drifts(1);
 
+%% Recalculate
+artoa.controller.edit.recalculateToaGpsAndPlot();
 
 
-artoa.controller.edit.timeOfArrival.plot();
-
 end
 
diff --git a/lib/+artoa/+controller/+edit/+offsets/checkboxUseOffsets.m b/lib/+artoa/+controller/+edit/+offsets/checkboxUseOffsets.m
index f040e60..245fab9 100644
--- a/lib/+artoa/+controller/+edit/+offsets/checkboxUseOffsets.m
+++ b/lib/+artoa/+controller/+edit/+offsets/checkboxUseOffsets.m
@@ -2,33 +2,14 @@ function [newValue] = checkboxUseOffsets(~, ~)
 %CHECKBOXDOPPLERCORRECTION Summary of this function goes here
 %   Detailed explanation goes here
 
-global artoaGui artoaWorkspace artoaDataInput;
+global artoaGui artoaWorkspace;
 
 newValue = logical(artoaGui.editOffsets.checkboxUseOffsets.Value);
 
 artoaWorkspace.editOffsets.useOffsets = newValue;
 
-%% Update TOA
-
-% combine initial toa and current applied soundsources
-toaData = artoaWorkspace.toaData;
-toaData.toa = artoaDataInput.toaData.toa;
-
-%% Recalculate drift
-artoa.controller.edit.offsets.updateWorkspaceOffsetsTable();
-artoa.controller.edit.offsets.updateGui();
-
-%% Select offset and drift from the table
-artoa.controller.edit.offsets.selectOffsetsAndDrift();
-
-artoaWorkspace.toaData = artoa.toa.recalculate( ...
-    artoaWorkspace.float, ...
-    toaData, ...
-    artoaWorkspace.editOffsets.selectedOffsets ...
-);
-
-artoa.controller.edit.timeOfArrival.plot();
-
+%% Recalculate
+artoa.controller.edit.recalculateToaGpsAndPlot();
 
 end
 
diff --git a/lib/+artoa/+controller/+edit/+offsets/selectOffsetsAndDrift.m b/lib/+artoa/+controller/+edit/+offsets/selectOffsetsAndDrift.m
deleted file mode 100644
index 63c9329..0000000
--- a/lib/+artoa/+controller/+edit/+offsets/selectOffsetsAndDrift.m
+++ /dev/null
@@ -1,69 +0,0 @@
-function [] = selectOffsetsAndDrift()
-%SELECTOFFSETSANDDRIFT Summary of this function goes here
-%   Detailed explanation goes here
-
-global artoaWorkspace;
-
-%% Initialize required variables
-selectedOffsets = table();
-
-%% Get required variables
-offsetsTable = artoaWorkspace.editOffsets.soundsourceOffsets;
-
-%% Select float offset and drift
-if artoaWorkspace.editOffsets.useOffsets
-    tmp = table();
-    tmp.offset = offsetsTable{'Float', 'OptimumTotalOffset'};
-    tmp.drift = offsetsTable{'Float', 'OptimumTotalDrift'};
-    selectedOffsets('Float', :) = tmp;
-    clear tmp;
-else
-    selectedOffsets('Float', :) = selectFromRow(offsetsTable('Float', :));
-end
-
-%% Run through all soundsources
-variableNames = offsetsTable.Properties.RowNames;
-for i = 1:length(variableNames)
-    if strcmp('Float', variableNames{i})
-        continue;
-    end
-    if artoaWorkspace.editOffsets.useOffsets
-        tmp = table();
-        tmp.offset = offsetsTable{variableNames{i}, 'OptimumTotalOffset'};
-        tmp.drift = offsetsTable{variableNames{i}, 'OptimumTotalDrift'};
-        selectedOffsets(variableNames{i}, :) = tmp;
-        clear tmp;
-    else
-        selectedOffsets(variableNames{i}, :) = selectFromRow(offsetsTable(variableNames{i}, :));
-    end
-end
-
-%% Store selected values
-artoaWorkspace.editOffsets.selectedOffsets = selectedOffsets;
-
-    function [selected] = selectFromRow(pTableRow)
-        % offset
-        offset = 0;
-        if ~isnan(pTableRow.EmpiricalOffset)
-            offset = pTableRow.EmpiricalOffset;
-        elseif ~isnan(pTableRow.OffsetStart)
-            offset = pTableRow.OffsetStart;
-        elseif ~isnan(pTableRow.OptimumTotalOffset)
-            offset = pTableRow.OptimumTotalOffset;
-        end
-        % drift
-        drift = 0;
-        if ~isnan(pTableRow.EmpiricalDrift)
-            drift = pTableRow.EmpiricalDrift;
-        elseif ~isnan(pTableRow.Drift)
-            drift = pTableRow.Drift;
-        elseif ~isnan(pTableRow.OptimumTotalDrift)
-            drift = pTableRow.OptimumTotalDrift;
-        end
-        selected = table();
-        selected.offset = offset;
-        selected.drift = drift;
-    end
-
-end
-
diff --git a/lib/+artoa/+controller/+edit/+offsets/tableSoundsourceOffsetsEdit.m b/lib/+artoa/+controller/+edit/+offsets/tableSoundsourceOffsetsEdit.m
index e27bfec..00cc304 100644
--- a/lib/+artoa/+controller/+edit/+offsets/tableSoundsourceOffsetsEdit.m
+++ b/lib/+artoa/+controller/+edit/+offsets/tableSoundsourceOffsetsEdit.m
@@ -3,15 +3,15 @@ function [] = tableSoundsourceOffsetsEdit(~, event)
 %   Detailed explanation goes here
 
 
-global artoaGui artoaWorkspace artoaDataInput;
+global artoaGui artoaWorkspace;
 
 %% Get data
 try
     selectedColumn = event.Indices(2);
+    %selectedRow = event.Indices(:, 1);
 catch
     selectedColumn = NaN;
-    artoa.controller.edit.offsets.updateGui();
-    return;
+    %return;
 end
 
 %% Check if the column that has been edited is locked
@@ -27,28 +27,10 @@ end
 %% Save to workspace
 artoaWorkspace.editOffsets.soundsourceOffsets(:, :) = artoaGui.editOffsets.tableSoundsourceOffsets.Data;
 
-%% Recalculate drift
+%% Update workspace (required because this function is call by time of arrival window)
 artoa.controller.edit.offsets.updateWorkspaceOffsetsTable();
 
-%% Select offset and drift from the table
-artoa.controller.edit.offsets.selectOffsetsAndDrift();
-% combine initial toa and current applied soundsources
-toaData = artoaWorkspace.toaData;
-toaData.toa = artoaDataInput.toaData.toa;
-
-artoaWorkspace.toaData = artoa.toa.recalculate( ...
-    artoaWorkspace.float, ...
-    toaData, ...
-    artoaWorkspace.editOffsets.selectedOffsets ...
-);
-
-%% Calculate GPS TOAs
-artoa.controller.edit.timeOfArrival.calculateGpsToas();
-
-%% Update GUI
-artoa.controller.edit.offsets.updateGui();
-
-%% Replot TOA window
-artoa.controller.edit.timeOfArrival.plot();
+%% Recalculate
+artoa.controller.edit.recalculateToaGpsAndPlot();
 
 end
diff --git a/lib/+artoa/+controller/+edit/+offsets/updateGui.m b/lib/+artoa/+controller/+edit/+offsets/updateGui.m
index f81d42f..a20d116 100644
--- a/lib/+artoa/+controller/+edit/+offsets/updateGui.m
+++ b/lib/+artoa/+controller/+edit/+offsets/updateGui.m
@@ -17,7 +17,7 @@ for i = 1:length(fieldNames)
     currentValue = fields.(fieldNames{i});
     switch fieldNames{i}
         case 'useOffsets'
-            artoaGui.editOffsets.checkboxForceOptimumOffsets.Value = currentValue;
+            artoaGui.editOffsets.checkboxUseOffsets.Value = currentValue;
         case 'soundsourceOffsets'
             artoaGui.editOffsets.tableSoundsourceOffsets.Data = table2cell( ...
                 currentValue ...
diff --git a/lib/+artoa/+controller/+edit/+timeOfArrival/calculateGpsToas.m b/lib/+artoa/+controller/+edit/+timeOfArrival/calculateGpsToas.m
index b3d78a5..eb49b7c 100644
--- a/lib/+artoa/+controller/+edit/+timeOfArrival/calculateGpsToas.m
+++ b/lib/+artoa/+controller/+edit/+timeOfArrival/calculateGpsToas.m
@@ -34,10 +34,11 @@ for o = 1:length(fnames)
     );
 
     % add soundsource drift
-    if artoa.data.hasMember(artoaWorkspace, {'editOffsets', 'selectedOffsets'}) ...
-            && any(contains(artoaWorkspace.editOffsets.selectedOffsets.Properties.RowNames, fnames{o}))
-        offset = artoaWorkspace.editOffsets.selectedOffsets{fnames{o}, 'offset'};
-        drift = artoaWorkspace.editOffsets.selectedOffsets{fnames{o}, 'drift'};
+    if artoa.data.hasMember(artoaWorkspace, {'editOffsets', 'soundsourceOffsets'}) ...
+            && any(contains(artoaWorkspace.editOffsets.soundsourceOffsets.Properties.RowNames, fnames{o})) ...
+            && artoaWorkspace.editOffsets.useOffsets
+        offset = artoaWorkspace.editOffsets.soundsourceOffsets{fnames{o}, 'EmpiricalOffset'};
+        drift = artoaWorkspace.editOffsets.soundsourceOffsets{fnames{o}, 'EmpiricalDrift'};
         % no sorting required, because fourth parameter contains timesteps
         predictedToas = artoa.toa.addDrift( ...
             predictedToas, ...
diff --git a/lib/+artoa/+controller/+edit/recalculateToaGpsAndPlot.m b/lib/+artoa/+controller/+edit/recalculateToaGpsAndPlot.m
new file mode 100644
index 0000000..9dacd57
--- /dev/null
+++ b/lib/+artoa/+controller/+edit/recalculateToaGpsAndPlot.m
@@ -0,0 +1,33 @@
+function [] = recalculateToaGpsAndPlot()
+%RECALCULATETOAGPSANDPLOT Summary of this function goes here
+%   Detailed explanation goes here
+
+global artoaWorkspace artoaDataInput;
+
+%% Recalculate TOAs
+
+% combine initial toa and current applied soundsources
+toaData = artoaWorkspace.toaData;
+toaData.toa = artoaDataInput.toaData.toa;
+
+if artoaWorkspace.editOffsets.useOffsets
+    artoaWorkspace.toaData = artoa.toa.recalculate( ...
+        artoaWorkspace.float, ...
+        toaData, ...
+        artoaWorkspace.editOffsets.soundsourceOffsets ...
+    );
+else
+    artoaWorkspace.toaData = toaData;
+end
+
+%% Calculate GPS TOAs
+artoa.controller.edit.timeOfArrival.calculateGpsToas();
+
+%% Update Offsets GUI
+artoa.controller.edit.offsets.updateGui();
+
+%% Replot all windows
+artoa.controller.edit.updateAvailablePlots();
+
+end
+
diff --git a/lib/+artoa/+controller/+track/run.m b/lib/+artoa/+controller/+track/run.m
index 75c5de0..d72f474 100644
--- a/lib/+artoa/+controller/+track/run.m
+++ b/lib/+artoa/+controller/+track/run.m
@@ -63,6 +63,10 @@ pressureAndDate = { ...
     artoaWorkspace.rafosDate(artoaWorkspace.statusPressure == 1) ...
 };
 
+% restore initial toa, because it is required by the calculate function
+toaData = artoaWorkspace.toaData;
+toaData.toa = artoaDataInput.toaData.toa;
+
 [ ...
     trajectory, ...
     trajectoryDates, ...
@@ -74,9 +78,10 @@ pressureAndDate = { ...
     artoaWorkspace.float, ...
     pressureAndDate, ...
     artoaWorkspace.satData, ...
-    artoaWorkspace.toaData, ...
+    toaData, ...
     artoaWorkspace.filteredSoundsources, ...
     artoaWorkspace.trackParameter, ...
+    artoaWorkspace.editOffsets, ...
     soundVelocity, ...
     artoaDataInput.ini.leapseconds ...
 );
diff --git a/lib/+artoa/+offsets/createCalculationMatrices.m b/lib/+artoa/+offsets/createCalculationMatrices.m
new file mode 100644
index 0000000..b8a62a0
--- /dev/null
+++ b/lib/+artoa/+offsets/createCalculationMatrices.m
@@ -0,0 +1,132 @@
+function [a, b] = createCalculationMatrices(pRfb, pSoundsources, pTrackingParameter, pToaData, pSatData, pAppliedTemperature, pAppliedPressure, pLeapsecondsMatrix)
+%CREATECALCULATIONMATRICES Summary of this function goes here
+%   Detailed explanation goes here
+
+%% Initialize required variables
+%pSoundsources = artoa.controller.getSoundsourcesWithAppliedToa();
+satPositions = [pSatData.lat_sat, pSatData.lon_sat];
+satDates = artoa.convert.dmy2rd(pSatData.day_sat, pSatData.month_sat, pSatData.year_sat);
+toaDates = [];
+satToas = [];
+satDistances = [];
+floatDetails = pRfb.FLOAT;
+
+%% Create table
+results = struct();
+
+%% Calculate SAT TOAs for every soundsource and get nearest neighbor measured toa
+fnames = fieldnames(pSoundsources);
+for i = 1:length(fnames)
+    results.(fnames{i}) = table();
+    [date, toa] = artoa.toa.predictFromGps( ...
+        pRfb, ...
+        pSoundsources.(fnames{i}), ...
+        struct( ...
+            'temperature', pAppliedTemperature, ...
+            'pressure', pAppliedPressure, ...
+            'method', pTrackingParameter.soundspeedMethodString, ...
+            'soundSource', NaN ...
+        ), ...
+        pLeapsecondsMatrix ...
+    );
+    results.(fnames{i}).satDate = date;
+    results.(fnames{i}).satToa = toa;
+    results.(fnames{i}).daysSinceStart = ...
+        date ...
+        - artoa.convert.dmy2rd( ...
+            pSoundsources.(fnames{i}).begemis(3), ...
+            pSoundsources.(fnames{i}).begemis(2), ...
+            pSoundsources.(fnames{i}).begemis(1) ...
+        );
+    tmpDistances = [];
+    % calculate distances
+    for oDistance = 1:length(toa)
+        if isnan(toa(oDistance)) | any(isnan(satPositions(oDistance, :)))
+            tmpDistances = [tmpDistances; NaN];
+            continue;
+        end
+        tmpDistances = [ ...
+            tmpDistances; ...
+            artoa.data.calculateGeodist( ...
+                satPositions(oDistance, :), ...
+                pSoundsources.(fnames{i}).position ...
+            ) ...
+        ];
+    end
+    results.(fnames{i}).satDistances = tmpDistances;
+    % find nearest neighbor toa
+    tmpMeasuredToas = NaN(size(results.(fnames{i}).satDate));
+    tmpIndexBelongingToSoso = strcmp(pToaData.soundSource, fnames{i});
+    if ~any(tmpIndexBelongingToSoso)
+        results.(fnames{i}).measuredToa = tmpMeasuredToas;
+        continue;
+    end
+    tmpIndexBelongingToSoso = tmpIndexBelongingToSoso & (pToaData.status ~= 2);
+    tmpToaDate = pToaData.toaDate(tmpIndexBelongingToSoso);
+    tmpToa = pToaData.toa(tmpIndexBelongingToSoso);
+    
+    % interpolate toa data
+    [ ...
+        tmpToaDate, ...
+        tmpToa, ...
+        ~ ...
+    ] = artoa.data.interpolateRafosData( ...
+        tmpToaDate, ...
+        tmpToa, ...
+        pTrackingParameter.interpolationInterval, ...
+        pTrackingParameter.gapSize, ...
+        lower(pTrackingParameter.interpolationMethodString) ...
+    );
+    
+    % find nearest neighbor
+    for oSatDates = 1:length(results.(fnames{i}).satDate)
+        neighborDate = artoa.data.findNearestNeighbor( ...
+            tmpToaDate, ...
+            results.(fnames{i}).satDate(oSatDates) ...
+        );
+        tmpMeasuredToas(oSatDates) = tmpToa(tmpToaDate == neighborDate);
+    end
+    results.(fnames{i}).measuredToa = tmpMeasuredToas;
+    clear tmpDistances;
+end
+
+%% Construct matrices A and B
+
+rowCount = length(satDates) * length(fnames);
+
+aCore = zeros(rowCount, 2 * length(fnames));
+b = zeros(rowCount, 1);
+distances = zeros(rowCount, 1);
+daysSinceFloatStart = NaN(rowCount, 1);
+
+for i = 1:length(fnames)
+    rowIndices = ((i - 1) * length(satDates) + 1):i * length(satDates);
+    startColIndex = (2 * (i - 1)) + 1;
+    distances(rowIndices, 1) = results.(fnames{i}).satDistances;
+    aCore(rowIndices, startColIndex) = 1;
+    aCore(rowIndices, startColIndex + 1) = results.(fnames{i}).daysSinceStart;
+    b(rowIndices, 1) = results.(fnames{i}).measuredToa;
+    daysSinceFloatStart(rowIndices, 1) = ...
+        results.(fnames{i}).satDate ...
+        - artoa.convert.dmy2rd( ...
+            floatDetails.launchtime(3), ...
+            floatDetails.launchtime(2), ...
+            floatDetails.launchtime(1) ...
+        );
+    % construct b
+%     foundToas = findCorrespondingToas(fnames{i}, results.(fnames{i}).satDate);
+end
+
+a = [ ...
+    distances, aCore, ones(size(daysSinceFloatStart)), daysSinceFloatStart ...
+];
+
+% remove all NaN from matrix
+indicesToUse = all(~isnan(a), 2) & all(aCore >= 0, 2) & ~isnan(b);
+
+a = a(indicesToUse, :);
+b = b(indicesToUse, :);
+
+
+end
+
diff --git a/lib/+artoa/+offsets/extractOffsetsDriftsFromSolved.m b/lib/+artoa/+offsets/extractOffsetsDriftsFromSolved.m
index d0800d4..f92fd65 100644
--- a/lib/+artoa/+offsets/extractOffsetsDriftsFromSolved.m
+++ b/lib/+artoa/+offsets/extractOffsetsDriftsFromSolved.m
@@ -1,6 +1,9 @@
 function [offsets, drifts] = extractOffsetsDriftsFromSolved(pX)
-%EXTRACTOFFSETSDRIFTSFROMSOLVED Summary of this function goes here
-%   Detailed explanation goes here
+%EXTRACTOFFSETSDRIFTSFROMSOLVED Extracts the vectors in a specific manner.
+%   Creates for each output a vector, that first value is belonging to the
+%   float, and all others following are belonging to the soundsources as
+%   they are returned from the
+%   artoa.controller.getSoundsourcesWithAppliedToa function.
 
 soundsourceCount = (length(pX) - 3) / 2;
 
diff --git a/lib/+artoa/+offsets/filterCalculationMatrices.m b/lib/+artoa/+offsets/filterCalculationMatrices.m
new file mode 100644
index 0000000..51542ca
--- /dev/null
+++ b/lib/+artoa/+offsets/filterCalculationMatrices.m
@@ -0,0 +1,25 @@
+function [A, B] = filterCalculationMatrices(pA, pB)
+%FILTERCALCULATIONMATRICES Summary of this function goes here
+%   Detailed explanation goes here
+
+%% Initialize return variables
+A = pA;
+B = pB;
+
+%% Get soundsource count
+soundsourceCount = length(A(end, 2:end - 2)) / 2;
+
+%% Split matrix A
+distance = A(:, 1);
+soundsources = A(:, 2:end - 2);
+float = A(:, end - 1:end);
+
+%% Remove all soundsources that have only zeros
+soundsourcesToUse = any(soundsources, 1);
+soundsources = soundsources(:, soundsourcesToUse);
+
+%% Rebuild A
+A = [distance, soundsources, float];
+
+end
+
diff --git a/lib/+artoa/+offsets/solve.m b/lib/+artoa/+offsets/solve.m
index 1e5bd98..c44e026 100644
--- a/lib/+artoa/+offsets/solve.m
+++ b/lib/+artoa/+offsets/solve.m
@@ -1,144 +1,24 @@
-function [a, b, x] = solve(pRfb, pSoundsources, pTrackingParameter, pToaData, pSatData, pAppliedTemperature, pAppliedPressure, pLeapsecondsMatrix)
+function [unfilteredA, unfilteredB, X, A, B] = solve(pRfb, pSoundsources, pTrackingParameter, pToaData, pSatData, pAppliedTemperature, pAppliedPressure, pLeapsecondsMatrix)
 %UNTITLED Summary of this function goes here
 %   Detailed explanation goes here
 
-%% Initialize required variables
-%pSoundsources = artoa.controller.getSoundsourcesWithAppliedToa();
-satPositions = [pSatData.lat_sat, pSatData.lon_sat];
-satDates = artoa.convert.dmy2rd(pSatData.day_sat, pSatData.month_sat, pSatData.year_sat);
-toaDates = [];
-satToas = [];
-satDistances = [];
-floatDetails = pRfb.FLOAT;
-
-%% Create table
-results = struct();
-
-%% Calculate SAT TOAs for every soundsource and get nearest neighbor measured toa
-fnames = fieldnames(pSoundsources);
-for i = 1:length(fnames)
-    results.(fnames{i}) = table();
-    [date, toa] = artoa.toa.predictFromGps( ...
-        pRfb, ...
-        pSoundsources.(fnames{i}), ...
-        struct( ...
-            'temperature', pAppliedTemperature, ...
-            'pressure', pAppliedPressure, ...
-            'method', pTrackingParameter.soundspeedMethodString, ...
-            'soundSource', NaN ...
-        ), ...
-        pLeapsecondsMatrix ...
-    );
-    results.(fnames{i}).satDate = date;
-    results.(fnames{i}).satToa = toa;
-    results.(fnames{i}).daysSinceStart = ...
-        date ...
-        - artoa.convert.dmy2rd( ...
-            pSoundsources.(fnames{i}).begemis(3), ...
-            pSoundsources.(fnames{i}).begemis(2), ...
-            pSoundsources.(fnames{i}).begemis(1) ...
-        );
-    tmpDistances = [];
-    % calculate distances
-    for oDistance = 1:length(toa)
-        if isnan(toa(oDistance)) | any(isnan(satPositions(oDistance, :)))
-            tmpDistances = [tmpDistances; NaN];
-            continue;
-        end
-        tmpDistances = [ ...
-            tmpDistances; ...
-            artoa.data.calculateGeodist( ...
-                satPositions(oDistance, :), ...
-                pSoundsources.(fnames{i}).position ...
-            ) ...
-        ];
-    end
-    results.(fnames{i}).satDistances = tmpDistances;
-    % find nearest neighbor toa
-    tmpMeasuredToas = NaN(size(results.(fnames{i}).satDate));
-    tmpIndexBelongingToSoso = strcmp(pToaData.soundSource, fnames{i});
-    if ~any(tmpIndexBelongingToSoso)
-        results.(fnames{i}).measuredToa = tmpMeasuredToas;
-        continue;
-    end
-    tmpIndexBelongingToSoso = tmpIndexBelongingToSoso & (pToaData.status ~= 2);
-    tmpToaDate = pToaData.toaDate(tmpIndexBelongingToSoso);
-    tmpToa = pToaData.toa(tmpIndexBelongingToSoso);
-    
-    % interpolate toa data
-    [ ...
-        tmpToaDate, ...
-        tmpToa, ...
-        ~ ...
-    ] = artoa.data.interpolateRafosData( ...
-        tmpToaDate, ...
-        tmpToa, ...
-        pTrackingParameter.interpolationInterval, ...
-        pTrackingParameter.gapSize, ...
-        lower(pTrackingParameter.interpolationMethodString) ...
-    );
-    
-    % find nearest neighbor
-    for oSatDates = 1:length(results.(fnames{i}).satDate)
-        neighborDate = artoa.data.findNearestNeighbor( ...
-            tmpToaDate, ...
-            results.(fnames{i}).satDate(oSatDates) ...
-        );
-        tmpMeasuredToas(oSatDates) = tmpToa(tmpToaDate == neighborDate);
-    end
-    results.(fnames{i}).measuredToa = tmpMeasuredToas;
-    clear tmpDistances;
-end
-
 %% Construct matrices A and B
+[unfilteredA, unfilteredB] = artoa.offsets.createCalculationMatrices(pRfb, ...
+    pSoundsources, ...
+    pTrackingParameter, ...
+    pToaData, ...
+    pSatData, ...
+    pAppliedTemperature, ...
+    pAppliedPressure, ...
+    pLeapsecondsMatrix ...
+);
+
+%% Optimize matrices for calculation
+[A, B] = artoa.offsets.filterCalculationMatrices(unfilteredA, unfilteredB);
+
+%% Solve it!
+X = pinv(A) * B;
 
-rowCount = length(satDates) * length(fnames);
-
-aCore = zeros(rowCount, 2 * length(fnames));
-b = zeros(rowCount, 1);
-distances = zeros(rowCount, 1);
-daysSinceFloatStart = NaN(rowCount, 1);
-
-for i = 1:length(fnames)
-    rowIndices = ((i - 1) * length(satDates) + 1):i * length(satDates);
-    startColIndex = (2 * (i - 1)) + 1;
-    distances(rowIndices, 1) = results.(fnames{i}).satDistances;
-    aCore(rowIndices, startColIndex) = 1;
-    aCore(rowIndices, startColIndex + 1) = results.(fnames{i}).daysSinceStart;
-    b(rowIndices, 1) = results.(fnames{i}).measuredToa;
-    daysSinceFloatStart(rowIndices, 1) = ...
-        results.(fnames{i}).satDate ...
-        - artoa.convert.dmy2rd( ...
-            floatDetails.launchtime(3), ...
-            floatDetails.launchtime(2), ...
-            floatDetails.launchtime(1) ...
-        );
-    % construct b
-%     foundToas = findCorrespondingToas(fnames{i}, results.(fnames{i}).satDate);
-end
-
-a = [ ...
-    distances, aCore, ones(size(daysSinceFloatStart)), daysSinceFloatStart ...
-];
-
-% remove all NaN from matrix
-indicesToUse = all(~isnan(a), 2) & all(aCore >= 0, 2) & ~isnan(b);
-
-a = a(indicesToUse, :);
-b = b(indicesToUse, :);
-
-x = pinv(a) * b;
-
-
-%     function [toas] = findCorrespondingToas(soundsourceName, satDate)
-%         toas = NaN(size(satDate));
-%         for o = 1:length(satDate)
-%             if isnan(satDate(o))
-%                 continue;
-%             end
-%             neighbor = artoa.data.findNearestNeighbor(pToaData.date, satDate(o));
-%         end
-%     end
 
 
 end
diff --git a/lib/+artoa/+toa/recalculate.m b/lib/+artoa/+toa/recalculate.m
index 8ed1bf7..d1225a1 100644
--- a/lib/+artoa/+toa/recalculate.m
+++ b/lib/+artoa/+toa/recalculate.m
@@ -15,6 +15,10 @@ function [toaData] = recalculate(pFloat, pToaData, pOffsetsTable, pAppliedSounds
 %   Returns:
 %       toaData (struct):       The toa data structure with adjusted TOAs
 
+%% Prepare constants
+driftColumnName = 'EmpiricalDrift';
+offsetColumnName = 'EmpiricalOffset';
+
 %% Prepare return variable
 toaData = pToaData;
 
@@ -37,8 +41,8 @@ for i = 1:stepSize:length(toaData.toa)
     endIndex = i + stepSize - 1;
     toaData.toa(startIndex:endIndex) = artoa.toa.addDrift( ...
         toaData.toa(startIndex:endIndex), ...
-        pOffsetsTable{'Float', 'offset'}, ...
-        pOffsetsTable{'Float', 'drift'}, ...
+        pOffsetsTable{'Float', offsetColumnName}, ...
+        pOffsetsTable{'Float', driftColumnName}, ...
         floor(min(toaData.toaDate) - floatLaunchRafosTime) ...
     );
 end
@@ -72,8 +76,8 @@ for i = 1:length(fnames)
     % calculate drift of soundsource
     toa = artoa.toa.addDrift( ...
         toa, ...
-        pOffsetsTable{fnames{i}, 'offset'}, ...
-        pOffsetsTable{fnames{i}, 'drift'}, ...
+        pOffsetsTable{fnames{i}, offsetColumnName}, ...
+        pOffsetsTable{fnames{i}, driftColumnName}, ...
         floor(toaDate - rafosBeginEmission) ...
     );
 
diff --git a/lib/+artoa/+trajectory/calculate.m b/lib/+artoa/+trajectory/calculate.m
index 188dc99..809287d 100644
--- a/lib/+artoa/+trajectory/calculate.m
+++ b/lib/+artoa/+trajectory/calculate.m
@@ -1,4 +1,4 @@
-function [trajectory, trajectoryDates, trajectoryClockError, trajectoryResiduals, trajectoryVelocities, trajectoryTimeDivergenceToGps] = calculate(pFloatDetails, pPressureAndDate, pSatData, pToaData, pSoundsources, pTrackingParameter, pSoundVelocity, pLeapsecondsMatrix)
+function [trajectory, trajectoryDates, trajectoryClockError, trajectoryResiduals, trajectoryVelocities, trajectoryTimeDivergenceToGps] = calculate(pFloatDetails, pPressureAndDate, pSatData, pToaData, pSoundsources, pTrackingParameter, pOffsetsParameter, 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.
@@ -45,6 +45,20 @@ trajectoryResiduals = false;
 trajectoryTimeDivergenceToGps = false;
 trajectoryVelocities = 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.soundsourceOffsets, ...
+        soundsourceStruct ...
+    );
+end
+
 %% Prepare data for every sound source
 preparedData = struct();
 for i = 1:length(involvedSoundsources)
@@ -88,21 +102,21 @@ for i = 1:length(involvedSoundsources)
         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
+%     % 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
     [ ...
-- 
GitLab