Merge pull request #5 from teamparodis/develop

Merge a number of bug fixes and improvements

@Controller/Controller.m

@@ -165,6 +165,10 @@ function compile(obj, model, agent, yalmipOptions)
                 costFunction = costFunctionCell{1};
 
                 slacks = costFunction.getSlacks(model, agent, obj.paramSyms);
+                if ~isa(slacks, 'struct')
+                    warning("PARODIS Controller:compile cost function getSlacks must return struct");
+                    continue;
+                end
                 obj.slackVariables = mergeStructs(obj.slackVariables, slacks);
             end
 
@@ -292,4 +296,4 @@ function compile(obj, model, agent, yalmipOptions)
             end
         end
     end
-end
+end

@CostFunction/CostFunction.m

@@ -2,15 +2,21 @@
     % Abstract super class for implementation of cost functions
 
     methods (Abstract,Static)
-        % Function for introducing custom slacks and corresponding constraints
-        [slacks] = getSlacks(model, agent, params)
-        [constraints] = getConstraints(model, agent, slacks, params)
 
         % Function returning an expression for the cost function
         [expr] = buildExpression(x, u, d, params, Ns, slacks, T_s)
     end
 
     methods (Static)
+        % Function for introducing custom slacks and corresponding constraints
+        function [slacks] = getSlacks(model, agent, params)
+            slacks = struct;
+        end
+
+        function [constraints] = getConstraints(model, agent, slacks, params)
+            constraints = [];
+        end
+
         % Optional function for expression for a single scenario
         function [exprSingle] = buildExpressionSingleScen(x, u, d, params, slacks, T_s)
             exprSingle = [];

@ExplicitController/ExplicitController.m

@@ -381,7 +381,7 @@ function addConstraint(obj, arg)
                 end
 
                 % vector for scaling dx to appropriate time steps
-                scale = T_s/T_s_ref;
+                scale = T_s(1:N_horz-1)/T_s_ref; % dx/du consider N_horz-1 many steps, and N_horz is shorter for du
 
                 if iscell( variableSym )
                     N_S = length( variableSym );

@ParetoController/ParetoController.m

@@ -159,7 +159,6 @@ function addCostFunction(obj, name, costFunctionInstance, defaultWeight )
                         slackValues = slacks(idx,:);
                         chosenParameters = paretoParameters(idx,:);
                         chosenSolution = front(idx,:);
-                        agent.status.chosenWeights = paretoParameters(idx,:);
                     else
                         [uPred, slackValues, chosenParameters, chosenSolution] = this.chooseSolution( agent, inputs, slacks, front, paretoParameters );
                     end
@@ -185,10 +184,10 @@ function addCostFunction(obj, name, costFunctionInstance, defaultWeight )
             % string is 'auto' the optimizer corresponding to the front determination scheme is
             % called.
 
-            if nargin == 5 && isequal(this.config.preperationMethod,'auto')
+            if nargin == 5 && isequal(this.config.preparationMethod,'auto')
                 method = this.config.frontDeterminationScheme;
             elseif nargin == 5
-                method = this.config.preperationMethod;
+                method = this.config.preparationMethod;
             end
 
             if isa(method, 'function_handle')
@@ -198,7 +197,7 @@ function addCostFunction(obj, name, costFunctionInstance, defaultWeight )
                 if ismethod(this, optimizerMethod)
                     optimizer = ParetoController.(optimizerMethod)(this, optimizeConstraints, costExpressions, agent, extremePoints);
                 else
-                    error("No function handle given for front determination scheme! Try 'AWDS', 'NBI', 'FPBI'!")
+                    error("No function handle given for front determination scheme! Try 'AWDS', 'NBI', 'FPBI', 'ASBI'!")
                 end
             end
         end
@@ -245,7 +244,7 @@ function addCostFunction(obj, name, costFunctionInstance, defaultWeight )
                 if ismethod(this, FDS)
                     [inputs, slacks, front, parametersFDS] = ParetoController.(FDS)(this, agent, optimizer, extremePoints, chosenParameters);
                 else
-                    error("No function handle given for metric function! Try 'AWDS', 'NBI' or 'FPBI'.")
+                    error("No function handle given for metric function! Try 'AWDS', 'NBI', 'FPBI' or 'ASBI'.")
                 end
             end
             inputs = [inputsEP; inputs];
@@ -287,6 +286,7 @@ function clear(this)
         optimizer = prepareNBI(paretoObj, optimizeConstraints, costExpressions, agent, extremePoints)
         optimizer = prepareFPBI(paretoObj, optimizeConstraints, costExpressions, agent, extremePoints)
         optimizer = prepareWS(paretoObj, optimizeConstraints, costExpressions, agent, utopia, nadir, additionalSymbols, additionalCostExpression)
+        optimizer = prepareASBI(paretoObj, optimizeConstraints, costExpressions, agent, ~)
 
         % extreme point functions
         [extremePoints, inputsEP, slacksEP, parametersEP] = initializeMWA(paretoObj, optimizeConstraints, costExpressions, agent);
@@ -297,6 +297,7 @@ function clear(this)
         [inputs, slacks, front, parameters] = determineAWDS(this, agent, optimizer, extremePoints, chosenParameters);
         [inputs, slacks, front, parameters] = determineNBI(this, agent, optimizer, extremePoints, chosenParameters);
         [inputs, slacks, front, parameters] = determineFPBI(this, agent, optimizer, extremePoints, chosenParameters);
+        [inputs, slacks, front, parameters] = determineASBI(this, agent, optimizer, extremePoints, chosenParameters);
 
         % metric functions
         [idx,utility] = selectCUP(varargin);
@@ -311,7 +312,7 @@ function clear(this)
             config.distance2AllMin = [];
             config.interactivity = false;
             config.plotLabels = {};
-            config.preperationMethod = 'auto';
+            config.preparationMethod = 'auto';
             config.extremePointFunction = 'MWAN';
             config.metricFunction = 'CUP';
             config.frontDeterminationScheme = 'FPBI';

@ParetoController/determineASBI.m

@@ -0,0 +1,205 @@
+function [inputs, slacks, front, parameters] = determineASBI(paretoObj, agent, optimizer, extremePoints, preselectedParameters)
+% ASBI: Adaptive Search Boundary Intersection. Function for evaluating an evenly
+% distributed Pareto front. Recursive approach
+%   
+% Input:
+%   paretoObj: The ParetoController object calling this function.
+%   agent: Agent object that this timestep is evaluated for.
+%   optimizer: YALMIP optimizer object for this determination method.
+%   extremePoints: Extreme Points found by an initialization algorithm,
+%       e.g. 'lexicographic'. Points minimizing exactly one cost function.
+%   preselectedParameters: Given when the paretoParameters are already fixed. If given,
+%       this function will only call the optimizer with these parameters.
+%
+% Output:
+%   front: Pareto optimal points.
+%   inputs: All possible inputs u determined by evaluating different Pareto-optimal points.
+%   slacks: Slack variables for every Pareto optimal point.    
+%   parameters: Evaluated paretoParameters for logging purposes.
+
+if nargin == 4 || isempty(preselectedParameters)
+    paretoObj.paretoCurrentStep = 0;
+    front = extremePoints;
+    paretoObj.evaluatedPoints = extremePoints;
+    numEP = size(extremePoints,1);
+
+    % if the minimal distance to all other Pareto-optimal points is not set it
+    % is set to a default values of 1/5 of the minimal distance ratio
+    if isempty(paretoObj.config.distance2AllMin)
+        paretoObj.config.distance2AllMin = paretoObj.config.drMin/5;
+    end
+
+    % get the number of unique Pareto-optimal points
+    numPoints = size(unique(front,'rows'),1);
+
+    % if the number of points is less than the number of conflicting objectives the ASBI
+    % cannot be applied
+    if numPoints < numel(paretoObj.status.conflictingObj)
+        warning("Not enough points found for AWDS! :(")
+        inputs = [];
+        paretoParameters = [];
+        slacks = [];
+        return
+    end
+
+    % get all possible recombinations, if more than n EP candidates are given
+    recombinations = nchoosek( 1:(numPoints), numel(paretoObj.status.conflictingObj) );
+    inputs = [];
+    paretoParameters = [];
+    slacks = [];
+
+    % do ASBI for all combinations of candidates
+    for ii = 1:size(recombinations,1)
+        [frontTemp, inputsTemp, slacksTemp, evaluatedWeightsTemp] = evaluateParetoFront(paretoObj, agent, optimizer, front(recombinations(ii,:),:), Inf);
+        front = [front; frontTemp];
+        inputs = [inputs; inputsTemp];
+        paretoParameters = [paretoParameters; evaluatedWeightsTemp];
+        slacks = [slacks, slacksTemp];
+    end
+
+    % eliminate all weakly Pareto-optimal points
+    filteredFront = ParetoController.paretoFilter(paretoObj, front, 1:numEP);
+    front = front(filteredFront,:);
+
+    filteredPIS = filteredFront - numEP;
+    filteredPIS(filteredPIS <= 0) = [];
+    parameters = paretoParameters(filteredPIS,:);
+    inputs = inputs(filteredPIS);
+    slacks = slacks(filteredPIS);
+
+elseif nargin == 5
+    optOut = optimizer(preselectedParameters);
+
+    [front, inputs, slacks] = paretoObj.calculateUnnormedObjectiveValues(optOut, agent);
+    parameters = preselectedParameters;
+else
+    error("Input error in determineAWDS, not enough inputs.")
+end
+
+paretoObj.paretoCurrentStep = [];
+
+end
+
+%%
+function [points, inputs, slacks, paretoParameters] = evaluateParetoFront(paretoObj, agent, optimizer, parents, drPrev)
+% Recursive function for evaluating the Pareto front. Takes n parents defining a
+% hyperplane. The starting point is the midpoint of all n parents, the search direction is
+% the normal vector of the hyperplane.
+
+nConflict = numel(paretoObj.status.conflictingObj);
+paretoObj.paretoCurrentStep = paretoObj.paretoCurrentStep + 1;
+agent.simulation.updateProgress();
+
+newParameters = computeParetoParameters(parents, paretoObj);
+
+% optimize with new paretoParameters for finding the optimal u
+[optOut, feasibilityCode] = optimizer(newParameters);
+
+% skip the solution if the problem was infeasible
+if feasibilityCode ~= 0
+    warning("Yalmip error: " + yalmiperror(feasibilityCode))
+    points = [];
+    paretoParameters = [];
+    inputs = [];
+    slacks = [];
+    return
+end
+
+[J_opt, u, sl] = calculateUnnormedObjectiveValues(paretoObj, optOut, agent);
+
+% calculate the minimal distance to all other Pareto-optimal points
+distance2All = minDistance2all(J_opt, paretoObj);
+
+if distance2All <= paretoObj.config.distance2AllMin || isinf(distance2All)
+    % Check if there is a solution similar to the new one, since this would only result in
+    % more similar solutions.
+    points = [];
+    paretoParameters = [];
+    inputs = [];
+    slacks = [];
+    return
+end
+paretoObj.evaluatedPoints(end+1,:) = J_opt;
+
+% update the nadir point if the new point is bigger in one objective
+if any(paretoObj.status.nadir < J_opt)
+    paretoObj.status.nadir = max([paretoObj.status.nadir; J_opt]);
+end
+
+inputs = {u};
+slacks = [sl];
+points = J_opt;
+paretoParameters = newParameters;
+
+% get the candidates for new parents
+candidates = unique(parents,'rows','stable');
+
+if size(candidates,1) < nConflict-1
+    points = [];
+    paretoParameters = [];
+    inputs = [];
+    slacks = [];
+    return
+end
+
+recombinations = nchoosek( 1:(size(candidates,1)), nConflict - 1);
+
+for jj = 1:size(recombinations,1)
+    newParents = [candidates(recombinations(jj,:),:); J_opt];
+    prevPoints = parents;
+
+    if isempty(prevPoints)
+        continue
+    end
+
+    dr = distance_ratio(prevPoints, J_opt, paretoObj);
+
+    if dr <= paretoObj.config.drMin
+        % Distance ratio is used to predict whether new solutions will be found
+        continue
+    end
+
+    [newJ_opt, u, newSlacks, JOptParameters] = evaluateParetoFront(paretoObj, agent, optimizer, newParents, dr);
+
+    inputs = [inputs; u];
+    slacks = [slacks; newSlacks];
+    points = [points; newJ_opt];
+    paretoParameters = [paretoParameters; JOptParameters];
+end
+end
+
+%%
+function [paretoParameters] = computeParetoParameters(hyperpoints, paretoObj)
+
+normedHyperpoints = ParetoController.ParetoNormalization(hyperpoints, paretoObj);
+midpoint = mean(normedHyperpoints);
+searchDirection = null(normedHyperpoints(2:end,:)-normedHyperpoints(1,:))';
+
+paretoParameters = [midpoint, searchDirection];
+end
+
+%%
+function dr = distance_ratio(previousPoints, newPoint, paretoObj)
+% calculates the distance ratio between previously found solutions and the
+% new solution. in the case of n = 2 dimensions, distance ratio is equal to
+% the euclidian distance in normalized (decision) space.
+numParents = numel(paretoObj.status.conflictingObj);
+r = paretoObj.status.nadir - paretoObj.status.utopia;
+dr = 0;
+
+for p=1:numParents-1
+    dr = dr + norm( (newPoint - previousPoints(p, :))./ r );
+end
+
+dr = dr/(numParents-1);
+
+end
+
+%%
+function distance2All = minDistance2all(newPoint, paretoObj)
+% Calculates the minimal distance to any previously found solution.
+% Prevents accumulation of similar solutions.
+checkPoints = ParetoController.ParetoNormalization(paretoObj.evaluatedPoints, paretoObj);
+distance2All = min(vecnorm(checkPoints - ParetoController.ParetoNormalization(newPoint, paretoObj),2,2));
+
+end

@ParetoController/determineAWDS.m

@@ -139,7 +139,7 @@
 end
 
 inputs = {u};
-slacks = {sl};
+slacks = [sl];
 points = J_opt;
 weights = newWeight;
 

@ParetoController/determineFPBI.m

@@ -15,24 +15,27 @@
 
 paretoObj.paretoMaxStep = size(planePoints,1)-1;
 
-for i = 2:size(planePoints,1)
-    paretoObj.paretoCurrentStep = i-1;
+front = [];
+inputs = {};
+
+for iParetoStep = 2:size(planePoints,1)
+    paretoObj.paretoCurrentStep = iParetoStep - 1;
     agent.simulation.updateProgress();
 
-    [optOut, feasibilityCode] = optimizer(planePoints(i,:));
+    [optOut, feasibilityCode] = optimizer(planePoints(iParetoStep,:));
 
     if feasibilityCode ~= 0
         continue
     end
 
-    pos = i-1;
-    [front(pos,:), inputs{pos,1}, slacks{pos,1}] = calculateUnnormedObjectiveValues(paretoObj, optOut, agent);
+    [front(iParetoStep,:), inputs{iParetoStep,1}, slacks(iParetoStep,1)] = calculateUnnormedObjectiveValues(...
+        paretoObj, optOut, agent);
 
-    if isequal(front(pos,:),[0 0 0])
+    if isequal(front(end,:),[0 0 0])
         continue
     end
 
-    startingPoints = [startingPoints; planePoints(i,:)];
+    startingPoints = [startingPoints; planePoints(iParetoStep,:)];
 end
 
 paretoObj.status.nadir = max(front);

@ParetoController/determineNBI.m

@@ -42,7 +42,7 @@
     end
 
     pos = i-n;
-    [front(pos,:), inputs{pos,1}, slacks{pos,1}] = calculateUnnormedObjectiveValues(paretoObj, optOut, agent);
+    [front(pos,:), inputs{pos,1}, slacks(pos,1)] = calculateUnnormedObjectiveValues(paretoObj, optOut, agent);
 
     if isequal(front(pos,:),[0 0 0])
         continue