diff --git a/tests/TestStateJumps.m b/tests/TestStateJumps.m
index 5c06e225..0aea9414 100644
--- a/tests/TestStateJumps.m
+++ b/tests/TestStateJumps.m
@@ -195,7 +195,7 @@ function testBounceball(testCase)
             sensTimes = [t0 sensTimes tEnd];
             fdStep = generateFDstep(numel(x0), numel(p), 'hy', 1e-6, 'hp', 1e-6);
             sensFun = generateSensitivityFunction(datahandle, sol, 'method', 'VDE', 'FDstep', fdStep, ...
-                'CalcGy', true, 'CalcGp', true, 'Gmatrices_intermediate', true);
+                'CalcGy', true, 'CalcGp', true, 'Gmatrices_intermediate', false);
             sens = sensFun(sensTimes);
             Gy = {sens.Gy};
             Gp = {sens.Gp};
@@ -257,7 +257,7 @@ function testSensitivitiesSimpleVDE(testCase)
             rtol2 = 1e-5;
 
             sensFun = generateSensitivityFunction(datahandle, sol, 'method', 'VDE', ...
-                'CalcGy', true, 'CalcGp', true, 'Gmatrices_intermediate', true);
+                'CalcGy', true, 'CalcGp', true, 'Gmatrices_intermediate', false);
 
             t1 = sol.switches(1);
             t1Minus = leftLimit(t1);
@@ -293,7 +293,7 @@ function testSensitivitiesSimpleEND_piecewise(testCase)
             rtol2 = 1e-5;
 
             sensFun = generateSensitivityFunction(datahandle, sol, 'method', 'END_piecewise', ...
-                'CalcGy', true, 'CalcGp', true, 'Gmatrices_intermediate', true);
+                'CalcGy', true, 'CalcGp', true, 'Gmatrices_intermediate', false);
 
             t1 = sol.switches(1);
             t1Minus = leftLimit(t1);
diff --git a/toolbox/examples/canonicalExample/analyticalSensitivities/analyticalSensitivities_test.m b/toolbox/examples/canonicalExample/analyticalSensitivities/analyticalSensitivities_test.m
index 3e615eff..c4b4d6b8 100644
--- a/toolbox/examples/canonicalExample/analyticalSensitivities/analyticalSensitivities_test.m
+++ b/toolbox/examples/canonicalExample/analyticalSensitivities/analyticalSensitivities_test.m
@@ -7,7 +7,7 @@
 tspan         = [0 20];
 initialvalues = [1;0];
 parameters    = 5.437;
-sol           = solveODE(datahandle, tspan, initialvalues, parameters); 
+sol           = solveODE(datahandle, tspan, initialvalues, parameters);
 
 %% Precalculations for sensitivities
 dim_y = size(sol.y, 1);
@@ -16,8 +16,10 @@
 
 %% Sensitivities VDE
 integrator_options = odeset( 'AbsTol', 1e-14,'RelTol', 1e-12);
-sensitivities_function_VDE = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, 'integrator_options', integrator_options, 'method', 'VDE', 'Gmatrices_intermediate', true);
+sensitivities_function_VDE = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, ...
+    'integrator_options', integrator_options, 'method', 'VDE', 'Gmatrices_intermediate', true, 'legacy', true);
 sensitivities_VDE = sensitivities_function_VDE(20);
+
 %% Analytical solution
 yA_1 = @(t) (1/300)*t.^3 + 1;
 yA_2 = 0;
@@ -89,4 +91,3 @@
 sensitivities_VDE.Up{1} - U1_p;
 sensitivities_VDE.Up{2} - U2_p;
 sensitivities_VDE.Gp - Gp_tf_t0;
-
diff --git a/toolbox/examples/canonicalExample/canonicalExampleAnalyticSolution.m b/toolbox/examples/canonicalExample/canonicalExampleAnalyticSolution.m
index 3c0ceeef..77237550 100644
--- a/toolbox/examples/canonicalExample/canonicalExampleAnalyticSolution.m
+++ b/toolbox/examples/canonicalExample/canonicalExampleAnalyticSolution.m
@@ -123,36 +123,5 @@
     yFull{idxModel} = @(t) [y{1, idxModel}(t); y{2, idxModel}(t)];
 end
 
-funcSolPiecewise = @(t) evalPiecewise(t, yFull, length(y0), tSwitch);
-end
-
-function y = evalPiecewise(t, solPiece, dimy, sw)
-% Make sure eval points and switching times are sorted in ascending order.
-[t, tSortIdx] = sort(t);
-sw = sort(sw);
-y = zeros(dimy, length(t));
-
-done = false;
-idxModelStart = 1;
-for idxSw=1:length(sw)
-    % Since eval points and switching times are sorted, we look for the first eval point that lies after the next switch.
-    idxModelEnd = (idxModelStart - 1) + find(t(idxModelStart:end) >= sw(idxSw), 1);
-    % If no such point exists, then we stay in the same model until the end.
-    if isempty(idxModelEnd)
-        y(:, idxModelStart:end) = solPiece{idxSw}(t(idxModelStart:end));
-        done = true;
-        break
-    end
-    % Otherwise, evaluate points before switch in the current model and update model for next iteration.
-    y(:, idxModelStart:idxModelEnd-1) = solPiece{idxSw}(t(idxModelStart:idxModelEnd-1));
-    idxModelStart = idxModelEnd;
-end
-
-% If we actually reach the last model or stay in first model, then we have to evaluate the remaining points in that model.
-if ~done
-    y(:, idxModelStart:end) = solPiece{end}(t(idxModelStart:end));
-end
-
-% Reverse the sorting
-y(:, tSortIdx) = y(:, 1:end);
+funcSolPiecewise = @(t) evalPiecewiseFunc(t, yFull, length(y0), tSwitch);
 end
diff --git a/toolbox/examples/canonicalExample/sensitivities_CanonicalExample.m b/toolbox/examples/canonicalExample/sensitivities_CanonicalExample.m
index 818973a5..d8153004 100644
--- a/toolbox/examples/canonicalExample/sensitivities_CanonicalExample.m
+++ b/toolbox/examples/canonicalExample/sensitivities_CanonicalExample.m
@@ -17,9 +17,9 @@
 %% Generation of sensitivity functions
 tic;
 integrator_options = odeset( 'AbsTol', 1e-14,'RelTol', 1e-12);
-sensitivities_function_ENDfull = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, 'method', 'END_full', 'calcGy', true,'calcGp',true, 'Gmatrices_intermediate', true, 'save_intermediates', true);
-sensitivities_function_ENDpiece = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, 'method', 'END_piecewise', 'calcGy', true,'calcGp',true, 'Gmatrices_intermediate', true, 'save_intermediates', false);
-sensitivities_function_VDE = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, 'integrator_options', integrator_options, 'method', 'VDE', 'calcGy', true,'calcGp',false, 'Gmatrices_intermediate', true, 'save_intermediates', false);
+sensitivities_function_ENDfull = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, 'method', 'END_full', 'calcGy', true,'calcGp',true, 'Gmatrices_intermediate', false, 'save_intermediates', true);
+sensitivities_function_ENDpiece = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, 'method', 'END_piecewise', 'calcGy', true,'calcGp',true, 'Gmatrices_intermediate', false, 'save_intermediates', false);
+sensitivities_function_VDE = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, 'integrator_options', integrator_options, 'method', 'VDE', 'calcGy', true,'calcGp',false, 'Gmatrices_intermediate', false, 'save_intermediates', false);
 toc;
 
 %% Sensitivty calculations
@@ -35,8 +35,8 @@
 toc;
 
 %% Plot sensitivities initial values
-sensitivities_function_ENDpiecewise = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, 'method', 'END_piecewise', 'Gy', true,'Gp',false, 'Gmatrices_intermediate', true, 'save_intermediates', true);
-sensitivities_function_VDE = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, 'method', 'VDE', 'calcGy', true,'calcGp',false, 'Gmatrices_intermediate', true, 'save_intermediates', true);
+sensitivities_function_ENDpiecewise = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, 'method', 'END_piecewise', 'Gy', true,'Gp',false, 'Gmatrices_intermediate', false, 'save_intermediates', true);
+sensitivities_function_VDE = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, 'method', 'VDE', 'calcGy', true,'calcGp',false, 'Gmatrices_intermediate', false, 'save_intermediates', true);
 
 t_plot = 0:0.01:20;
 %sensitivities_END_plot = sensitivities_function_ENDpiecewise(t_plot);
@@ -97,8 +97,8 @@
 
 %% Plot sensitivities parameters
 
-%sensitivities_function_ENDpiecewise = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, 'method', 'END_piecewise', 'Gy', false,'Gp',true, 'Gmatrices_intermediate', true, 'save_intermediates', true);
-sensitivities_function_VDE = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, 'method', 'VDE', 'calcGy', false,'calcGp',true, 'Gmatrices_intermediate', true, 'save_intermediates', true);
+%sensitivities_function_ENDpiecewise = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, 'method', 'END_piecewise', 'Gy', false,'Gp',true, 'Gmatrices_intermediate', false, 'save_intermediates', true);
+sensitivities_function_VDE = generateSensitivityFunction(datahandle, sol, 'FDstep', FDstep, 'method', 'VDE', 'calcGy', false,'calcGp',true, 'Gmatrices_intermediate', false, 'save_intermediates', true);
 
 t_plot = 0:0.01:20;
 %sensitivities_END_plot = sensitivities_function_ENDpiecewise(t_plot);
diff --git a/toolbox/examples/liveExamples/CanonicalSensitivities.mlx b/toolbox/examples/liveExamples/CanonicalSensitivities.mlx
index 9b8f2ed9..d4486ded 100644
Binary files a/toolbox/examples/liveExamples/CanonicalSensitivities.mlx and b/toolbox/examples/liveExamples/CanonicalSensitivities.mlx differ
diff --git a/toolbox/generateSensitivityFunction.m b/toolbox/generateSensitivityFunction.m
index eda7b9c7..4e2d5d74 100644
--- a/toolbox/generateSensitivityFunction.m
+++ b/toolbox/generateSensitivityFunction.m
@@ -1,118 +1,171 @@
 function sensitivities_function = generateSensitivityFunction(datahandle, sol, varargin)
-   % sensitivities_function = generateSensitivityFunction(datahandle, sol, varargin)
-   %
-   % Generates a function that can take a vector of timepoints and calculates the sensitivities at the given timepoints.
-   % Here the user can specify his requirements to the generated function.
-   % The methods possible for the calculation of sensitivities are END_full, END_piecewise or VDE.
-   % The calculation of the sensitivities at the time point of a switch is only possible using the methods END_piecewise or VDE.
-   % Therefore we look at the function y(t) as cadlag ("right continuous with left limits") and the calculation is possible using the updates.
-   %
-   % INPUT: datahandle - datahandle you get from the integration process with solveODE
-   %        sol        - solution object from the integration with solveODE
-   %        varargin   - optional specification of certain parameters:
-   %                     'FDstep'                 - struct that contains the step sizes for calculation of derivatives using finite differences
-   %                     'integrator'             - integrator for computing the END and solve the VDE
-   %                     'integrator_options'     - integrator options for computing the END and solve the VDE
-   %                     'calcGy'                 - flag that is true if the sensitivities w.r.t. the initial values should be calculated
-   %                     'calcGp'                 - flag that is true if the sensitivities w.r.t. the parameters should be calculated
-   %                     'save_intermediates'     - flag that is true if the intermediate G-matrices and updates that have been calculated for END_piecewise 
-   %                                                or VDE should be saved for the next function call for computing sensitivities
-   %                     'Gmatrices_intermediate' - flag that is true if the intermediate G-matrices that have been calculated for END_piecewise 
-   %                                                or VDE should be included in the output struct
-   %                     'method'                 - method that should be used to calculate the sensitivities (you can choose from END_full, 
-   %                                                END_piecewise and VDE)
-   %                     'directions_y'           - matrix that contains the directions in which you want to calculate the sensitivities w.r.t. y0 if you use END_full
-   %                     'directions_p'           - matrix that contains the directions in which you want to calculate the sensitivities w.r.t. p if you use END_full
-   %
-   % OUTPUT: sensitivities_function - function that that can take a vector of timepoints and the struct FDstep with the stepsizes for END 
-   %                                  as inputs and calculates the sensitivities at the given timepoints.
-
-   sensitivities_function = @computeSensitivities;
-   
-   generateData(datahandle, sol);
-   data = datahandle.getData();
-   dim_y = data.computeSensitivity.dim_y;
-   dim_p = data.computeSensitivity.dim_p;
-
-   % default settings
-   FDstep                      = olGetOption(varargin, 'FDstep',                 generateFDstep(dim_y, dim_p));
-   integrator                  = olGetOption(varargin, 'integrator',             data.integratorSettings.numericIntegrator);
-   integrator_options          = olGetOption(varargin, 'integrator_options',     data.integratorSettings.options);
-   method                      = olGetOption(varargin, 'method',                 'VDE');
-   Gmatrices_intermediate_flag = olGetOption(varargin, 'Gmatrices_intermediate', false);
-   Gy_flag                     = olGetOption(varargin, 'calcGy',                 true);
-   directions_y                = olGetOption(varargin, 'directions_y',           0);
-   Gp_flag                     = olGetOption(varargin, 'calcGp',                 true);
-   directions_p                = olGetOption(varargin, 'directions_p',           0);
-   save_intermediates          = olGetOption(varargin, 'save_intermediates',     true);
-
-   methodCoded.END_piecewise = 1;
-   methodCoded.VDE           = 2;
-   methodCoded.END_full      = 3;
-   
-   if strcmpi(method, 'END_piecewise'),method = methodCoded.END_piecewise; end
-   if strcmpi(method, 'VDE'),          method = methodCoded.VDE; end
-   if strcmpi(method, 'END_full'),     method = methodCoded.END_full; end
-   
-   % switches includes tspan(1) and tspan(end)
-   switches      = data.computeSensitivity.switches_extended;
-   switches_left = data.computeSensitivity.switches_extended_left;
-   
-   tspan = data.SWP_detection.tspan;
-
-   options.FDstep = FDstep;
-   options.integrator = integrator;
-   options.integrator_options = integrator_options;
-   options.method = method;
-   options.methodCoded = methodCoded;
- 
-   Gmatrices_intermediate_template.Gy = {eye(dim_y)};
-   Gmatrices_intermediate_template.Uy = {eye(dim_y)};
-   Gmatrices_intermediate_template.Gp = {zeros(dim_y, dim_p)};
-   Gmatrices_intermediate_template.Up = {zeros(dim_y, dim_p)};
-   return
-   
-   function sensitivities = computeSensitivities(t_all)
-       if ~Gy_flag && ~Gp_flag
-           return;
-       end
-      % sensitivities = computeSensitivities(t_all)
-      % 
-      % Calculates the sensitivities w.r.t. y0 and p at the given timepoints.
-      %
-      % INPUT: t_all  - vector of timepoints at which you want to calculate the sensitivities
-      %
-      % OUTPUT: sensitivities - struct that contains the given timepoints, the calculated sensitivities and the intermediate G-matrices
-   
-      [t_sort_unique, ~, unique_indices] = unique(t_all);
-      
-      if t_sort_unique(1) < tspan(1) || t_sort_unique(end) > tspan(end)
-         error('Time point is outside of the time interval of the IVP solution.')
-      end
-
-      config = makeConfig();
-      data = datahandle.getData();
-      data.caseCtrlif = config.caseCtrlif.computeSensitivities;
-      datahandle.setData(data);
-
-      Gmatrices_intermediate = Gmatrices_intermediate_template;
-
-      if method == methodCoded.END_full
-          sensitivities = compute_sensitivity_ENDfull(datahandle, t_sort_unique, sol, FDstep, Gy_flag, Gp_flag, directions_y, directions_p);
-          sensitivities = sensitivities(unique_indices);
-          return;
-      end
+% sensitivities_function = generateSensitivityFunction(datahandle, sol, varargin)
+%
+% Generates a function that can take a vector of timepoints and calculates the sensitivities at the given timepoints.
+% Here the user can specify his requirements to the generated function.
+% The methods possible for the calculation of sensitivities are END_full, END_piecewise or VDE.
+% The calculation of the sensitivities at the time point of a switch is only possible using the methods END_piecewise or VDE.
+% Therefore we look at the function y(t) as cadlag ("right continuous with left limits") and the calculation is possible using the updates.
+%
+% INPUT: datahandle - datahandle you get from the integration process with solveODE
+%        sol        - solution object from the integration with solveODE
+%        varargin   - optional specification of certain parameters:
+%                     'FDstep'                 - struct that contains the step sizes for calculation of derivatives using finite differences
+%                     'integrator'             - integrator for computing the END and solve the VDE
+%                     'integrator_options'     - integrator options for computing the END and solve the VDE
+%                     'calcGy'                 - flag that is true if the sensitivities w.r.t. the initial values should be calculated
+%                     'calcGp'                 - flag that is true if the sensitivities w.r.t. the parameters should be calculated
+%                     'save_intermediates'     - flag that is true if the intermediate G-matrices and updates that have been calculated for END_piecewise
+%                                                or VDE should be saved for the next function call for computing sensitivities
+%                     'Gmatrices_intermediate' - flag that is true if the intermediate G-matrices that have been calculated for END_piecewise
+%                                                or VDE should be included in the output struct
+%                     'method'                 - method that should be used to calculate the sensitivities (you can choose from END_full,
+%                                                END_piecewise and VDE)
+%                     'directions_y'           - matrix that contains the directions in which you want to calculate the sensitivities w.r.t. y0 if you use END_full or VDE
+%                     'directions_p'           - matrix that contains the directions in which you want to calculate the sensitivities w.r.t. p if you use END_full or VDE
+%
+% OUTPUT: sensitivities_function - function that that can take a vector of timepoints and the struct FDstep with the stepsizes for END
+%                                  as inputs and calculates the sensitivities at the given timepoints.
+
+sensitivities_function = @computeSensitivities;
+
+generateData(datahandle, sol);
+data = datahandle.getData();
+dim_y = data.computeSensitivity.dim_y;
+dim_p = data.computeSensitivity.dim_p;
+
+% default settings
+FDstep                      = olGetOption(varargin, 'FDstep',                 generateFDstep(dim_y, dim_p));
+integrator                  = olGetOption(varargin, 'integrator',             data.integratorSettings.numericIntegrator);
+integrator_options          = olGetOption(varargin, 'integrator_options',     data.integratorSettings.options);
+methodStr                   = olGetOption(varargin, 'method',                 'VDE');
+Gmatrices_intermediate_flag = olGetOption(varargin, 'Gmatrices_intermediate', false);
+Gy_flag                     = olGetOption(varargin, 'calcGy',                 true);
+directions_y                = olGetOption(varargin, 'directions_y',           []);
+Gp_flag                     = olGetOption(varargin, 'calcGp',                 true);
+directions_p                = olGetOption(varargin, 'directions_p',           []);
+save_intermediates          = olGetOption(varargin, 'save_intermediates',     true);
+legacy                      = olGetOption(varargin, 'legacy',                 false);
+
+methodCoded.END_piecewise = 1;
+methodCoded.VDE           = 2;
+methodCoded.END_full      = 3;
+
+if strcmpi(methodStr, 'END_piecewise'),method = methodCoded.END_piecewise; end
+if strcmpi(methodStr, 'VDE'),          method = methodCoded.VDE; end
+if strcmpi(methodStr, 'END_full'),     method = methodCoded.END_full; end
+
+% switches includes tspan(1) and tspan(end)
+switches      = data.computeSensitivity.switches_extended;
+switches_left = data.computeSensitivity.switches_extended_left;
+
+tspan = data.SWP_detection.tspan;
+
+methodCoded.END_piecewise = 1;
+methodCoded.VDE           = 2;
+methodCoded.END_full      = 3;
+
+if strcmpi(method, 'END_piecewise'),method = methodCoded.END_piecewise; end
+if strcmpi(method, 'VDE'),          method = methodCoded.VDE; end
+if strcmpi(method, 'END_full'),     method = methodCoded.END_full; end
+
+% switches includes tspan(1) and tspan(end)
+switches      = data.computeSensitivity.switches_extended;
+switches_left = data.computeSensitivity.switches_extended_left;
+
+tspan = data.SWP_detection.tspan;
+dim_y = data.computeSensitivity.dim_y;
+dim_p = data.computeSensitivity.dim_p;
+
+options.FDstep = FDstep;
+options.integrator = integrator;
+options.integrator_options = integrator_options;
+options.method = method;
+options.methodCoded = methodCoded;
+
+Gmatrices_intermediate_template.Gy = {eye(dim_y)};
+Gmatrices_intermediate_template.Uy = {eye(dim_y)};
+Gmatrices_intermediate_template.Gp = {zeros(dim_y, dim_p)};
+Gmatrices_intermediate_template.Up = {zeros(dim_y, dim_p)};
+return
+
+    function sensitivities = computeSensitivities(t_all)
+        if ~Gy_flag && ~Gp_flag
+            return;
+        end
+        % sensitivities = computeSensitivities(t_all)
+        %
+        % Calculates the sensitivities w.r.t. y0 and p at the given timepoints.
+        %
+        % INPUT: t_all  - vector of timepoints at which you want to calculate the sensitivities
+        %
+        % OUTPUT: sensitivities - struct that contains the given timepoints, the calculated sensitivities and the intermediate G-matrices
+
+
+        % TODO: Combine legacy and new computation methods.
+        if ~legacy && method ~= methodCoded.END_full
+            if Gmatrices_intermediate_flag
+                msg = [
+                    'Intermediate matrices can not be computed for directional sensitivities.\n', ...
+                    'Call this function with the parameter (''legacy'', true) instead.'];
+                error('IFDIFF:Sensitivity:DirectionalIntermediateMatrices', msg);
+            end
+
+            sensObj = IFDIFFSensitivity(datahandle, sol, Gy_flag, Gp_flag, directions_y, directions_p, FDstep, methodStr);
+            sens = sensObj.eval(t_all);
+            nPoints = size(sens, 3);
+            % TODO: For now keep compatibility with inefficient legacy format output.
+            fieldnames = {'t', 'Gy', 'Gy_intermediate', 'Uy', 'Gp', 'Gp_intermediate', 'Up'};
+            sensitivities = generateStructArray(fieldnames, nPoints);
+            if Gy_flag
+                ndirY = size(directions_y, 2);
+                if ndirY == 0; ndirY = dim_y; end
+            else
+                ndirY = 0;
+            end
+
+            for i=1:nPoints
+                if Gy_flag
+                    sensitivities(i).t = t_all(i);
+                    sensitivities(i).Gy = sens(:, 1:ndirY, i);
+                end
+                if Gp_flag
+                    % Parameter sensitivities are stored after initial value sensitivities (if there are any).
+                    sensitivities(i).Gp = sens(:, 1+ndirY:end, i);
+                end
+            end
+            return
+        end
+
+        [t_sort_unique, ~, unique_indices] = unique(t_all);
+
+        if t_sort_unique(1) < tspan(1) || t_sort_unique(end) > tspan(end)
+            error('Time point is outside of the time interval of the IVP solution.')
+        end
+
+        config = makeConfig();
+        data = datahandle.getData();
+        data.caseCtrlif = config.caseCtrlif.computeSensitivities;
+        datahandle.setData(data);
+
+        Gmatrices_intermediate = Gmatrices_intermediate_template;
+
+        if method == methodCoded.END_full
+            sensitivities = compute_sensitivity_ENDfull(datahandle, t_sort_unique, sol, FDstep, Gy_flag, Gp_flag, directions_y, directions_p);
+            sensitivities = sensitivities(unique_indices);
+            return;
+        end
         % VDE and END_piecewise share most of their code for obvious reasons. Only the construction of the intermediate
         % matrices G(t_s, t) varies.
         % The unique timepoints are separated into groups according to their model number, that means all time
         % points that are between the same two switching points are in one group. If one group is empty (so
-        % no timepoint is given between two switching points), 
-        % the group is left empty and will be skipped in the calculations later. 
+        % no timepoint is given between two switching points),
+        % the group is left empty and will be skipped in the calculations later.
         switches_temp = switches;
         switches_temp(end) = switches_temp(end) + eps(switches_temp(end));
         numTimeGroups = length(switches) - 1;
         timeGroups = cell(1, numTimeGroups);
-  
+
         for i = 1:length(switches)-1
             indices = (t_sort_unique >= switches_temp(i)) & (t_sort_unique < switches_temp(i+1));
             timeGroups{i} = t_sort_unique(indices);
@@ -125,7 +178,7 @@
         % because they are only needed until the last switch before the latest timepoint.
         t_max = t_sort_unique(end);
         numModels = find(t_max < switches_temp, 1) - 1;
-        
+
         % if the method END_full has been chosen before, the data for the sensitivity generation needs to be generated again here.
         generateData(datahandle, sol);
 
@@ -232,5 +285,5 @@
             sensitivitiesOutput = assembleSensitivityOutput(sensData, t_sort_unique, Gy_flag, Gp_flag, []);
         end
         sensitivities(:) = sensitivitiesOutput(unique_indices);
-   end
+    end
 end
diff --git a/toolbox/internal/sensitivities/IFDIFFDerivative.m b/toolbox/internal/sensitivities/IFDIFFDerivative.m
new file mode 100644
index 00000000..f1a8350d
--- /dev/null
+++ b/toolbox/internal/sensitivities/IFDIFFDerivative.m
@@ -0,0 +1,7 @@
+classdef IFDIFFDerivative < handle
+    methods (Abstract, Access=public)
+        d = dt(this, t, y, p, v)
+        d = dy(this, t, y, p, v)
+        d = dp(this, t, y, p, v)
+    end
+end
diff --git a/toolbox/internal/sensitivities/IFDIFFDerivativeFiniteDifferences.m b/toolbox/internal/sensitivities/IFDIFFDerivativeFiniteDifferences.m
new file mode 100644
index 00000000..b1f09f79
--- /dev/null
+++ b/toolbox/internal/sensitivities/IFDIFFDerivativeFiniteDifferences.m
@@ -0,0 +1,148 @@
+classdef IFDIFFDerivativeFiniteDifferences < IFDIFFDerivative
+    %this = IFDIFFDERIVATIVEFINITEDIFFERENCES(datahandle, f, step)
+    %
+    %Compute derivatives w.r.t. t, y, p for functions generated by IFDIFF (i.e. rhs, switching, jump, submodel).
+    %First order forward finite differences are used for the computation.
+    %
+    %INPUT:
+    %   datahandle - Provides signature for forced branching if function contains ctrlifs (only the case for rhs).
+    %       struct
+    %
+    %   f - Function whose derivative will be computed. Signature must be: out = f(datahandle, t, y, p).
+    %       function_handle
+    %
+    %   step - Step size information for the finite difference method. Should always come from a call to generateFDstep.
+    %       struct
+
+    properties (Access=public)
+        datahandle
+        f % out = f(datahandle, t, y, p)
+        dimOut
+        step % from generateFDstep
+    end
+
+    methods (Access=public)
+        function this = IFDIFFDerivativeFiniteDifferences(datahandle, f, dimOut, step)
+            if nargin == 0
+                return
+            end
+
+            this.datahandle = datahandle;
+            this.f = f;
+            this.dimOut = dimOut;
+            this.step = step;
+        end
+
+        %% Derivatives
+        % Note: Code for computing derivatives for different arguments (i.e. t,y,p) is deliberately duplicated here,
+        % because these functions will be called many times by the integrator when solving the VDE and we want to avoid
+        % performance loss due to overhead from anonymous functions.
+        function df = dt(this, t, y, p, v)
+            x = t;
+            h = this.ht(x);
+            [df, v, h, idxNonzero, f0] = this.init(x, t, y, p, v, h);
+            if isempty(idxNonzero)
+                return
+            end
+            % Evaluate function at forward points.
+            for i=idxNonzero
+                fh = this.f(this.datahandle, v(:, i), y, p);
+                df(:, i, :) = fh;
+            end
+            % Compute finite differences.
+            df(:, idxNonzero, :) = (df(:, idxNonzero, :) - f0) ./ h;
+        end
+        function df = dy(this, t, y, p, v)
+            x = y;
+            h = this.hy(x);
+            [df, v, h, idxNonzero, f0] = this.init(x, t, y, p, v, h);
+            if isempty(idxNonzero)
+                return
+            end
+            % Evaluate function at forward points.
+            for i=idxNonzero
+                fh = this.f(this.datahandle, t, v(:, i), p);
+                df(:, i, :) = fh;
+            end
+            % Compute finite differences.
+            df(:, idxNonzero, :) = (df(:, idxNonzero, :) - f0) ./ h;
+        end
+        function df = dp(this, t, y, p, v)
+            x = p;
+            h = this.hp(x);
+            [df, v, h, idxNonzero, f0] = this.init(x, t, y, p, v, h);
+            if isempty(idxNonzero)
+                return
+            end
+            % Evaluate function at forward points.
+            for i=idxNonzero
+                fh = this.f(this.datahandle, t, y, v(:, i));
+                df(:, i, :) = fh;
+            end
+            % Compute finite differences.
+            df(:, idxNonzero, :) = (df(:, idxNonzero, :) - f0) ./ h;
+        end
+    end
+
+    %% Helper functions
+    methods (Access=private)
+        function [df, v, h, idxNonzero, f0] = init(this, x, t, y, p, v, h)
+            % Init output: Derivatives are stored column-wise for each direction giving a matrix for each time point.
+            nv = size(v, 2);
+            nt = length(t);
+            df = zeros(this.dimOut, nv, nt);
+            % Compute forward evaluation points.
+            [v, h, idxNonzero] = this.scaleDirections(x, v, h);
+            % Compute function value at target point if needed.
+            if ~isempty(idxNonzero)
+                f0 = this.f(this.datahandle, t, y, p);
+            else
+                f0 = [];
+            end
+        end
+    end
+
+    methods (Static, Access=public)
+        function [v, h, idxNonzero] = scaleDirections(x, v, h)
+            % Filter zero directions.
+            normv = vecnorm(v, 2, 1);
+            idxNonzero = find(normv);
+            if isempty(idxNonzero)
+                return
+            end
+            % Scale each non-zero direction to unit length.
+            v(:, idxNonzero) = v(:, idxNonzero) ./ normv(idxNonzero);
+            % Assume that h is a column vector which contains a step size for each component.
+            % To determine the step size for an arbitrary direction v compute the scalar product of h and v.
+            h = h' * v(:, idxNonzero);
+            v(:, idxNonzero) = x + v(:, idxNonzero) .* h;
+            % Restore scaling for finite differences.
+            h = h ./ normv(idxNonzero);
+        end
+    end
+
+    %% Step size
+    methods (Access=public)
+        function h = ht(this, t)
+            if this.step.t_rel
+                h = max(this.step.t_min, abs(t) .* this.step.t);
+            else
+                h = this.step.t;
+            end
+        end
+        function h = hy(this, y)
+            if this.step.y_rel
+                h = max(this.step.y_min, abs(y) .* this.step.y);
+            else
+                h = this.step.y;
+            end
+        end
+        function h = hp(this, p)
+            if this.step.p_rel
+                h = max(this.step.p_min, abs(p) .* this.step.p);
+            else
+                h = this.step.p;
+            end
+        end
+    end
+end
diff --git a/toolbox/internal/sensitivities/IFDIFFSensitivity.m b/toolbox/internal/sensitivities/IFDIFFSensitivity.m
new file mode 100644
index 00000000..cf8220d4
--- /dev/null
+++ b/toolbox/internal/sensitivities/IFDIFFSensitivity.m
@@ -0,0 +1,284 @@
+classdef IFDIFFSensitivity
+    %IFDIFFSENSITIVITY Summary of this class goes here
+    %   Detailed explanation goes here
+
+    properties
+        datahandle
+        solution
+        parameters
+        initialValues
+        tspan
+        switches
+        switchesY
+        switchesLeft
+        switchesLeftY
+        dimy
+        dimp
+        integrator
+        integratorOptions
+        dirY
+        dirP
+        switchingFunctions
+        jumpFunctions
+        fdStep
+        method
+    end
+
+    properties (Access=private)
+        solver
+    end
+
+    properties (Constant)
+        METHOD = struct( ...
+            'VDE', 1, ...
+            'END_piecewise', 2, ...
+            'END_full', 3)
+    end
+
+    methods (Static)
+        function f = getFiniteDifferenceSolFun(sol, dim, solDisturbY, solDisturbP, hy, hp)
+            f = @computeSens;
+            nDir = length(solDisturbY);
+
+            function sens = computeSens(t)
+                nt = length(t);
+                sens = zeros(dim, nDir, nt);
+
+                solT = [];
+                for i=1:nDir
+                    solY = solDisturbY{i};
+                    solP = solDisturbP{i};
+                    % Make sure we evaluate the solution only once if needed.
+                    if isempty(solT) && (~isempty(solY) | ~isempty(solT))
+                        solT = deval(sol, t);
+                    end
+
+                    if ~isempty(solY)
+                        sens(:, i, :) = (deval(solY, t) - solT) ./ hy(i);
+                    end
+                    if ~isempty(solP)
+                        sens(:, i, :) = sens(:, i, :) + reshape((deval(solP, t) - solT) ./ hp(i), dim, 1, nt);
+                    end
+                end
+                sens = reshape(sens, [], nt);
+            end
+        end
+    end
+
+    methods
+        function this = IFDIFFSensitivity(datahandle, sol, calcGy, calcGp, dirY, dirP, fdStep, method)
+            if nargin == 0
+                return;
+            end
+
+            this.datahandle = datahandle;
+            this.solution = sol;
+            data = datahandle.getData();
+            this.switchingFunctions = data.SWP_detection.switchingFunction;
+            this.jumpFunctions = data.SWP_detection.jumpFunction;
+
+            this.initialValues = data.SWP_detection.initialvalues;
+            this.parameters = data.SWP_detection.parameters;
+            this.dimy = length(this.initialValues);
+            this.dimp = length(this.parameters);
+
+            this.tspan = data.SWP_detection.tspan;
+            this.switches = sort(sol.switches);
+
+            [isSwitchInSol, switchesIdx] = ismember(this.switches, sol.x);
+            if ~all(isSwitchInSol)
+                error('IFDIFF:Sensitivity:SwitchNotInSol', ...
+                    'Switch at t=%.10g not found in solution time points.', ...
+                    this.switches(find(~isSwitchInSol, 1)));
+            end
+            this.switchesY = sol.y(:, switchesIdx);
+
+            this.switchesLeft = this.switches;
+            this.switchesLeftY = this.switchesY;
+            % Use left limit of switch if there was a jump.
+            this.switchesLeft(sol.jumps) = sol.x(switchesIdx(sol.jumps) - 1);
+            this.switchesLeftY(:, sol.jumps) = sol.y(:, switchesIdx(sol.jumps) - 1);
+
+            this.integrator = data.integratorSettings.numericIntegrator;
+            this.integratorOptions = data.integratorSettings.options;
+
+            if ~calcGy && ~calcGp
+                error('IFDIFF:Sensitivity:NothingToCompute', 'Neither initial value nor parameter sensitivity was requested.')
+            end
+            if ~calcGy
+                dirY = [];
+            elseif isempty(dirY)
+                dirY = eye(this.dimy);
+            end
+            if ~calcGp
+                dirP = [];
+            elseif isempty(dirP)
+                dirP = eye(this.dimp);
+            end
+            this.dirY = dirY;
+            this.dirP = dirP;
+
+            this.fdStep = fdStep;
+
+            methodNum = this.METHOD.(method);
+            switch methodNum
+                case this.METHOD.VDE
+                    this.solver = @this.solveVde;
+                case this.METHOD.END_piecewise
+                    this.solver = @this.solveEnd;
+                otherwise
+                    id = 'IFDIFF:Sensitivity:UnrecognizedSolver';
+                    msg = 'The solver with number %d derived from string ''%s'' does not exist.';
+                    error(id, msg, methodNum, method);
+            end
+        end
+
+        function [sensEnd, sensFun] = solveVde(this, idxModel, tspan, sensStart, nDirY)
+            rhs = getRhsFromModelNum(this.datahandle, idxModel);
+            df = IFDIFFDerivativeFiniteDifferences(this.datahandle, rhs, this.dimy, this.fdStep);
+
+            rhsVde = @(t, G) vdeRhs(t, G, this.parameters, this.solution, nDirY, df, this.dirP);
+            sensStart = sensStart(:);
+            sol = this.integrator(rhsVde, tspan, sensStart, this.integratorOptions);
+
+            sensEnd = sol.y(:, end);
+            sensFun = @(t) deval(sol, t);
+        end
+
+        function [sensEnd, sensFun] = solveEnd(this, idxModel, tspan, sensStart, nDirY)
+            rhs = getRhsFromModelNum(this.datahandle, idxModel);
+
+            if idxModel == 1
+                y0 = this.initialValues;
+            else
+                y0 = this.switchesY(:, idxModel - 1);
+            end
+
+            nDir = size(sensStart, 2);
+            solDisturbY = cell(1, nDir);
+            solDisturbP = cell(1, nDir);
+            % Solve with disturbed initial values.
+            fd = IFDIFFDerivativeFiniteDifferences([], [], [], this.fdStep);
+            hy = fd.hy(y0);
+            [yh, hy, idxNonzeroY] = fd.scaleDirections(y0, sensStart, hy);
+            % Pad to full length to avoid indexing issues.
+            hyFull = zeros(1, nDir);
+            hyFull(idxNonzeroY) = hy;
+            for i=idxNonzeroY
+                solDisturbY{i} = this.integrator( ...
+                    @(t, y) rhs(this.datahandle, t, y, this.parameters), tspan, yh(:, i), this.integratorOptions);
+            end
+            % Solve with disturbed parameters.
+            hpFull = [];
+            if nDirY < nDir
+                hp = fd.hp(this.parameters);
+                [ph, hp, idxNonzeroP] = fd.scaleDirections(this.parameters, this.dirP, hp);
+                % Pad to full length to avoid indexing issues.
+                hpFull = zeros(1, nDir);
+                hpFull(nDirY + idxNonzeroP) = hp;
+                for i=idxNonzeroP
+                    solDisturbP{nDirY + i} = this.integrator( ...
+                        @(t, y) rhs(this.datahandle, t, y, ph(:, i)), tspan, y0, this.integratorOptions);
+                end
+            end
+
+            sensEnd = zeros(this.dimy, nDir);
+            % Re-solve the undisturbed RHS here to be more consistent with the disturbed solution.
+            sol = this.integrator(@(t, y) rhs(this.datahandle, t, y, this.parameters), tspan, y0, this.integratorOptions);
+            yEnd = deval(sol, tspan(end));
+            for i=1:nDir
+                solY = solDisturbY{i};
+                solP = solDisturbP{i};
+                if ~isempty(solY)
+                    sensEnd(:, i) = (solY.y(:, end) - yEnd) ./ hyFull(i);
+                end
+                if ~isempty(solP)
+                    sensEnd(:, i) = sensEnd(:, i) + (solP.y(:, end) - yEnd) ./ hpFull(i);
+                end
+            end
+            sensFun = this.getFiniteDifferenceSolFun(sol, this.dimy, solDisturbY, solDisturbP, hyFull, hpFull);
+        end
+
+        function [sens, fPlus] = applySensitivitySwitchUpdate(this, sens, idxModel, fMinus)
+            tMinus = this.switchesLeft(idxModel);
+            tPlus = this.switches(idxModel);
+            yMinus = this.switchesLeftY(:, idxModel);
+            yPlus = this.switchesY(:, idxModel);
+
+            % TODO: Fix ugly workaround after datahandle refactor.
+            % If the submodel is not exported as a separate function and instead relies on the datahandle,
+            % then we have to evaluate fMinus here, so that the signature is set correctly for fPlus.
+            if ~makeConfig().removeCtrlifForSensComputation
+                fMinusEval = fMinus(this.datahandle, tMinus, yMinus, this.parameters);
+                fMinus = @(~, ~, ~, ~) fMinusEval;
+            end
+            fPlus = getRhsFromModelNum(this.datahandle, idxModel + 1);
+
+            % Setup derivatives.
+            sigma = this.switchingFunctions{idxModel};
+            jump = this.jumpFunctions{idxModel};
+
+            dsigma = IFDIFFDerivativeFiniteDifferences([], sigma, 1, this.fdStep);
+            if ~isempty(jump)
+                djump = IFDIFFDerivativeFiniteDifferences([], jump, this.dimy, this.fdStep);
+            else
+                djump = [];
+            end
+
+            sens = computeSensitivitySwitchUpdate( ...
+                sens, this.dirP, ...
+                tMinus, tPlus, yMinus, yPlus, this.parameters, ...
+                @(t, y, p) fMinus(this.datahandle, t, y, p), @(t, y, p) fPlus(this.datahandle, t, y, p), ...
+                dsigma, djump);
+        end
+
+        function [sens, sensFun] = eval(this, timepoints)
+            % Ensure timepoints are strictly increasing.
+            [t, ~, idxTimepointsUndoSort] = unique(timepoints);
+
+            if t(1) < this.tspan(1) || t(end) > this.tspan(end)
+                error('IFDIFF:Sensitivity:TimepointOutOfBounds', ...
+                    ['Requested sensitivity evaluation timepoint is not contained within,', ...
+                    'the solution interval of the IVP.']);
+            end
+
+            % TODO: May cache solution from previous runs for the same parameters.
+            % For now, always start solving from the first model.
+            idxModelStart = 1;
+            % Determine the model of the last evaluation timepoint.
+            idxModelEnd = find(t(end) <= [this.switches, this.tspan(end)], 1);
+
+            tModelStart = this.tspan(1);
+            sensInitialValue = [this.dirY, zeros(this.dimy, size(this.dirP, 2))];
+            nDirY = size(this.dirY, 2);
+
+            % Integrate each submodel until switch and apply update at the end.
+            nModels = idxModelEnd - idxModelStart + 1; % Guaranteed to be greater than zero.
+            sensFun = cell(1, nModels);
+            for idxModel=idxModelStart:idxModelEnd-1
+                tModelEnd = this.switchesLeft(idxModel);
+                [sensEnd, sensFun{idxModel - idxModelStart + 1}] = this.solver( ...
+                    idxModel, [tModelStart, tModelEnd], sensInitialValue, nDirY);
+
+                % Prepare next model.
+                tModelStart = this.switches(idxModel);
+                % Update initial value for next VDE at switch using update formula.
+                sensInitialValue = reshape(sensEnd, this.dimy, []);
+                % Setup fMinus beforehand for the first update. Next updates can just use fPlus from the preceding update.
+                if idxModel == idxModelStart
+                    fMinus = getRhsFromModelNum(this.datahandle, idxModelStart);
+                end
+                [sensInitialValue, fMinus] = this.applySensitivitySwitchUpdate(sensInitialValue, idxModel, fMinus);
+            end
+            % No more switches left, so solve until the end.
+            tModelEnd = t(end);
+            [~, sensFun{idxModelEnd - idxModelStart + 1}] = this.solver( ...
+                idxModelEnd, [tModelStart, tModelEnd], sensInitialValue, nDirY);
+
+            % Return sensitivity at requested timepoints
+            sens = evalPiecewiseFunc(t, sensFun, numel(sensInitialValue), this.switches);
+            sens = reshape(sens, this.dimy, size(sensInitialValue, 2), []);
+            sens = sens(:, :, idxTimepointsUndoSort);
+        end
+    end
+end
diff --git a/toolbox/internal/sensitivities/computeSensitivitySwitchUpdate.m b/toolbox/internal/sensitivities/computeSensitivitySwitchUpdate.m
new file mode 100644
index 00000000..4b306527
--- /dev/null
+++ b/toolbox/internal/sensitivities/computeSensitivitySwitchUpdate.m
@@ -0,0 +1,97 @@
+function update = computeSensitivitySwitchUpdate( ...
+    sens, dirP, ...
+    tMinus, tPlus, yMinus, yPlus, p, ...
+    fMinusFunc, fPlusFunc, ...
+    dsigma, djump)
+
+update = sens;
+% Determine which columns contain parameter sensitivities and which initial value sensitivities.
+% Assumption: Initial value sensitivities come first, then parameter sensitivities.
+[dim, nDir] = size(sens);
+nDirP = size(dirP, 2);
+nDirY = nDir - nDirP;
+hasParameter = nDirP > 0;
+
+if ~isempty(fPlusFunc) || ~isempty(djump)
+    fMinus = fMinusFunc(tMinus, yMinus, p);
+else
+    fMinus = [];
+end
+
+% Compute derivative of solution wrt switching time: f_p - f_m - jump_t - jump_y*f_m
+% Also apply state jump updates: s += jump_y*s; for parameters additionally do s += jump_p*dir_p
+derivativeSolutionWrtSwitchingTime = zeros(dim, 1);
+if ~isempty(fPlusFunc)
+    fPlus = fPlusFunc(tPlus, yPlus, p);
+    derivativeSolutionWrtSwitchingTime = derivativeSolutionWrtSwitchingTime + fPlus - fMinus;
+end
+if ~isempty(djump)
+    derivativeSolutionWrtSwitchingTime = derivativeSolutionWrtSwitchingTime - djump.dt(tMinus, yMinus, p, 1);
+
+    % Evaluate jump_y in direction of sens and fMinus in one call for efficiency.
+    % Last column belongs to fMinus.
+    jumpDyPartial = djump.dy(tMinus, yMinus, p, [sens, fMinus]);
+    update = update + jumpDyPartial(:, 1:end-1);
+    derivativeSolutionWrtSwitchingTime = derivativeSolutionWrtSwitchingTime - jumpDyPartial(:, end);
+
+    if hasParameter
+        % Apply only to parameter sensitivities.
+        update(:, nDirY+1:end) = update(:, nDirY+1:end) + djump.dp(tMinus, yMinus, p, dirP);
+    end
+end
+
+if all(derivativeSolutionWrtSwitchingTime == 0)
+    % Remaining outer product term would be zero, so we are done here.
+    return
+end
+
+% Compute derivative of switching time wrt initial value and parameter: sigma_y*s + sigma_p*dir_p
+derivativeSwitchingTimeWrtParams = zeros(1, nDir);
+if ~isempty(dsigma)
+    derivativeSwitchingTimeWrtParams = derivativeSwitchingTimeWrtParams + dsigma.dy(tMinus, yMinus, p, sens);
+    if hasParameter
+        % Apply only to parameter sensitivity components
+        derivativeSwitchingTimeWrtParams(nDirY+1:end) = derivativeSwitchingTimeWrtParams(nDirY+1:end) ...
+            + dsigma.dp(tMinus, yMinus, p, dirP);
+    end
+end
+if all(derivativeSwitchingTimeWrtParams == 0)
+    % Remaining outer product term would be zero, so we are done here.
+    return
+end
+
+% In case we haven't evaluated fMinus in the previous section.
+if isempty(fMinus)
+    fMinus = fMinusFunc(tMinus, yMinus, p);
+end
+
+% Compute sigma/dt using chain rule.
+sigmaDtPartial = dsigma.dt(tMinus, yMinus, p, 1);
+sigmaDyPartialFminus = dsigma.dy(tMinus, yMinus, p, fMinus);
+sigmaDtTotal = sigmaDtPartial + sigmaDyPartialFminus;
+
+% Check that sigma/dt is not too close to zero.
+sigmaDtThreshold = 10*eps(tMinus);
+if abs(sigmaDtTotal) < sigmaDtThreshold
+    throw(switchingFunctionDerivativeZeroException(sigmaDtTotal, sigmaDtThreshold));
+end
+
+% Scale smaller vector in outer product by sigma/dt for efficiency
+if dim <= nDir
+    derivativeSolutionWrtSwitchingTime = derivativeSolutionWrtSwitchingTime./sigmaDtTotal;
+else
+    derivativeSwitchingTimeWrtParams = derivativeSwitchingTimeWrtParams./sigmaDtTotal;
+end
+
+update = update + derivativeSolutionWrtSwitchingTime * derivativeSwitchingTimeWrtParams;
+end
+
+
+%% Exceptions
+function e = switchingFunctionDerivativeZeroException(dt, thresh)
+id = 'IFDIFF:Sensitvity:UpdateSwitchingFunctionDerivativeZero';
+msg = [ ...
+    'Unable to compute sensitivity update, ', ...
+    'because total derivative of switching function w.r.t. time is close to zero: dt=%.10g<%.10g'];
+e = MException(id, msg, dt, thresh);
+end
diff --git a/toolbox/internal/sensitivities/evalPiecewiseFunc.m b/toolbox/internal/sensitivities/evalPiecewiseFunc.m
new file mode 100644
index 00000000..caf3ea30
--- /dev/null
+++ b/toolbox/internal/sensitivities/evalPiecewiseFunc.m
@@ -0,0 +1,30 @@
+function y = evalPiecewiseFunc(t, piecewiseFunc, dimy, switches)
+% Make sure eval points and switching times are sorted in ascending order.
+[t, ~, tUndoSortIdx] = unique(t);
+switches = unique(switches);
+y = zeros(dimy, length(t));
+
+done = false;
+idxModelStart = 1;
+for idxSw=1:length(switches)
+    % Since eval points and switching times are sorted, we look for the first eval point that lies after the next switch.
+    idxModelEnd = (idxModelStart - 1) + find(t(idxModelStart:end) >= switches(idxSw), 1);
+    % If no such point exists, then we stay in the same model until the end.
+    if isempty(idxModelEnd)
+        y(:, idxModelStart:end) = piecewiseFunc{idxSw}(t(idxModelStart:end));
+        done = true;
+        break
+    end
+    % Otherwise, evaluate points before switch in the current model and update model for next iteration.
+    y(:, idxModelStart:idxModelEnd-1) = piecewiseFunc{idxSw}(t(idxModelStart:idxModelEnd-1));
+    idxModelStart = idxModelEnd;
+end
+
+% If we actually reach the last model or stay in first model, then we have to evaluate the remaining points in that model.
+if ~done
+    y(:, idxModelStart:end) = piecewiseFunc{end}(t(idxModelStart:end));
+end
+
+% Reverse the sorting
+y(:, tUndoSortIdx) = y(:, 1:end);
+end
diff --git a/toolbox/internal/sensitivities/getRhsFromModelNum.m b/toolbox/internal/sensitivities/getRhsFromModelNum.m
index c1abfa42..ef853d7f 100644
--- a/toolbox/internal/sensitivities/getRhsFromModelNum.m
+++ b/toolbox/internal/sensitivities/getRhsFromModelNum.m
@@ -43,5 +43,7 @@
     return
 end
 
-% Default case, just return the preprocessed RHS
+% Default case, just return the preprocessed RHS and set the model number for correct ctrlif branching.
+data.computeSensitivity.modelStage = modelNum;
+datahandle.setData(data);
 rhs = data.integratorSettings.preprocessed_rhs;
diff --git a/toolbox/internal/sensitivities/VDE_RHS_p.m b/toolbox/internal/sensitivities/legacy/VDE_RHS_p.m
similarity index 100%
rename from toolbox/internal/sensitivities/VDE_RHS_p.m
rename to toolbox/internal/sensitivities/legacy/VDE_RHS_p.m
diff --git a/toolbox/internal/sensitivities/VDE_RHS_y.m b/toolbox/internal/sensitivities/legacy/VDE_RHS_y.m
similarity index 100%
rename from toolbox/internal/sensitivities/VDE_RHS_y.m
rename to toolbox/internal/sensitivities/legacy/VDE_RHS_y.m
diff --git a/toolbox/internal/sensitivities/assembleSensitivityOutput.m b/toolbox/internal/sensitivities/legacy/assembleSensitivityOutput.m
similarity index 100%
rename from toolbox/internal/sensitivities/assembleSensitivityOutput.m
rename to toolbox/internal/sensitivities/legacy/assembleSensitivityOutput.m
diff --git a/toolbox/internal/sensitivities/compute_sensitivity_ENDfull.m b/toolbox/internal/sensitivities/legacy/compute_sensitivity_ENDfull.m
similarity index 100%
rename from toolbox/internal/sensitivities/compute_sensitivity_ENDfull.m
rename to toolbox/internal/sensitivities/legacy/compute_sensitivity_ENDfull.m
diff --git a/toolbox/internal/sensitivities/compute_sensitivity_ENDfull_p.m b/toolbox/internal/sensitivities/legacy/compute_sensitivity_ENDfull_p.m
similarity index 98%
rename from toolbox/internal/sensitivities/compute_sensitivity_ENDfull_p.m
rename to toolbox/internal/sensitivities/legacy/compute_sensitivity_ENDfull_p.m
index ec6bab39..fb81e259 100644
--- a/toolbox/internal/sensitivities/compute_sensitivity_ENDfull_p.m
+++ b/toolbox/internal/sensitivities/legacy/compute_sensitivity_ENDfull_p.m
@@ -23,7 +23,7 @@
    h_p = FDstep.dir;
    
    % If no directions for directional derivatives were given then the usual sensitivities are calculated
-   if isscalar(directions_p) && directions_p == 0
+   if isempty(directions_p)
       directions_p = eye(dim_p);
       h_p = fdStep_getH_p(FDstep, parameters);
    end
@@ -60,4 +60,4 @@
    data.SWP_detection.parameters = parameters;
    data.SWP_detection.tspan = tspan;
    datahandle.setData(data);
-end
\ No newline at end of file
+end
diff --git a/toolbox/internal/sensitivities/compute_sensitivity_ENDfull_y.m b/toolbox/internal/sensitivities/legacy/compute_sensitivity_ENDfull_y.m
similarity index 98%
rename from toolbox/internal/sensitivities/compute_sensitivity_ENDfull_y.m
rename to toolbox/internal/sensitivities/legacy/compute_sensitivity_ENDfull_y.m
index 50688b0e..92f0c43d 100644
--- a/toolbox/internal/sensitivities/compute_sensitivity_ENDfull_y.m
+++ b/toolbox/internal/sensitivities/legacy/compute_sensitivity_ENDfull_y.m
@@ -23,7 +23,7 @@
    h_y = FDstep.dir;
    
    % If no directions for directional derivatives were given then the usual sensitivities are calculated
-   if isscalar(directions_y) && directions_y == 0
+   if isempty(directions_y)
       directions_y = eye(dim_y);
       h_y = fdStep_getH_y(FDstep, initialvalues);
    end
@@ -60,4 +60,4 @@
    data.SWP_detection.initialvalues = initialvalues;
    data.SWP_detection.tspan = tspan;
    datahandle.setData(data);
-end
\ No newline at end of file
+end
diff --git a/toolbox/internal/sensitivities/del_f_del_p.m b/toolbox/internal/sensitivities/legacy/del_f_del_p.m
similarity index 100%
rename from toolbox/internal/sensitivities/del_f_del_p.m
rename to toolbox/internal/sensitivities/legacy/del_f_del_p.m
diff --git a/toolbox/internal/sensitivities/del_f_del_t.m b/toolbox/internal/sensitivities/legacy/del_f_del_t.m
similarity index 100%
rename from toolbox/internal/sensitivities/del_f_del_t.m
rename to toolbox/internal/sensitivities/legacy/del_f_del_t.m
diff --git a/toolbox/internal/sensitivities/del_f_del_y.m b/toolbox/internal/sensitivities/legacy/del_f_del_y.m
similarity index 100%
rename from toolbox/internal/sensitivities/del_f_del_y.m
rename to toolbox/internal/sensitivities/legacy/del_f_del_y.m
diff --git a/toolbox/internal/sensitivities/fdStep_getH_p.m b/toolbox/internal/sensitivities/legacy/fdStep_getH_p.m
similarity index 100%
rename from toolbox/internal/sensitivities/fdStep_getH_p.m
rename to toolbox/internal/sensitivities/legacy/fdStep_getH_p.m
diff --git a/toolbox/internal/sensitivities/fdStep_getH_t.m b/toolbox/internal/sensitivities/legacy/fdStep_getH_t.m
similarity index 100%
rename from toolbox/internal/sensitivities/fdStep_getH_t.m
rename to toolbox/internal/sensitivities/legacy/fdStep_getH_t.m
diff --git a/toolbox/internal/sensitivities/fdStep_getH_y.m b/toolbox/internal/sensitivities/legacy/fdStep_getH_y.m
similarity index 100%
rename from toolbox/internal/sensitivities/fdStep_getH_y.m
rename to toolbox/internal/sensitivities/legacy/fdStep_getH_y.m
diff --git a/toolbox/internal/sensitivities/generateData.m b/toolbox/internal/sensitivities/legacy/generateData.m
similarity index 100%
rename from toolbox/internal/sensitivities/generateData.m
rename to toolbox/internal/sensitivities/legacy/generateData.m
diff --git a/toolbox/internal/sensitivities/generateSensData.m b/toolbox/internal/sensitivities/legacy/generateSensData.m
similarity index 100%
rename from toolbox/internal/sensitivities/generateSensData.m
rename to toolbox/internal/sensitivities/legacy/generateSensData.m
diff --git a/toolbox/internal/sensitivities/generateStructArray.m b/toolbox/internal/sensitivities/legacy/generateStructArray.m
similarity index 100%
rename from toolbox/internal/sensitivities/generateStructArray.m
rename to toolbox/internal/sensitivities/legacy/generateStructArray.m
diff --git a/toolbox/internal/sensitivities/getGp_intermediate_END.m b/toolbox/internal/sensitivities/legacy/getGp_intermediate_END.m
similarity index 100%
rename from toolbox/internal/sensitivities/getGp_intermediate_END.m
rename to toolbox/internal/sensitivities/legacy/getGp_intermediate_END.m
diff --git a/toolbox/internal/sensitivities/getGp_intermediate_VDE.m b/toolbox/internal/sensitivities/legacy/getGp_intermediate_VDE.m
similarity index 100%
rename from toolbox/internal/sensitivities/getGp_intermediate_VDE.m
rename to toolbox/internal/sensitivities/legacy/getGp_intermediate_VDE.m
diff --git a/toolbox/internal/sensitivities/getGy_Gp_update.m b/toolbox/internal/sensitivities/legacy/getGy_Gp_update.m
similarity index 100%
rename from toolbox/internal/sensitivities/getGy_Gp_update.m
rename to toolbox/internal/sensitivities/legacy/getGy_Gp_update.m
diff --git a/toolbox/internal/sensitivities/getGy_intermediate_END.m b/toolbox/internal/sensitivities/legacy/getGy_intermediate_END.m
similarity index 100%
rename from toolbox/internal/sensitivities/getGy_intermediate_END.m
rename to toolbox/internal/sensitivities/legacy/getGy_intermediate_END.m
diff --git a/toolbox/internal/sensitivities/getGy_intermediate_VDE.m b/toolbox/internal/sensitivities/legacy/getGy_intermediate_VDE.m
similarity index 100%
rename from toolbox/internal/sensitivities/getGy_intermediate_VDE.m
rename to toolbox/internal/sensitivities/legacy/getGy_intermediate_VDE.m
diff --git a/toolbox/internal/sensitivities/solveDisturbed_Gp.m b/toolbox/internal/sensitivities/legacy/solveDisturbed_Gp.m
similarity index 100%
rename from toolbox/internal/sensitivities/solveDisturbed_Gp.m
rename to toolbox/internal/sensitivities/legacy/solveDisturbed_Gp.m
diff --git a/toolbox/internal/sensitivities/solveDisturbed_Gy.m b/toolbox/internal/sensitivities/legacy/solveDisturbed_Gy.m
similarity index 100%
rename from toolbox/internal/sensitivities/solveDisturbed_Gy.m
rename to toolbox/internal/sensitivities/legacy/solveDisturbed_Gy.m
diff --git a/toolbox/internal/sensitivities/solveVDE_Gp.m b/toolbox/internal/sensitivities/legacy/solveVDE_Gp.m
similarity index 100%
rename from toolbox/internal/sensitivities/solveVDE_Gp.m
rename to toolbox/internal/sensitivities/legacy/solveVDE_Gp.m
diff --git a/toolbox/internal/sensitivities/solveVDE_Gy.m b/toolbox/internal/sensitivities/legacy/solveVDE_Gy.m
similarity index 100%
rename from toolbox/internal/sensitivities/solveVDE_Gy.m
rename to toolbox/internal/sensitivities/legacy/solveVDE_Gy.m
diff --git a/toolbox/internal/sensitivities/vdeRhs.m b/toolbox/internal/sensitivities/vdeRhs.m
new file mode 100644
index 00000000..2eab0c38
--- /dev/null
+++ b/toolbox/internal/sensitivities/vdeRhs.m
@@ -0,0 +1,20 @@
+function dt = vdeRhs(t, sens, p, solNominalTrajectory, nDirY, df, dirP)
+y = deval(solNominalTrajectory, t);
+dimy = length(y);
+
+sensMatrix = reshape(sens, dimy, []);
+% Separate columns into initial value and parameter sensitivities.
+nDir = size(sensMatrix, 2);
+nDirP = nDir - nDirY;
+
+% State propagation applies to both sensitivities w.r.t. initial values and parameters.
+sensMatrix = df.dy(t, y, p, sensMatrix);
+
+% Add source term for parameter sensitivities.
+if nDirP > 0
+    sensMatrix(:, nDirY+1:end) = sensMatrix(:, nDirY+1:end) + df.dp(t, y, p, dirP);
+end
+
+% Flatten output in column-major order.
+dt = sensMatrix(:);
+end
diff --git a/toolbox/internal/solving/solveODE_assembleOutput.m b/toolbox/internal/solving/solveODE_assembleOutput.m
index a70dd830..902d2aec 100644
--- a/toolbox/internal/solving/solveODE_assembleOutput.m
+++ b/toolbox/internal/solving/solveODE_assembleOutput.m
@@ -16,7 +16,7 @@
 
 numSwitches = length(data.SWP_detection.switchingpoints);
 switches = zeros(1, numSwitches);
-jumps    = zeros(1, numSwitches);
+jumps    = false(1, numSwitches);
 
 for i = 1:numSwitches
     switches(1, i) = data.SWP_detection.switchingpoints{i};