diff --git a/@datadriven/emptyStruct.m b/@datadriven/emptyStruct.m
index 6469b53..68bab10 100644
--- a/@datadriven/emptyStruct.m
+++ b/@datadriven/emptyStruct.m
@@ -1,20 +1,20 @@
 function [SYS, OBJ, CON, PAR] = emptyStruct()
 % datadriven.emptyStruct()
 %
 % Author: Philippe Schuchert
 %         Philippe.schuchert@epfl.ch
 %         EPFL
 %         June 2021
 %
 % Empty structures required for the datadriven solve function.
 
 ctrl = struct('num',[],'den',[],'Ts',[],'Fx',[],'Fy',[]);
 SYS = struct('model',[],'W',[],'controller',ctrl);
 
 
-PAR = struct('tol',1e-6,'maxIter',100,'radius',1,'robustNyquist',true,'solveForm','dual');
+PAR = struct('tol',1e-6,'maxIter',50,'radius',1,'robustNyquist',true,'solveForm','dual');
 
 OBJ  = struct('inf',struct('meanNorm',1,'W1',[],'W2',[],'W3',[],'W4',[]),...
               'two', struct('meanNorm',1,'W1',[],'W2',[],'W3',[],'W4',[]));
 CON = struct('W1',[],'W2',[],'W3',[],'W4',[]);
 end
\ No newline at end of file
diff --git a/@datadriven/private/solveddlpv.m b/@datadriven/private/solveddlpv.m
index 521f9a1..8075265 100644
--- a/@datadriven/private/solveddlpv.m
+++ b/@datadriven/private/solveddlpv.m
@@ -1,401 +1,401 @@
 function [controller,sol,diagnostic] = solveddlpv(system,objective,constraint,parameters,sol)
 % solveddlpv
 %
 % Author: Philippe Schuchert
 %         Philippe.schuchert@epfl.ch
 %         EPFL
 %         June 2021
 %
 % Solve the problem using MOSEK 9.2 + MOSEK Fusion
 controller = system.controller;
 import mosek.fusion.*;
 
 Ts = controller.Ts;
 W = system.W;
 
 M = Model('DATA-DRIVEN OPTIMIZATION');
 
 nCon = length(W); % number of constraint
 nMod = length(system.model); % number of different models
 
 
 if isempty(sol)
     % solution struct is not provided: 1st iter
     sol.satisfyConstraints  = 0;
     sol.nIter = 0;
     sol.H2 = 0;
     sol.Hinf = 0;
 else
     if (sol.H2 || sol.Hinf)
         % scale such that objective is approx 2 (better nuerically scaled)
         % Not done
     end
 end
 
 autoScaling =  min(1e3,max(1e-3,max(abs(controller.num),[],'all')))/1;
 if  sol.satisfyConstraints
     % If last solution almost satisfies the constraint, fix slack and
     % optimize over the objective
     slack = Expr.constTerm(zeros(2,1));
     sol.slack = 0;
     sol.satisfyconstraint = 1;
     
 else
     sol.satisfyConstraints = 0;
     slack = M.variable('slack',2,Domain.greaterThan(0));
 end
 
 %%
 
 
 Xlpv = controller.num/autoScaling;
 Ylpv = controller.den; % rescale numerator
 
 
 
 [szx,ntheta] = size(Xlpv); % number of parameters num
 [szy,~] = size(Ylpv); % number of parameters num
 
 % Controller coefficient variables
 
 X = M.variable('X',[szx,ntheta]);
 Y = M.variable('Y',[szy,ntheta]);
 
 for ii = 1 : (ntheta)-1
     % 1st coefficient of Y has do dependency on scheduling paramters
     M.constraint(Y.index([0,ii]), Domain.equalsTo(0)); % leading coefficient does not depend on scheduling parameter(s)
 end
 M.constraint(Y.index([0,0]), Domain.equalsTo(1));
 
 z = resp(tf('z',Ts),W);
 Zy = z.^((szy-1):-1:0); % [0,z,...,z^nx]
 Zx = z.^((szx-1):-1:0); % [0,z,...,z^ny]
 ZFy = Zy.*resp(tf(controller.Fy,1,Ts),W);
 ZFx = Zx.*resp(tf(controller.Fx,1,Ts),W);
 
 
 %%
 
 
 %{
 OBJECTIVE : 1st step find controller that satisfies the constraint,
 then optimize over the objective using constraint. Can be changed using
 the softconstraint field, but it is generally not advised (root cause:
 constraint impossible to achieve, "bad" system, etc..).
 %}
 
 % H_infinity objectives
 
 
 gamma_inf = M.variable('gamma_inf',[1,1],Domain.greaterThan(0));
 gamma_inf_mmod = M.variable('gamma_inf_mmod',[1,nMod],Domain.greaterThan(0));
 
 if objective.inf.meanNorm
     M.constraint(Expr.sub(gamma_inf,Expr.mul(1/nMod,Expr.sum(gamma_inf_mmod))),Domain.greaterThan(0));
 else
     M.constraint(Expr.sub(Expr.mul(gamma_inf,Matrix.dense(ones(1,nMod))),gamma_inf_mmod),Domain.greaterThan(0));
 end
 
 
 gamma_Inf = Expr.mul(Matrix.dense(ones(nCon,1)),gamma_inf_mmod);
 
 
 % H_2 objectives
 integ =  Ts/(2*pi)*([diff(W(:));0] + [W(1);diff(W(:))]);
 gamma_2_mmod = M.variable('gamma2',[nCon,nMod],Domain.greaterThan(0));
 meanNorm = Expr.constTerm(zeros(1,1));
 
 % do not "new" frequencies from the adaptive grid in the objective.
 % Objective may be non-decreasing because of the additionals frequency
 % points.
 obj_2 = M.variable('o_2',1,Domain.greaterThan(0)); % used for obj_2.level()
 
 if not(sol.satisfyConstraints) % some constraint not satified
     OBJ = Expr.sum(slack) ;
 else
     % all constraint are satified. Optimize over the H2/Hinf objectives
     OBJ = Expr.add(Expr.sum(gamma_inf), obj_2);
 end
 
 
 obj = M.variable('objective',1,Domain.greaterThan(0));
 M.constraint(Expr.sub(obj,OBJ),Domain.greaterThan(0)); % for obj.level();
 
 M.objective('obj', ObjectiveSense.Minimize, OBJ);
 
 for mod_ =  1: nMod
     % Get Local controller
     Q = controller.theta(system.model(:,:,mod_));
     X_c = Xlpv*Q;
     Y_c = Ylpv*Q;
     X_n = Expr.mul(X,Matrix.dense(Q));
     Y_n = Expr.mul(Y,Matrix.dense(Q));
     XY_n = Expr.vstack(X_n,Y_n);
     
     Yc = ZFy*Y_c; Xc = ZFx*X_c; % Frequency response Kinit with the fixed parts
     
     %% Important (new) values
     
     PLANT = resp(system.model(:,:,mod_),W)*autoScaling; % Model Frequency response
     
     Pc =   PLANT.*Xc + Yc; % previous "" Open-loop "" (without numerator Yc)
     Cp =  [PLANT.*ZFx, ZFy]; % P = Cp*[X;Y], Cp complex, [X;Y] real, new "" Open-loop "" frequency response
     
     %% STABILITY STUFF, MUY IMPORTANTE!
     
     % Nyquist stability at |z| = infinity
     if abs(sum(controller.Fy)) < 1e-4
         if X_c(1)
             M.constraint(Expr.mul(sign(X_c(1)),X_n.index([0,0])),Domain.greaterThan(0));
         end
         M.constraint(Expr.mul(Matrix.dense(sign(sum(X_c))),Expr.sum(X_n)),Domain.greaterThan(0));
     end
     
     % Stability constraint
     if  parameters.robustNyquist
         % close the polygonal chain
         PcExtended = [conj(Pc(1));Pc;conj(Pc(end))];
         CpExtended = [conj(Cp(1,:));Cp;conj(Cp(end,:))];
         
         dP = getNormalDirection(PcExtended);
         
         x1_a = real(CpExtended(2:end,:).*conj(dP));
         x1_b = real(CpExtended(1:end-1,:).*conj(dP));
         
         M.constraint(Expr.mul(Matrix.dense(x1_a),XY_n),Domain.greaterThan(1e-6));
         M.constraint(Expr.mul(Matrix.dense(x1_b),XY_n),Domain.greaterThan(1e-6));
     end
     
     % Pole controller location
     if  (parameters.radius) && (numel(Q)>1 || mod_==1)
         % (numel(Q)>1 || mod==1) : for non LPV controller, constraint poles
         % only once
         %close the polygonal chain
         
         zExtended = [conj(z(1));z;conj(z(end))];
         ZyExtended = (parameters.radius*zExtended).^((szy-1):-1:0);
         YcsExtended= ZyExtended*(Y_c);
         
         dY = getNormalDirection(YcsExtended);
         
         x1_a = 2*real(conj(dY).*ZyExtended(2:end,:));
         x1_b = 2*real(conj(dY).*ZyExtended(1:end-1,:));
         
         M.constraint(Expr.mul(Matrix.dense(x1_a),Y_n),Domain.greaterThan(1e-6));
         M.constraint(Expr.mul(Matrix.dense(x1_b),Y_n),Domain.greaterThan(1e-6));
         
         %{
           % Stability of the controller pole can also be implemented this
           % way
           %
           % [-rX, AX; XA', -rX] <0 --> |eig(A)| < t
           % Find X by applying usinh the Schur complement lemma
           %     -rX +1/r*A*X*A' < 0
           %     A*X*A'/r^2 - X = -I
           %     X = dlyap(A/r,I)
           %
           % but solving SDP is hard(er)
         
         r = (parameters.radius);
         X_ = dlyap(compan(Y_c)/r,eye(length(Y_c)-1));
         RY = Y_n.slice([1,0],[szy,1]);
         companA = Expr.vstack(Expr.transpose(RY), Expr.constTerm(-eye(szy-2,szy-1)));
          
      
         P = Expr.constTerm(r*X_);
           
         Ap = Expr.mul(companA, X_);
           
           
          LYAP = Expr.vstack( Expr.hstack(P,Ap), ...
                              Expr.hstack(Expr.transpose(Ap),P));
         M.constraint(LYAP,Domain.inPSDCone()); %
         %}
     end
     
     %% OBJECTIVES
     %  only do if controller satisifies constraint
     x1 = Expr.add( Expr.mul(2*real(Cp.*conj(Pc)),XY_n), -conj(Pc).*Pc );
     
     if  (sol.satisfyConstraints)
         % H_inf objectives
         % Reminder : rotated cones 2*x1*x2 ≥ ||x3||^2, ||.||  Euclidean norm
         
         anyInfObjective = (~isempty(objective.inf.W1) || (~isempty(objective.inf.W4))) || ...
             (~isempty(objective.inf.W2) || (~isempty(objective.inf.W3)));
         if anyInfObjective
             x3 = [];
             
             if (~isempty(objective.inf.W1) || (~isempty(objective.inf.W4)))
                 W1 = respOrZero(objective.inf.W1,W);
                 W4 = respOrZero(objective.inf.W4,W,PLANT);
                 
                 infW14 = sqrt(abs(W1).^2+abs(W4).^2);
                 x3_d = infW14.*ZFy;
                 
                 x3 =  Expr.hstack( Expr.mul(real(x3_d),Y_n),Expr.mul(imag(x3_d),Y_n));
             else
                 infW14 = 0;
             end
             
             if (~isempty(objective.inf.W2) || (~isempty(objective.inf.W3)))
                 W2 = respOrZero(objective.inf.W2,W,PLANT);
                 W3 = respOrZero(objective.inf.W3,W,autoScaling);
                 
                 infW23 = sqrt(abs(W2).^2+abs(W3).^2);
                 x3_d = infW23.*ZFx;
                 
                 
                 if isempty(x3)
                     x3 =  Expr.hstack(Expr.mul(real(x3_d),X_n),Expr.mul(imag(x3_d),X_n));
                 else
                     x3 =  Expr.hstack(x3, Expr.mul(real(x3_d),X_n),Expr.mul(imag(x3_d),X_n));
                 end
             else
                 infW23 = 0;
             end
             if ~isempty(x3)
                 % 2*x1*x2 ≥ ||x3||^2, ||.||  Euclidean norm
                 scaling = 1./(sqrt(abs(infW14.*Yc).^2 + abs(infW23.*Xc).^2)./(abs(Pc)));
                 sz = x3.getShape();
                 
                 x2_scaled = Expr.mulElm(Matrix.dense(scaling*0.5),gamma_Inf.slice([0,mod_-1],[nCon,mod_]));
                 x3_scaled = Expr.mulElm(Matrix.dense(kron(sqrt(scaling),ones(1,sz(2)))),x3);
                 M.constraint(Expr.hstack(x1,x2_scaled,x3_scaled), Domain.inRotatedQCone());
             end % END Hinf
         end
         % H2
         anyTwoObjective = (~isempty(objective.two.W1) || (~isempty(objective.two.W4))) || ...
             (~isempty(objective.two.W2) || (~isempty(objective.two.W3)));
         if anyTwoObjective
             % local 2 norm
             gamma_2  = gamma_2_mmod.slice([0,mod_-1],[nCon,mod_]);
             
             if objective.two.meanNorm
                 meanNorm = Expr.add(meanNorm, Expr.dot(integ/nMod,gamma_2));
             else
                 M.constraint(Expr.sub(obj_2,Expr.dot(integ,gamma_2)),Domain.greaterThan(0));
             end
             x3 = [];
             
             if (~isempty(objective.two.W1) || (~isempty(objective.two.W4)))
                 W1 = respOrZero(objective.two.W1,W);
                 W4 = respOrZero(objective.two.W4,W,PLANT);
                 
                 twoW14 = sqrt(abs(W1).^2+abs(W4).^2);
                 x3_d = twoW14.*ZFy;
                 x3 =  Expr.hstack( Expr.mul(real(x3_d),Y_n),Expr.mul(imag(x3_d),Y_n));
             else
                 twoW14 = 0;
             end
             
             if (~isempty(objective.two.W2) || (~isempty(objective.two.W3)))
                 W2 = respOrZero(objective.two.W2,W,PLANT);
                 W3 = respOrZero(objective.two.W3,W,autoScaling);
                 
-                twoW23 = sqrt(abs(W2).^2+abs(W3).^2)*sqrt(scaObj);
+                twoW23 = sqrt(abs(W2).^2+abs(W3).^2);
                 x3_d = twoW23.*ZFx;
                 
                 if isempty(x3)
                     x3 =  Expr.hstack( Expr.mul(real(x3_d),X_n),Expr.mul(imag(x3_d),X_n));
                 else
                     x3 =  Expr.hstack( x3, Expr.mul(real(x3_d),X_n),Expr.mul(imag(x3_d),X_n));
                 end
             else
                 twoW23 = 0;
             end
             
             if ~isempty(x3)
                 scaling = .1./(sqrt(abs(twoW14.*Yc).^2 + abs(twoW23.*Xc).^2)./(abs(Pc)));
                 sz = x3.getShape();
                 
                 x2_scaled = Expr.mulElm(Matrix.dense(scaling*0.5),gamma_2);
                 x3_scaled = Expr.mulElm(Matrix.dense(kron(sqrt(scaling),ones(1,sz(2)))),x3);
                 M.constraint(Expr.hstack(x1,x2_scaled,x3_scaled), Domain.inRotatedQCone());
             end % END H2
             
         end
     end % END OBJ
     
     %% Constraint
     % CONSTAINTS ||W_n S_n||_inf< 1    W1, W2, W3, W4
     % ||W1 S||_inf< 1   -> max(|W1|,|system.model*W4|)*||Y/P||_inf < 1
     % ||W2 T||_inf< 1
     % ||W3 U||_inf< 1   -> max(|system.model*W2|,|W3|)*||X/P||_inf < 1
     % ||W3 D||_inf< 1
     
     lambda =  max(1e-4,min((sqrt(abs(1./Pc))),1e4));
     x1 = Expr.add( Expr.mul(2*real(Cp.*conj(Pc).*lambda),XY_n), -conj(Pc).*Pc.*lambda );
     if (~isempty(constraint.W1) || ~isempty(constraint.W4))
         x2 = Expr.add(0.5*ones(nCon,1), Expr.mul(Matrix.dense(0.5*ones(nCon,1)),slack.pick(0)));
         % batch W1 and W4 -> max(|W1|,|system.model*W4|)*||Y/P||_inf < 1
         W1 = respOrZero(constraint.W1,W);
         W4 = respOrZero(constraint.W4,W,PLANT*autoScaling);
         
         cW14 = max(abs(W1),abs(W4));
         
         x3_d = cW14.*ZFy.*sqrt(lambda) ;
         x3 =  Expr.hstack( Expr.mul(real(x3_d),Y_n),Expr.mul(imag(x3_d),Y_n));
         
         M.constraint((Expr.hstack(x1,x2,x3)), Domain.inRotatedQCone());
     end
     
     if (~isempty(constraint.W2) || ~isempty(constraint.W3))
         x2 = Expr.add(0.5*ones(nCon,1), Expr.mul(Matrix.dense(0.5*ones(nCon,1)),slack.pick(1)));
         % batch W2 and W3 -> max(|system.model*W2|,|W3|)*||X/P||_inf < 1
         W2 = respOrZero(constraint.W2,W,PLANT);
         W3 = respOrZero(constraint.W3,W,autoScaling);
         
         cW23 = max(abs(W2),abs(W3));
         
         x3_d = cW23.*ZFx.*sqrt(lambda) ;
         x3 =  Expr.hstack( Expr.mul(real(x3_d),X_n),Expr.mul(imag(x3_d),X_n));
         
         M.constraint((Expr.hstack(x1,x2,x3)), Domain.inRotatedQCone());
     end
     
     
 end
 if objective.two.meanNorm
     M.constraint(Expr.sub(obj_2,meanNorm),Domain.greaterThan(0));
 end
 
 if (sol.H2 || sol.Hinf)
     %  M.constraint(Expr.sub(sol.obj,obj),Domain.greaterThan(0));
 end
 
 M.setSolverParam("intpntSolveForm", parameters.solveForm);
 M.setSolverParam("intpntTolPsafe", 1e-4);
 M.setSolverParam("intpntTolDsafe", 1e-4);
-M.setSolverParam("intpntTolPath",  1e-2);
+M.setSolverParam("intpntTolPath",  1e-3);
 
 t1 = tic;
 M.solve();
 
 diagnostic.solvertime = toc(t1);
 M.acceptedSolutionStatus(AccSolutionStatus.Anything);
 
 
 sol.H2        = sqrt(obj_2.level());
 sol.Hinf      = sqrt(gamma_inf.level());
 sol.obj       = max(obj.level(),0);
 
 diagnostic.primal    = char(M.getPrimalSolutionStatus);
 diagnostic.dual      = char(M.getDualSolutionStatus);
 diagnostic.primalVal = M.primalObjValue();
 diagnostic.dualVal   = M.dualObjValue();
 
 
 if ~sol.satisfyConstraints
     sol.slack     = max(abs(slack.level()));
 end
 
 if ~(sol.slack <= 1e-4 && ~sol.satisfyConstraints)
     controller.num = reshape(X.level(),[ntheta, szx])'*autoScaling;
     controller.den = reshape(Y.level(),[ntheta, szy])';
 end
 
 
 sol.nIter = sol.nIter +1;
 M.dispose();
 end % END DATA_DRIVEN_SOLVE
\ No newline at end of file
diff --git a/@datadriven/private/solveddlpvFF.m b/@datadriven/private/solveddlpvFF.m
index 2a1fe21..6143ea3 100644
--- a/@datadriven/private/solveddlpvFF.m
+++ b/@datadriven/private/solveddlpvFF.m
@@ -1,260 +1,260 @@
 function [controller,sol,diagnostic] = solveddlpvFF(system,objective,constraint,parameters,sol)
 % solveddlpvff
 %
 % Author: Philippe Schuchert
 %         Philippe.schuchert@epfl.ch
 %         EPFL
 %         June 2021
 %
 % Solve the problem using MOSEK 9.2 + MOSEK Fusion
 controller = system.controller_ff;
 import mosek.fusion.*;
 
 Ts = controller.Ts;
 W = system.W;
 
 M = Model('DATA-DRIVEN OPTIMIZATION');
 
 nCon = length(W); % number of constraint
 nMod = length(system.model); % number of different models
 
 
 if isempty(sol)
     % solution struct is not provided: 1st iter
     sol.satisfyConstraints  = 0;
     sol.nIter = 0;
 end
 
 autoScaling =  min(1e3,max(1e-1,max(abs(controller.num),[],'all')));
 if autoScaling==0
     autoScaling = 1;
 end
 
 if  sol.satisfyConstraints
     % If last solution almost satisfies the constraint, fix them and
     % optimize over the objective
     % M.constraint(slack,Domain.equalsTo(0));
     slack = Expr.constTerm(zeros(5,1)); %Matrix.dense(zeros(12,1));
     sol.slack = 0;
     sol.satisfyconstraint = 1;
     
 else
     sol.satisfyConstraints = 0;
     slack = M.variable('slack',5,Domain.greaterThan(0));
 end
 
 %%
 
 
 Xlpv = controller.num/autoScaling;
 Ylpv = controller.den; % rescale numerator
 
 [szx,ntheta] = size(Xlpv); % number of parameters num
 [szy,~] = size(Ylpv); % number of parameters num
 
 % Controller coefficient variables
 
 X = M.variable('X',[szx,ntheta]);
 Y = M.variable('Y',[szy,ntheta]);
 
 for ii = 1 : (ntheta)-1
     % 1st coefficient of Y has do dependency on scheduling paramters
     M.constraint(Y.index([0,ii]), Domain.equalsTo(0)); % leading coefficient does not depend on scheduling parameter(s)
 end
 M.constraint(Y.index([0,0]), Domain.equalsTo(1));
 
 
 
 z = resp(tf('z',Ts),W);
 Zy = z.^((szy-1):-1:0); % [0,z,...,z^nx]
 Zx = z.^((szx-1):-1:0); % [0,z,...,z^ny]
 ZFy = Zy.*resp(tf(controller.Fy,1,Ts),W);
 ZFx = Zx.*resp(tf(controller.Fx,1,Ts),W);
 
 
 %%
 
 %{
 OBJECTIVE : 1st step find controller that satisfies the constraint,
 then optimize over the objective using constraint. Can be changed using
 the softconstraint field, but it is generally not advised (root cause:
 constraint impossible to achieve, "bad" system, etc..).
 %}
 
 % H_infinity objectives
 gamma_inf = M.variable('gamma_inf',[1,1],Domain.greaterThan(0));
 gamma_Inf = Expr.mul(Matrix.dense(ones(nCon,1)),gamma_inf);
 
 
 % H_2 objectives
 integ =  Ts/(2*pi)* ([diff(W(:));0] + [W(1);diff(W(:))]);
 gamma_2_mmod = M.variable('gamma2',[nCon,nMod],Domain.greaterThan(0));
 
 
 % do not "new" frequencies from the adaptive grid in the objective.
 % Objective may be non-decreasing because of the additionals frequency
 % points.
 obj_2 = M.variable('o_2',1,Domain.greaterThan(0)); % used for obj_2.level()
 
 if not(sol.satisfyConstraints) % some constraint not satified
     OBJ = Expr.sum(slack) ;
 else
     OBJ = Expr.add(Expr.sum(Expr.mul(Matrix.dense(1/1),gamma_inf)), obj_2);
 end
 
 
 obj = M.variable('objective',1,Domain.greaterThan(0));
 M.constraint(Expr.sub(obj,OBJ),Domain.greaterThan(0)); % for obj.level();
 
 M.objective('obj', ObjectiveSense.Minimize, OBJ);
 
 
 KFB_LPV = datadriven.getController(system)/autoScaling;
 for mod =  1: nMod
     
     %%Important (new) values
     if length(KFB_LPV) == nMod
         KLPV_loc = resp(KFB_LPV(:,:,mod),W);
     else
          KLPV_loc = resp(KFB_LPV(:,:,1),W);
     end
     
     Q = controller.theta(system.model(:,:,mod));
     X_c = Xlpv*Q;
     Y_c = Ylpv*Q;
     X_n = Expr.mul(X,Matrix.dense(Q));
     Y_n = Expr.mul(Y,Matrix.dense(Q));
     XY_n = Expr.vstack(X_n,Y_n);
     
     Yc = ZFy*Y_c; % Xc = Xcs.*Fx; % Frequency response Kinit with the fixed parts
     Xc = ZFx*X_c; % Xc = Xcs.*Fx; % Frequency response Kinit with the fixed parts
     
     PLANT = resp(system.model(:,:,mod),W)*autoScaling; % Model Frequency response
     Denominator = abs(1+PLANT.*KLPV_loc);
     
     
     
     %% Stability constraint
     
     % Pole controller location
     if ~isempty(parameters.radius)
         % close the polygonal chain
         zstart = conj(z(1));
         zfinish = conj(z(end));
         
         zExtended = [zstart;z;zfinish];
         
         ZyExtended = (parameters.radius*zExtended).^((szy-1):-1:0);
         YcsExtended= ZyExtended*(Y_c);
         dY = getNormalDirection(YcsExtended);
         
         x1_a = 2*real(conj(dY).*ZyExtended(2:end,:));
         x1_b = 2*real(conj(dY).*ZyExtended(1:end-1,:));
         
         M.constraint(Expr.mul(Matrix.dense(x1_a),Y_n),Domain.greaterThan(1e-5));
         M.constraint(Expr.mul(Matrix.dense(x1_b),Y_n),Domain.greaterThan(1e-5));
     end
     
     %% OBJECTIVE
     if  (sol.satisfyConstraints)
         x1 = 2.*real(ZFy.*conj(Yc));
          z1 = Expr.sub( Expr.mul(Matrix.dense(x1),Y_n), real(conj(Yc).*Yc));
         % H_inf objectives
         % Reminder : rotated cones 2*x1*x2 ≥ ||x3||^2, ||.||  Euclidean norm
         if (~isempty(objective.inf.W1))
             
             scaling = 1./abs((-PLANT.*Xc+Yc)./Denominator.*resp(objective.inf.W1,W)./Yc);
             
             x2 =   Expr.mulElm(Matrix.dense(0.5*scaling),gamma_Inf) ;
             x3_d = [-PLANT.*ZFx,ZFy]./Denominator.*resp(objective.inf.W1,W).*sqrt(scaling);
             x3 =  Expr.stack(1, Expr.mul(Matrix.dense(real(x3_d)),XY_n),Expr.mul(Matrix.dense(imag(x3_d)),XY_n));
             
 
             M.constraint(Expr.hstack(z1,x2,x3), Domain.inRotatedQCone());  
         end % END Hinf
         
         % H2
         if ~isempty(objective.two.W1)
             gamma_2   = gamma_2_mmod.slice([0,mod-1],[nCon,mod]);
             M.constraint(Expr.sub(obj_2,Expr.dot(integ,gamma_2)),Domain.greaterThan(0));
             
             scaling = 1./abs((-PLANT.*Xc+Yc)./Denominator.*resp(objective.two.W1,W)./Yc);
             
             x2 =   Expr.mulElm(Matrix.dense(0.5*scaling),gamma_2) ;
             x3_d = [-PLANT.*ZFx,ZFy]./Denominator.*resp(objective.two.W1,W).*sqrt(scaling);
             x3 =  Expr.stack(1, Expr.mul(Matrix.dense(real(x3_d)),XY_n),Expr.mul(Matrix.dense(imag(x3_d)),XY_n));
             
 
             M.constraint(Expr.hstack(z1,x2,x3), Domain.inRotatedQCone());
         end
         
     end
     
     %% CONSTRAINTS
     x1 = Expr.sub( Expr.mul(Matrix.dense( 2.*real(ZFy.*conj(Yc))),Y_n), real(conj(Yc).*Yc));
  
     if (~isempty(constraint.W1) || ~isempty(constraint.W4))
         x2 = Expr.add(0.5*ones(nCon,1), Expr.mul(Matrix.dense(0.5*ones(nCon,1)),slack.pick(0)));
         % batch W1 and W4 -> max(|W1|,|system.model*W4|)*||Y/P||_inf < 1
         W1 = respOrZero(constraint.W1,W);
         W4 = respOrZero(constraint.W4,W,PLANT*autoScaling);
 
         W14 = max(abs(W1),abs(W4));
         
         x3_d = [-PLANT.*ZFx,ZFy].*W14./Denominator ;
         x3 =  Expr.stack(1, Expr.mul(Matrix.dense(real(x3_d)),XY_n),Expr.mul(Matrix.dense(imag(x3_d)),XY_n));
         
         M.constraint((Expr.hstack(x1,x2,x3)), Domain.inRotatedQCone());
     end
     
     if (~isempty(constraint.W2) || ~isempty(constraint.W3))
         x2 = Expr.add(0.5*ones(nCon,1), Expr.mul(Matrix.dense(0.5*ones(nCon,1)),slack.pick(1)));
         % batch W2 and W3 -> max(|system.model*W2|,|W3|)*||X/P||_inf < 1
         W2 = respOrZero(constraint.W2,W,PLANT);
         W3 = respOrZero(constraint.W3,W,autoScaling);
         
         W23 = max(abs(W2),abs(W3));
         
         x3_d = [ZFx,KLPV_loc.*ZFy].*W23./Denominator ;
         x3 =  Expr.stack(1, Expr.mul(Matrix.dense(real(x3_d)),XY_n),Expr.mul(Matrix.dense(imag(x3_d)),XY_n));
         
         M.constraint((Expr.hstack(x1,x2,x3)), Domain.inRotatedQCone());
     end
     
 end
 
 M.setSolverParam("intpntSolveForm", parameters.solveForm)
 M.setSolverParam("intpntTolPsafe", 1e-4)
 M.setSolverParam("intpntTolDsafe", 1e-4)
-M.setSolverParam("intpntTolPath",  1e-2)
+M.setSolverParam("intpntTolPath",  1e-4)
 
 t1 = tic;
 
 M.solve();
 
 diagnostic.solvertime = toc(t1);
 M.acceptedSolutionStatus(AccSolutionStatus.Anything)
 
 
 sol.H2        = sqrt(obj_2.level());
 sol.Hinf      = sqrt(gamma_inf.level());
 sol.obj       = max(obj.level(),0);
 
 diagnostic.primal    = char(M.getPrimalSolutionStatus);
 diagnostic.dual      = char(M.getDualSolutionStatus);
 diagnostic.primalVal = M.primalObjValue();
 diagnostic.dualVal   = M.dualObjValue();
 
 
 if ~sol.satisfyConstraints
     sol.slack     = max(abs(slack.level()));
 end
 
 if ~(sol.slack <= 1e-4 && ~sol.satisfyConstraints)
     controller.num = reshape(X.level(),[ntheta, szx])'*autoScaling;
     controller.den = reshape(Y.level(),[ntheta, szy])';
 end
 
 sol.nIter = sol.nIter +1;
 M.dispose();
 end % END DATA_DRIVEN_SOLVE
\ No newline at end of file
diff --git a/Examples/SimultaneousUnstable.m b/Examples/SimultaneousUnstable.m
index 35a2cfc..b229783 100644
--- a/Examples/SimultaneousUnstable.m
+++ b/Examples/SimultaneousUnstable.m
@@ -1,63 +1,63 @@
    % Example derived from
 % https://ch.mathworks.com/help/robust/ug/simultaneous-stabilization-using-robust-control.html
 %
 % Design a digital controller for a continious (and unstable) system.
 
 %addpath ../toolbox/
 %%
 Pnom = tf(2,[1 -2]);                  % Nominal model
 
 p1 = Pnom*tf(1,[.06 1]);              % extra lag
 p2 = Pnom*tf([-.02 1],[.02 1]);       % time delay
 p3 = Pnom*tf(50^2,[1 2*.1*50 50^2]);  % high frequency resonance
 p4 = Pnom*tf(70^2,[1 2*.2*70 70^2]);  % high frequency resonance
 p5 = tf(2.4,[1 -2.2]);                % pole/gain migration
 p6 = tf(1.6,[1 -1.8]);                % pole/gain migration
 
 Parray = stack(1,p1,p2,p3,p4,p5,p6);
 %%
 Ts = 1e-3; % Sample fast enough such that continious model is a good approxiamtion of the model using zero order hold
 z = tf('z',Ts);
 
 Kinit = 2+0.01/(z-1); % Initial stabilizing controller
 
-w = logspace2(0.1,pi/Ts,300);
+w = logspace(-1,log10(pi/Ts),300);
 
 %% Set-up problem
 
 SYS = datadriven.system('G',Parray,'W',w,'Kinit',Kinit,'order', 20);
 OBJ = datadriven.objectives('two.W1',.1/(tf('z',Ts)-1));
 CON = datadriven.constraints('W1',1/2,'W2',1/2,'W3',db2mag(-30));
 PAR = datadriven.parameters('tol',1e-5,'maxIter',100,'radius',0.95);
 
 %% SOVLE
 
 % !! REQUIRES MOSEK + MOSEK FUSION !!
 % Make sure the license is avalible, and Fusion installed. 
 % type
 % >> mosekdiag
 % to check if mosek read to go
 
 [SYS.controller,obj] = datadriven.siso(SYS,OBJ,CON,PAR); % implementation for SISO controllers
 
 FB = datadriven.getController(SYS); 
 
 %%
 
 Gf = frd(Parray,w);
 Gf.Ts = Ts; 
 % !!! Gf (ie Parray) is NOT sampled at Ts. The different sampling time is
 % changed only for the plots (can't mix continious and discrete systems)
 
 figure
 S = feedback(1,Gf*FB); % compute sensitivity
 
 bodemag(S,1-S,FB*S, ...
         tf(1/CON.W1),'-k',tf(1/CON.W2),'-k',tf(1/CON.W3),'-k',...
         w)
 
 legend('sensitivity','complementary sensitivity','input sensitivity','1/constraints')
 
 %%
 nyquist(Gf*FB)
 shg
\ No newline at end of file
diff --git a/readme.md b/readme.md
index 19a6d1c..9b1f795 100644
--- a/readme.md
+++ b/readme.md
@@ -1,10 +1,10 @@
 A couple of personal scripts I have used during my PhD (many of questionable use/implementation)
 
 
 
 The datadriven class requires MOSEK 9  and MOSEK Fusion. Download MOSEK, and add the Fusion interface to MATLAB
 
 
 javaaddpath /__PATH_TO_MOSEK__/mosek/9.3/tools/platform/__PLATEFORM__/bin/mosek.jar
 
-© Philippe Schuchert
+© Philippe Schuchert 2021